diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 7dd71060608bd87f8b3188f9827f213d181322e6..6aafb68474cf2c95f12544b18f50bc0e809b284d 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -29,418 +29,328 @@
using namespace Eigen;
using namespace wolf;
-// Input odometry data and covariance
-Matrix6d data_cov = 0.1*Matrix6d::Identity();
-Matrix5d delta_cov = 0.1*Matrix5d::Identity();
-
-// Jacobian of the bias
-MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0 ).finished());
-//preintegration bias
-Vector3d b0_preint = Vector3d::Zero();
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 2);
-SolverCeres solver(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
-
-// Imu2d sensor
-SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h
-
-//capture from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
-auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
-
-TEST(FactorImu2d, check_tree)
+class FactorImu2d_test : public testing::Test
{
- ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST(FactorImu2d, bias_zero_solve_f1)
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ SensorImu2dPtr sensor;
+ FrameBasePtr frm0, frm1;
+ CaptureImuPtr cap0, cap1;
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias;
+ Vector3d b0_preint;
+
+ void SetUp() override
+ {
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias = ((MatrixXd(5, 3) << 1, 0, 0, //
+ 0, 1, 0, //
+ 0, 0, 1, //
+ 1, 0, 0, //
+ 0, 1, 0).finished());
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // sensor
+ sensor = SensorBase::emplace<SensorImu2d>(problem->getHardware(), Vector3d::Zero(), ParamsSensorImu2d());
+ sensor->setStateBlockDynamic('I', false);
+
+ // Two frames
+ frm0 = problem->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+ frm1 = problem->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+
+ // capture from frm1 to frm0
+ cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, cap0);
+ }
+};
+
+TEST_F(FactorImu2d_test, problem_check)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- frm1->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ ASSERT_TRUE(problem->check(0));
}
-TEST(FactorImu2d, bias_zero_solve_f0)
+TEST_F(FactorImu2d_test, bias_zero_solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- frm0->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.01);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, bias_zero_solve_b1)
+TEST_F(FactorImu2d_test, bias_zero_solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Zero();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.01);
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_b0)
+TEST_F(FactorImu2d_test, bias_zero_solve)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->unfix();
- frm1->fix();
- cap1->fix();
- cap0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
-}
+ for (int i = 0; i < 50; i++)
+ {
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
-TEST(FactorImu2d, solve_b1)
-{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Random();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and frm1, unfix bias, perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.01);
+ frm0->fix();
+ frm1->fix();
+
+ // solve to estimate bias at cap1
+ auto report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f0)
+TEST_F(FactorImu2d_test, solve_b0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- frm0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f1)
+TEST_F(FactorImu2d_test, solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- frm1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.1);
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f1_b1)
+TEST_F(FactorImu2d_test, solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->unfix();
- frm1->perturb(0.1);
- cap1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+ // ::testing::GTEST_FLAG(filter) = "FactorImu2d_test.solve_f1_b1"; // Test only this one
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 931fcd3db298b8aa27e4f0a9edf189fbc98f9bb5..51ee79d6ee34e460d02c6022a016828d1bdfd627 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -29,572 +29,473 @@
using namespace Eigen;
using namespace wolf;
-// Input odometry data and covariance
-Matrix6d data_cov = 0.1*Matrix6d::Identity();
-Matrix5d delta_cov = 0.1*Matrix5d::Identity();
-
-// Jacobian of the bias
-MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0 ).finished());
-//preintegration bias
-Vector3d b0_preint = Vector3d::Zero();
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 2);
-SolverCeres solver(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
-
-// Imu2d sensor
-ParamsSensorImu2dPtr sensorparams = std::make_shared<ParamsSensorImu2d>(false);
-SensorBasePtr sensor = SensorBase::emplace<SensorImu2d>(problem_ptr->getHardware(), Vector3d::Zero(), *sensorparams );
-
-//capture from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
-auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
-
-TEST(FactorImu2dWithGravity, check_tree)
+class FactorImu2dWithGravity_test : public testing::Test
{
- ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST(FactorImu2dWithGravity, bias_zero_solve_f1)
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ SensorImu2dPtr sensor;
+ FrameBasePtr frm0, frm1;
+ CaptureImuPtr cap0, cap1;
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias;
+ Vector3d b0_preint;
+
+ void SetUp() override
+ {
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias = ((MatrixXd(5, 3) << 1, 0, 0, //
+ 0, 1, 0, //
+ 0, 0, 1, //
+ 1, 0, 0, //
+ 0, 1, 0)
+ .finished());
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // sensor
+ ParamsSensorImu2dPtr sensorparams = std::make_shared<ParamsSensorImu2d>(false);
+ sensor = SensorBase::emplace<SensorImu2d>(problem->getHardware(), Vector3d::Zero(), sensorparams);
+ sensor->setStateBlockDynamic('I', false);
+
+ // Two frames
+ frm0 = problem->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+ frm1 = problem->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+
+ // capture from frm1 to frm0
+ cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, cap0);
+
+ problem->print(4,1,1,1);
+ }
+};
+
+TEST_F(FactorImu2dWithGravity_test, check_tree)
{
- for(int i = 0; i < 50; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- //std::cout << report << std::endl;
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ ASSERT_TRUE(problem->check(0));
}
-TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1 and biases, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.01);
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ // std::cout << report << std::endl;
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Zero();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1 and biases, perturb frm0
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.01);
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_b0)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->unfix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- cap0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap0->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.01);
+ frm0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve to estimate bias at cap1
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_b1)
+TEST_F(FactorImu2dWithGravity_test, solve_b0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Random();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and frm1, unfix bias, perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.1);
+ frm0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f0)
+TEST_F(FactorImu2dWithGravity_test, solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.1);
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f1)
+TEST_F(FactorImu2dWithGravity_test, solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.1);
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f1_b1)
-{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- sensor->getStateBlock('G')->fix();
- frm1->unfix();
- cap1->unfix();
- frm1->perturb(0.1);
- cap1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
-}
-TEST(FactorImu2dWithGravity, bias_zero_solve_G)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_G)
{
- for(int i = 0; i < 50; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- //Vector2d g = Vector2d::Random()*5;
- Vector2d g = Vector2d::Zero();
-
- //Zero bias
- Vector3d b0 = Vector3d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and biases, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ // Vector2d g = Vector2d::Random()*5;
+ Vector2d g = Vector2d::Zero();
+
+ // Zero bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and biases, perturb g
+ sensor->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_G)
+TEST_F(FactorImu2dWithGravity_test, solve_G)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and captures, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and captures, perturb g
+ sensor->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
int main(int argc, char **argv)