FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

worked on grid-walking

Browse Source 
worked on grid-generation
added helper library for nav-meshes
started working on nav meshes
This commit is contained in:
FrankE
2018-01-08 20:55:50 +01:00
parent c346b7f222
commit ca6fed5371
33 changed files with 12991 additions and 1913 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
tests/sensors/imu/TestTurnDetection.cpp
View File

@@ -6,33 +6,33 @@
TEST(TurnDetection, rotationMatrix) {
Eigen::Vector3f dst; dst << 0, 0, 1;
Eigen::Vector3f src; src << 1, 1, 0; src.normalize();
Vector3 dst(0, 0, 1);
Vector3 src(1, 1, 0); src = src.normalized();
// get a matrix that rotates "src" into "dst"
Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst);
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
Eigen::Vector3f res = rot * src;
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);
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) {
Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
Eigen::Vector3f acc; acc << 0, 7.0, 7.0;
Vector3 zAxis(0, 0, 1);
Vector3 acc(0, 7.0, 7.0);
Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(acc, zAxis);
Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
Eigen::Vector3f gyro; gyro << 0, 60, 60;
Vector3 gyro(0, 60, 60);
Eigen::Vector3f gyro2; gyro2 << 0, 0, 84;
Vector3 gyro2(0, 0, 84);
Eigen::Vector3f gyro3 = rot * gyro;
Vector3 gyro3 = rot * gyro;
ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);
@@ -41,10 +41,10 @@ TEST(TurnDetection, gyroRotate) {
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
Vector3 dst(0, 0, 1);
Vector3 src(0.0, 2.9, -10.0); src = src.normalized(); // sample accelerometer readings
Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst);
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
// Eigen::Vector3f x; x << 1, 0, 0;
// Eigen::Vector3f z = src.normalized();
@@ -55,14 +55,14 @@ TEST(TurnDetection, xx) {
// rot.row(1) = y;
// rot.row(2) = z;
Eigen::Vector3f res = rot * src;
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);
Eigen::Vector3f gyro; gyro << 0, 10, 30;
Vector3 gyro(0, 10, 30);
Eigen::Vector3f gyro2 = rot * gyro;
Vector3 gyro2 = rot * gyro;
int i = 0; (void) i;
}