diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index 8e97b7f177ef034f94e18c881c94630fcff96668..39f0af8be47f69bb928cc06e3e0318b1759f2435 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -444,10 +444,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) Vector3d x1(1.5, -1.5, -M_PI/2.0); Vector3d x2(3.0, -3.0, 0.0); // Matrix3d P0; P0.setIdentity(); - VectorComposite P0(Vector3d(0,0,0), "PO", {2,1}); + VectorComposite s0(Vector3d(0,0,0), "PO", {2,1}); // set prior at t=0 and processor origin - auto F0 = problem->setPriorFactor(x0, P0, t, 0.1); + auto F0 = problem->setPriorFactor(x0, s0, t, 0.1); processor->setOrigin(F0); // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2) @@ -578,10 +578,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) Vector3d x1(1.5, -1.5, -M_PI/2.0); Vector3d x2(3.0, -3.0, 0.0); // Matrix3d P0; P0.setIdentity(); - VectorComposite P0(Vector3d(1,1,1), "PO", {2,1}); + VectorComposite s0(Vector3d(1,1,1), "PO", {2,1}); // set prior at t=0 and processor origin - auto F0 = problem->setPriorFactor(x0, P0, t, 0.1); + auto F0 = problem->setPriorFactor(x0, s0, t, 0.1); processor->setOrigin(F0); // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2) diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index b4c3e267bb5f408c2b54252dfe999c26e4246f21..8d97fc8d063ed1a08c36b25f259353cc74852f76 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -120,7 +120,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) TimeStamp t0(0.0), t = t0; double dt = .01; VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); - VectorComposite P0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1}); + VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1}); Vector3d delta (2,0,0); Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02; @@ -128,7 +128,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) CeresManager ceres_manager(Pr); // KF0 and absolute prior - FrameBasePtr F0 = Pr->setPriorFactor(x0, P0,t0, dt/2); + FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2); // KF1 and motion from KF0 t += dt; @@ -171,8 +171,8 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver)); ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6); - auto P0_vector = P0.vector("PO"); - ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((P0_vector.array() * P0_vector.array()).matrix().asDiagonal()), 1e-6); + auto s0_vector = s0.vector("PO"); + ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((s0_vector.array() * s0_vector.array()).matrix().asDiagonal()), 1e-6); ASSERT_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6); ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6); ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6); @@ -191,7 +191,7 @@ TEST(Odom2d, VoteForKfAndSolve) // Vector3d x0(0,0,0); VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); // Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1; - VectorComposite P0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1}); + VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1}); // motion data VectorXd data(Vector2d(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01; @@ -221,7 +221,7 @@ TEST(Odom2d, VoteForKfAndSolve) CeresManager ceres_manager(problem); // Origin Key Frame - auto KF = problem->setPriorFactor(x0, P0, t, dt/2); + auto KF = problem->setPriorFactor(x0, s0, t, dt/2); processor_odom2d->setOrigin(KF); ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 7704bd572763577eb2bf8185e2dd3c602fd2c04c..509f495c1bd4c56be08f2d70279e6fba9a5b6ccc 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -109,8 +109,8 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior) // Vector3d x0; x0 << 0, 0, 0; VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); // Matrix3d P0; P0.setIdentity(); - VectorComposite P0(Vector3d(1,1,1), "PO", {2,1}); - auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01); + VectorComposite s0(Vector3d(1,1,1), "PO", {2,1}); + auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01); processor->setOrigin(KF_0); data << 1, 0; // advance straight @@ -136,7 +136,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior) // Vector3d x0; x0 << 0, 0, 0; // Matrix3d P0; P0.setIdentity(); VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); - VectorComposite P0(Vector3d(1,1,1), "PO", {2,1}); + VectorComposite s0(Vector3d(1,1,1), "PO", {2,1}); auto KF_0 = problem->setPriorFix(x0, t, 0.01); processor->setOrigin(KF_0); @@ -186,8 +186,8 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior) // Vector3d x0; x0 << 0, 0, 0; VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); // Matrix3d P0; P0.setIdentity(); - VectorComposite P0(Vector3d(1,1,1), "PO", {2,1}); - auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01); + VectorComposite s0(Vector3d(1,1,1), "PO", {2,1}); + auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01); processor->setOrigin(KF_0); data << 1, 2*M_PI/10; // advance in circle @@ -213,7 +213,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior) // Vector3d x0; x0 << 0, 0, 0; // Matrix3d P0; P0.setIdentity(); VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); - VectorComposite P0(Vector3d(1,1,1), "PO", {2,1}); + VectorComposite s0(Vector3d(1,1,1), "PO", {2,1}); auto KF_0 = problem->setPriorFix(x0, t, 0.01); processor->setOrigin(KF_0); @@ -281,8 +281,8 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior) // Vector3d x0; x0 << 0, 0, 0; VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); // Matrix3d P0; P0.setIdentity(); - VectorComposite P0(Vector3d(1,1,1), "PO", {2,1}); - auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01); + VectorComposite s0(Vector3d(1,1,1), "PO", {2,1}); + auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01); processor->setOrigin(KF_0); data << 1, 2*M_PI/10; // advance in circle @@ -326,7 +326,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior) // Vector3d x0; x0 << 0, 0, 0; // Matrix3d P0; P0.setIdentity(); VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); - VectorComposite P0(Vector3d(1,1,1), "PO", {2,1}); + VectorComposite s0(Vector3d(1,1,1), "PO", {2,1}); auto KF_0 = problem->setPriorFix(x0, t, 0.01); processor->setOrigin(KF_0);