FrankE
a2c9e575a2
- 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
72 lines
1.5 KiB
C++
72 lines
1.5 KiB
C++
#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
|