88 lines
2.4 KiB
C++
88 lines
2.4 KiB
C++
/*
|
|
*This file is part of msckf_vio
|
|
*
|
|
* msckf_vio is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* msckf_vio is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with msckf_vio. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <iostream>
|
|
#include <Eigen/Dense>
|
|
#include <gtest/gtest.h>
|
|
#include <msckf_vio/math_utils.hpp>
|
|
|
|
using namespace std;
|
|
using namespace Eigen;
|
|
using namespace msckf_vio;
|
|
|
|
TEST(MathUtilsTest, skewSymmetric) {
|
|
Vector3d w(1.0, 2.0, 3.0);
|
|
Matrix3d w_hat = skewSymmetric(w);
|
|
Vector3d zero_vector = w_hat * w;
|
|
|
|
FullPivLU<Matrix3d> lu_helper(w_hat);
|
|
EXPECT_EQ(lu_helper.rank(), 2);
|
|
EXPECT_DOUBLE_EQ(zero_vector.norm(), 0.0);
|
|
return;
|
|
}
|
|
|
|
TEST(MathUtilsTest, quaternionNormalize) {
|
|
Vector4d q(1.0, 1.0, 1.0, 1.0);
|
|
quaternionNormalize(q);
|
|
|
|
EXPECT_DOUBLE_EQ(q.norm(), 1.0);
|
|
return;
|
|
}
|
|
|
|
TEST(MathUtilsTest, quaternionToRotation) {
|
|
Vector4d q(0.0, 0.0, 0.0, 1.0);
|
|
Matrix3d R = quaternionToRotation(q);
|
|
Matrix3d zero_matrix = R - Matrix3d::Identity();
|
|
|
|
FullPivLU<Matrix3d> lu_helper(zero_matrix);
|
|
EXPECT_EQ(lu_helper.rank(), 0);
|
|
return;
|
|
}
|
|
|
|
TEST(MathUtilsTest, rotationToQuaternion) {
|
|
Vector4d q1(0.0, 0.0, 0.0, 1.0);
|
|
Matrix3d I = Matrix3d::Identity();
|
|
Vector4d q2 = rotationToQuaternion(I);
|
|
Vector4d zero_vector = q1 - q2;
|
|
|
|
EXPECT_DOUBLE_EQ(zero_vector.norm(), 0.0);
|
|
return;
|
|
}
|
|
|
|
TEST(MathUtilsTest, quaternionMultiplication) {
|
|
Vector4d q1(2.0, 2.0, 1.0, 1.0);
|
|
Vector4d q2(1.0, 2.0, 3.0, 1.0);
|
|
q1 = q1 / q1.norm();
|
|
q2 = q2 / q2.norm();
|
|
Vector4d q_prod = quaternionMultiplication(q1, q2);
|
|
|
|
Matrix3d R1 = quaternionToRotation(q1);
|
|
Matrix3d R2 = quaternionToRotation(q2);
|
|
Matrix3d R_prod = R1 * R2;
|
|
Matrix3d R_prod_cp = quaternionToRotation(q_prod);
|
|
|
|
Matrix3d zero_matrix = R_prod - R_prod_cp;
|
|
|
|
EXPECT_NEAR(zero_matrix.sum(), 0.0, 1e-10);
|
|
return;
|
|
}
|
|
|
|
int main(int argc, char** argv) {
|
|
testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|