From 184606f6f96a88f629960aecff46af17265c2e04 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 1 Feb 2019 12:18:21 +0100
Subject: [PATCH] Little fixes, remove unnecessary fcns, document
---
src/processor_motion.cpp | 6 +++---
src/processor_motion.h | 4 ++++
src/test/gtest_processor_motion.cpp | 27 +++++++++------------------
3 files changed, 16 insertions(+), 21 deletions(-)
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 450cd9d8c..cfca3dd80 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 4e20cab46..8e9f4bf83 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 77d2a5518..94a2f0c15 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);
--
GitLab