diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index 4c7852585c53a3cf176ffd63f25cb3449eb62fd8..7033299e56cc5cf6c243fb1ed5422a2d7d56774b 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -113,6 +113,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D) // GND // `absolute` is made with FactorFix // `motion` is made with FactorOdom2D + // Odom sensor extrinsics: Tr_s = identity std::cout << std::setprecision(4); @@ -126,20 +127,22 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D) ProblemPtr Pr = Problem::create("PO", 2); CeresManager ceres_manager(Pr); + SensorBasePtr sensor_odom2d = Pr->installSensor("ODOM 2D", "odom", Vector3s(0,0,0)); + // KF0 and absolute prior FrameBasePtr F0 = Pr->setPrior(x0, P0,t0, dt/2); // KF1 and motion from KF0 t += dt; FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t); - auto C1 = CaptureBase::emplace<CaptureBase>(F1, "ODOM 2D", t); + auto C1 = CaptureBase::emplace<CaptureBase>(F1, "ODOM 2D", t, sensor_odom2d); auto f1 = FeatureBase::emplace<FeatureBase>(C1, "ODOM 2D", delta, delta_cov); auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr); // KF2 and motion from KF1 t += dt; FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t); - auto C2 = CaptureBase::emplace<CaptureBase>(F2, "ODOM 2D", t); + auto C2 = CaptureBase::emplace<CaptureBase>(F2, "ODOM 2D", t, sensor_odom2d); auto f2 = FeatureBase::emplace<FeatureBase>(C2, "ODOM 2D", delta, delta_cov); auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr); @@ -486,4 +489,3 @@ int main(int argc, char **argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } -