huge commit
- worked on about everything - grid walker using plugable modules - wifi models - new distributions - worked on geometric data-structures - added typesafe timestamps - worked on grid-building - added sensor-classes - added sensor analysis (step-detection, turn-detection) - offline data reader - many test-cases
This commit is contained in:
71
tests/sensors/imu/TestTurnDetection.cpp
Normal file
71
tests/sensors/imu/TestTurnDetection.cpp
Normal file
@@ -0,0 +1,71 @@
|
||||
#ifdef WITH_TESTS
|
||||
|
||||
#include "../../Tests.h"
|
||||
|
||||
#include "../../../sensors/imu/TurnDetection.h"
|
||||
|
||||
TEST(TurnDetection, rotationMatrix) {
|
||||
|
||||
Eigen::Vector3f dst; dst << 0, 0, 1;
|
||||
Eigen::Vector3f src; src << 1, 1, 0; src.normalize();
|
||||
|
||||
// get a matrix that rotates "src" into "dst"
|
||||
Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst);
|
||||
|
||||
Eigen::Vector3f res = rot * src;
|
||||
|
||||
ASSERT_NEAR(dst(0), res(0), 0.01);
|
||||
ASSERT_NEAR(dst(1), res(1), 0.01);
|
||||
ASSERT_NEAR(dst(2), res(2), 0.01);
|
||||
|
||||
}
|
||||
|
||||
TEST(TurnDetection, gyroRotate) {
|
||||
|
||||
|
||||
Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
||||
Eigen::Vector3f acc; acc << 0, 7.0, 7.0;
|
||||
|
||||
Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(acc, zAxis);
|
||||
|
||||
Eigen::Vector3f gyro; gyro << 0, 60, 60;
|
||||
|
||||
Eigen::Vector3f gyro2; gyro2 << 0, 0, 84;
|
||||
|
||||
Eigen::Vector3f gyro3 = rot * gyro;
|
||||
|
||||
ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
TEST(TurnDetection, xx) {
|
||||
|
||||
Eigen::Vector3f dst; dst << 0, 0, 1;
|
||||
Eigen::Vector3f src; src << 0.0, 2.9, -10.0; src.normalize(); // sample accelerometer readings
|
||||
|
||||
Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst);
|
||||
|
||||
// Eigen::Vector3f x; x << 1, 0, 0;
|
||||
// Eigen::Vector3f z = src.normalized();
|
||||
// Eigen::Vector3f y = z.cross(x);
|
||||
|
||||
// Eigen::Matrix3f rot;
|
||||
// rot.row(0) = x;
|
||||
// rot.row(1) = y;
|
||||
// rot.row(2) = z;
|
||||
|
||||
Eigen::Vector3f res = rot * src;
|
||||
// ASSERT_NEAR(dst(0), res(0), 0.01);
|
||||
// ASSERT_NEAR(dst(1), res(1), 0.01);
|
||||
// ASSERT_NEAR(dst(2), res(2), 0.01);
|
||||
|
||||
Eigen::Vector3f gyro; gyro << 0, 10, 30;
|
||||
|
||||
Eigen::Vector3f gyro2 = rot * gyro;
|
||||
int i = 0; (void) i;
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user