diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp index 450cd9d8c2333e06ef76a269939430fe42a18705..cfca3dd80ed5b12cf9d92f3c51a954c924aef379 100644 --- a/src/processor_motion.cpp +++ b/src/processor_motion.cpp @@ -501,7 +501,7 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta else { // _ts is closest to _second - Motion interpolated ( _second ); + Motion interpolated ( _second ); interpolated.ts_ = _ts; _second.data_ . setZero(); _second.data_cov_ . setZero(); @@ -518,7 +518,7 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const { // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp - // Note: since the buffer goes from a FK through the past until the previous KF, we need to: + // Note: since the buffer goes from a KF in the past until the next KF, we need to: // 1. See that the KF contains a CaptureMotion // 2. See that the TS is smaller than the KF's TS // 3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer @@ -533,7 +533,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp capture = frame->getCaptureOf(getSensorPtr()); if (capture != nullptr) { - // We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion + // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion capture_motion = std::static_pointer_cast<CaptureMotion>(capture); if (_ts >= capture_motion->getBuffer().get().front().ts_) // Found time stamp satisfying rule 3 above !! ==> break for loop diff --git a/src/processor_motion.h b/src/processor_motion.h index 4e20cab461bfb9cb5555f3091b1abd5d086d6573..8e9f4bf838fd904b1213f171001f18ed1b65a558 100644 --- a/src/processor_motion.h +++ b/src/processor_motion.h @@ -399,6 +399,10 @@ class ProcessorMotion : public ProcessorBase * \param _capture_motion: the parent capture */ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own); + + /** \brief create a feature corresponding to given capture + * \param _capture_motion: the parent capture + */ virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0; /** \brief create a constraint and link it in the wolf tree diff --git a/src/test/gtest_processor_motion.cpp b/src/test/gtest_processor_motion.cpp index 77d2a55186ae6084f54e6832c71bc88067edc081..94a2f0c15f61439d8f5d800d893d6ae95f45016a 100644 --- a/src/test/gtest_processor_motion.cpp +++ b/src/test/gtest_processor_motion.cpp @@ -29,13 +29,17 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{ Vector2s data; Matrix2s data_cov; - ProcessorMotion_test() : ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()) { } + ProcessorMotion_test() : + ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()), + dt(0) + { } virtual void SetUp() { - dt = 1.0; + dt = 1.0; problem = Problem::create("PO 2D"); ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>()); + params->time_tolerance = 0.25; params->dist_traveled = 100; params->angle_turned = 6.28; params->max_time_span = 2.5*dt; // force KF at every third process() @@ -52,19 +56,6 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{ virtual void TearDown(){} - virtual Motion interpolate(const Motion& _ref, - Motion& _second, - TimeStamp& _ts) - { - return ProcessorMotion::interpolate(_ref, _second, _ts); - } - - virtual Motion motionZero(TimeStamp& t) - { - return ProcessorOdom2D::motionZero(t); - } - - }; @@ -93,7 +84,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircle) data_cov.setIdentity(); TimeStamp t(0.0); - for (int i = 0; i<10; i++) // one full turn + for (int i = 0; i<10; i++) // one full turn exactly { t += dt; capture->setTimeStamp(t); @@ -108,13 +99,13 @@ TEST_F(ProcessorMotion_test, IntegrateCircle) TEST_F(ProcessorMotion_test, Interpolate) { - data << 1, 2*M_PI/10; // advance straight + data << 1, 2*M_PI/10; // advance in turn data_cov.setIdentity(); TimeStamp t(0.0); std::vector<Motion> motions; motions.push_back(motionZero(t)); - for (int i = 0; i<10; i++) + for (int i = 0; i<10; i++) // one full turn exactly { t += dt; capture->setTimeStamp(t);