diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index a25cf1733f6f78f43bb717613956f38306bbfd1b..4c7852585c53a3cf176ffd63f25cb3449eb62fd8 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -408,8 +408,8 @@ TEST(Odom2D, KF_callback) processor_odom2d->keyFrameCallback(keyframe_2, dt/2); ASSERT_TRUE(problem->check(0)); t += dt; - capture->setTimeStamp(t); - processor_odom2d->process(capture); + capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); + sensor_odom2d->process(capture); ASSERT_TRUE(problem->check(0)); CaptureMotionPtr key_capture_n = std::static_pointer_cast<CaptureMotion>(keyframe_2->getCaptureList().front()); @@ -440,8 +440,8 @@ TEST(Odom2D, KF_callback) processor_odom2d->keyFrameCallback(keyframe_1, dt/2); ASSERT_TRUE(problem->check(0)); t += dt; - capture->setTimeStamp(t); - processor_odom2d->process(capture); + capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); + sensor_odom2d->process(capture); ASSERT_TRUE(problem->check(0)); CaptureMotionPtr key_capture_m = std::static_pointer_cast<CaptureMotion>(keyframe_1->getCaptureList().front()); diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 05ce862fddbdfb76937205e65d49471b0f324795..d4e4f082be12c5a39ecf79e5fbb184a0f6777182 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -65,7 +65,7 @@ TEST(ProcessorBase, KeyFrameCallback) // Track CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk)); - proc_trk->process(capt_trk); + proc_trk->captureCallback(capt_trk); for (size_t ii=0; ii<10; ii++ ) { @@ -74,11 +74,11 @@ TEST(ProcessorBase, KeyFrameCallback) WOLF_INFO("----------------------- ts: ", t , " --------------------------"); capt_odo->setTimeStamp(t); - proc_odo->process(capt_odo); + proc_odo->captureCallback(capt_odo); // Track capt_trk = make_shared<CaptureVoid>(t, sens_trk); - proc_trk->process(capt_trk); + proc_trk->captureCallback(capt_trk); // problem->print(4,1,1,0); diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp index a7fc6818d0b739682eb546923bab9cb40e9bef27..8d48be3958841fd6a7da8e98869997f379bb896d 100644 --- a/test/gtest_processor_loopclosure.cpp +++ b/test/gtest_processor_loopclosure.cpp @@ -29,17 +29,19 @@ public: std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();}; protected: - bool voteComputeFeatures() { return true;}; - bool voteSearchLoopClosure() { return true;}; - bool detectFeatures(CaptureBasePtr cap) { return true;}; - CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) { - for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) { - if (kf->isKey()) { - for (CaptureBasePtr cap : kf->getCaptureList()) { - return cap; } - };} return nullptr; + virtual bool voteComputeFeatures() { return true;}; + virtual bool voteSearchLoopClosure() { return true;}; + virtual bool detectFeatures(CaptureBasePtr cap) { return true;}; + virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) + { + for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) + if (kf->isKey()) + for (CaptureBasePtr cap : kf->getCaptureList()) + if (cap != _capture) + return cap; + return nullptr; }; - void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) { + virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) { std::cout << "factor created\n"; *factor_created = true; }; @@ -79,16 +81,16 @@ TEST(ProcessorLoopClosure, installProcessor) Matrix3s P = Matrix3s::Identity() * 0.1; problem->setPrior(x, P, t, dt/2); // KF1 + // new KF t += dt; - FrameBasePtr kf = problem->emplaceFrame(KEY, x, t); //KF2 - proc_lc->keyFrameCallback(kf, dt/2); - - // new capture - CaptureVoidPtr capt_lc(make_shared<CaptureVoid>(t, sens_lc)); + auto kf = problem->emplaceFrame(KEY, x, t); //KF2 + // emplace a capture in KF + auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc); proc_lc->captureCallback(capt_lc); - proc_lc->process(capt_lc); + // callback KF + proc_lc->keyFrameCallback(kf, dt/2); ASSERT_TRUE(factor_created); } diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 57d7a5c685f6e60aa13d57829e8f83b9afcb3233..e27e5cfbfe531427802ea12c40a157875596afbc 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -80,7 +80,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraight) capture->setTimeStamp(t); capture->setData(data); capture->setDataCovariance(data_cov); - processor->process(capture); + processor->captureCallback(capture); WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); } @@ -99,7 +99,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircle) capture->setTimeStamp(t); capture->setData(data); capture->setDataCovariance(data_cov); - processor->process(capture); + processor->captureCallback(capture); WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); } @@ -120,7 +120,7 @@ TEST_F(ProcessorMotion_test, Interpolate) capture->setTimeStamp(t); capture->setData(data); capture->setDataCovariance(data_cov); - processor->process(capture); + processor->captureCallback(capture); motions.push_back(processor->getMotion(t)); WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); }