73 lines
1.3 KiB
C++
73 lines
1.3 KiB
C++
#ifdef WITH_TESTS
|
|
|
|
#ifdef TODO
|
|
#include "../../Tests.h"
|
|
|
|
#include "../../../sensors/imu/TurnDetection.h"
|
|
|
|
TEST(TurnDetection, rotationMatrix) {
|
|
|
|
Vector3 dst(0, 0, 1);
|
|
Vector3 src(1, 1, 0); src = src.normalized();
|
|
|
|
// get a matrix that rotates "src" into "dst"
|
|
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
|
|
|
|
Vector3 res = rot * src;
|
|
|
|
ASSERT_NEAR(dst.x, res.x, 0.01);
|
|
ASSERT_NEAR(dst.y, res.y, 0.01);
|
|
ASSERT_NEAR(dst.z, res.z, 0.01);
|
|
|
|
}
|
|
|
|
TEST(TurnDetection, gyroRotate) {
|
|
|
|
|
|
Vector3 zAxis(0, 0, 1);
|
|
Vector3 acc(0, 7.0, 7.0);
|
|
|
|
Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
|
|
|
|
Vector3 gyro(0, 60, 60);
|
|
|
|
Vector3 gyro2(0, 0, 84);
|
|
|
|
Vector3 gyro3 = rot * gyro;
|
|
|
|
ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);
|
|
|
|
}
|
|
|
|
|
|
TEST(TurnDetection, xx) {
|
|
|
|
Vector3 dst(0, 0, 1);
|
|
Vector3 src(0.0, 2.9, -10.0); src = src.normalized(); // sample accelerometer readings
|
|
|
|
Matrix3 rot = PoseDetection::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;
|
|
|
|
Vector3 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);
|
|
|
|
Vector3 gyro(0, 10, 30);
|
|
|
|
Vector3 gyro2 = rot * gyro;
|
|
int i = 0; (void) i;
|
|
|
|
}
|
|
|
|
#endif
|
|
#endif
|