diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index a6c8592ea5548eaacb5da8a409a5bf84e9590eba..0c31cdfc4d79c85cf632d3e56443e7cb5159fa3b 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -70,7 +70,7 @@ TEST(FrameBase, LinksToTree) intrinsics_odo.k_rot_to_rot = 1; auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo); auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr); WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size()); auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>()); diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index b13061e657badbd2ed81e9ea82b0f6078f3803f0..fc30487fe96765f36caea276defddc7abee9a6ac 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -352,9 +352,11 @@ TEST_F(ProcessorDiffDriveTest, linear) data(0) = 100.0 ; // one turn of the wheels data(1) = 100.0 ; + t += 1.0; auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front()); C->process(); + WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose()); // radius is 1.0m, 100 ticks per revolution, so advanced distance is @@ -377,6 +379,7 @@ TEST_F(ProcessorDiffDriveTest, angular) processor->setOrigin(F0); // Straight one turn of the wheels, in one go + t += 1.0; data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse data(1) = 20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 384ca142cee72d01b58275f1faf664b3d89616f9..215122442dbc1c2cc9d032fcd169ec11a2237f0e 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -569,14 +569,17 @@ TEST(SolverManager, MultiThreadingTruncatedNotifications) // loop emplacing and removing frames (window of 10 KF) auto start = std::chrono::high_resolution_clock::now(); + TimeStamp ts(0); while (true) { // Emplace Frame, Capture, feature and factor pose 2d - FrameBasePtr F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero()); - auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + FrameBasePtr F = P->emplaceKeyFrame(ts, P->stateZero()); + auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity()); auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); + ts += 1.0; + if (P->getTrajectory()->getFrameList().size() > 10) (*P->getTrajectory()->begin())->remove();