diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7d1e07237c0c45ada958f0b662e2cf9d469ebfec..fdc81d7a6ed05ce6f166e14f198009c63ad64dad 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -258,6 +258,7 @@ SET(HDRS_LANDMARK
include/core/landmark/landmark_match.h
)
SET(HDRS_PROCESSOR
+ include/core/processor/is_motion.h
include/core/processor/motion_buffer.h
include/core/processor/processor_base.h
include/core/processor/processor_diff_drive.h
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 2d2f0e572e9b0ce32729d4a9da4b4e2e135206fd..9d306d5983883edea43f8712f07484d2086df065 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -21,6 +21,7 @@ struct ProcessorParamsBase;
#include "core/utils/params_server.hpp"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
+#include "core/processor/is_motion.h"
// std includes
#include <mutex>
@@ -45,7 +46,8 @@ class Problem : public std::enable_shared_from_this<Problem>
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
- ProcessorMotionPtr processor_motion_ptr_;
+ IsMotionPtr processor_motion_ptr_;
+// IsMotionPtr is_motion_ptr_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -147,12 +149,12 @@ class Problem : public std::enable_shared_from_this<Problem>
*
* Set the processor motion.
*/
- void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
- ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
+ void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
+ IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
void clearProcessorMotion();
public:
- ProcessorMotionPtr& getProcessorMotion();
+ IsMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
@@ -360,7 +362,7 @@ inline bool Problem::priorIsSet() const
return prior_is_set_;
}
-inline ProcessorMotionPtr& Problem::getProcessorMotion()
+inline IsMotionPtr& Problem::getProcessorMotion()
{
return processor_motion_ptr_;
}
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
new file mode 100644
index 0000000000000000000000000000000000000000..f3cf0d781f39c9fda6ffb9e64817ceed0acfa162
--- /dev/null
+++ b/include/core/processor/is_motion.h
@@ -0,0 +1,84 @@
+/**
+ * \file is_motion.h
+ *
+ * Created on: Mar 10, 2020
+ * \author: jsola
+ */
+
+#ifndef PROCESSOR_IS_MOTION_H_
+#define PROCESSOR_IS_MOTION_H_
+
+#include "core/common/wolf.h"
+
+namespace wolf
+{
+
+class TimeStamp;
+
+WOLF_PTR_TYPEDEFS(IsMotion);
+
+class IsMotion
+{
+ public:
+
+ virtual ~IsMotion();
+
+ // Queries to the processor:
+
+ /** \brief Fill a reference to the state integrated so far
+ * \param _x the returned state vector
+ */
+ virtual void getCurrentState(Eigen::VectorXd& _x) const = 0;
+ virtual void getCurrentTimeStamp(TimeStamp& _ts) const = 0;
+ /** \brief Fill the state corresponding to the provided time-stamp
+ * \param _ts the time stamp
+ * \param _x the returned state
+ * \return if state in the provided time-stamp could be resolved
+ */
+ virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const = 0;
+
+
+ // Overloaded getters
+ Eigen::VectorXd getCurrentState() const;
+ TimeStamp getCurrentTimeStamp() const;
+ Eigen::VectorXd getState(const TimeStamp& _ts) const;
+
+};
+
+}
+
+///// IMPLEMENTATION ///////
+#include "core/common/time_stamp.h"
+
+namespace wolf{
+
+inline IsMotion::~IsMotion()
+{
+}
+
+inline Eigen::VectorXd IsMotion::getCurrentState() const
+{
+ Eigen::VectorXd x;
+ getCurrentState(x);
+ return x;
+}
+
+inline TimeStamp IsMotion::getCurrentTimeStamp() const
+{
+ TimeStamp ts;
+ getCurrentTimeStamp(ts);
+ return ts;
+}
+
+inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
+{
+ Eigen::VectorXd x;
+ getState(_ts, x);
+ return x;
+}
+
+
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_IS_MOTION_H_ */
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c4c325aac18b0435357c73af53e44f7a208186ae..bd39ef49da82605edd6f48bb27337c9823ef8ed9 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -9,6 +9,7 @@ class SensorBase;
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/processor/is_motion.h"
#include "core/sensor/sensor_base.h"
#include "core/frame/frame_base.h"
#include "core/common/time_stamp.h"
@@ -338,7 +339,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
public:
- virtual bool isMotion() const;
+ bool isMotion() const;
void setTimeTolerance(double _time_tolerance);
@@ -376,7 +377,8 @@ inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
inline bool ProcessorBase::isMotion() const
{
- return false;
+ // check if this inherits from IsMotion
+ return (std::dynamic_pointer_cast<const IsMotion>(shared_from_this()) != nullptr);
}
inline unsigned int ProcessorBase::id() const
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 88ee84a24470cc715adc8bf512db0d8004e827b0..b08862e676c294dba7e0a82d59b9cc4891a0812c 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -11,6 +11,7 @@
// Wolf
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
#include "core/utils/params_server.hpp"
@@ -129,7 +130,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
* // TODO: JS: review instructions up to here
*
*/
-class ProcessorMotion : public ProcessorBase
+class ProcessorMotion : public ProcessorBase, public IsMotion
{
public:
typedef enum {
@@ -159,32 +160,23 @@ class ProcessorMotion : public ProcessorBase
virtual void resetDerived();
// Queries to the processor:
- virtual bool isMotion() const override;
/** \brief Fill a reference to the state integrated so far
* \param _x the returned state vector
*/
- void getCurrentState(Eigen::VectorXd& _x) const;
- void getCurrentTimeStamp(TimeStamp& _ts) const { _ts = getBuffer().get().back().ts_; }
+ virtual void getCurrentState(Eigen::VectorXd& _x) const override;
+ virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; }
+ using IsMotion::getCurrentState;
+ using IsMotion::getCurrentTimeStamp;
- /** \brief Get the state integrated so far
- * \return the state vector
- */
- Eigen::VectorXd getCurrentState() const;
- TimeStamp getCurrentTimeStamp() const;
/** \brief Fill the state corresponding to the provided time-stamp
* \param _ts the time stamp
* \param _x the returned state
* \return if state in the provided time-stamp could be resolved
*/
- bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const;
-
- /** \brief Get the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \return the state vector
- */
- Eigen::VectorXd getState(const TimeStamp& _ts) const;
+ virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const override;
+ using IsMotion::getState;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
@@ -506,25 +498,6 @@ inline bool ProcessorMotion::voteForKeyFrame() const
return false;
}
-inline Eigen::VectorXd ProcessorMotion::getState(const TimeStamp& _ts) const
-{
- Eigen::VectorXd x(getProblem()->getFrameStructureSize());
- getState(_ts, x);
- return x;
-}
-
-inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const
-{
- return getBuffer().get().back().ts_;
-}
-
-inline Eigen::VectorXd ProcessorMotion::getCurrentState() const
-{
- Eigen::VectorXd x(getProblem()->getFrameStructureSize());
- getCurrentState(x);
- return x;
-}
-
inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
{
// ensure integrity
@@ -556,11 +529,6 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
return capture_ptr->getBuffer().getMotion(_ts);
}
-inline bool ProcessorMotion::isMotion() const
-{
- return true;
-}
-
inline double ProcessorMotion::updateDt()
{
return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index e5e61d3f097779e48a179a37b51c7be5b87038c8..ed07d3de55a3ec8e4b9181cb8c9b3af85ba4f1cb 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -110,9 +110,6 @@ class ProcessorTracker : public ProcessorBase
ProcessorParamsTrackerPtr _params_tracker);
virtual ~ProcessorTracker();
- virtual bool isMotion() const final override {return false; }
-
-
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c25761dc9c73a56d9d7e0e728c0d7e794844aa8d..c2fd1334f5c3d95f14f698be81c1ec72ac87c4ce 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -229,7 +229,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
// setting the main processor motion
if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
+ processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
return prc_ptr;
}
@@ -291,7 +291,7 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
+IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
{
for (auto sen : getHardware()->getSensorList()) // loop all sensors
{
@@ -307,7 +307,7 @@ ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_nam
if ((*prc_it)->isMotion()) // found, and it's motion!
{
std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(*prc_it);
+ processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
return processor_motion_ptr_;
}
else // found, but it's not motion!
@@ -322,7 +322,7 @@ ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_nam
return nullptr;
}
-void Problem::setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr)
+void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
{
processor_motion_ptr_ = _processor_motion_ptr;
}
@@ -495,16 +495,17 @@ bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
- if (_processor_ptr)
- {
- WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
- }
- else
- {
- WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
- }
-
- processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+ // TODO
+// if (_processor_ptr)
+// {
+// WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+// }
+// else
+// {
+// WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+// }
+//
+// processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
}
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 39b77e59f6980f1a2047db5527a017358572159b..3cf7743c4e673dc358f076db4dbc56376a71f46e 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -71,7 +71,7 @@ void ProcessorBase::remove()
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && P->getProcessorMotion()->id() == this->id())
+ if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
P->clearProcessorMotion();
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c667b7193bc3b566782351371c7d4f314799a93b..39a3d83f425201b6bc148bdd8c3ca93681ac4329 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -1,4 +1,8 @@
+
+
#include "core/processor/processor_motion.h"
+
+
namespace wolf
{
@@ -365,6 +369,9 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint);
VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
+ // ensure proper size of the provided reference
+ _x.resize( getProblem()->getFrameStructureSize() );
+
// Compose on top of origin state using the buffered time stamp, not the query time stamp
double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
statePlusDelta(state_0, delta, dt, _x);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 1b74256d81808822dd1de31eed99e83098511c4d..17f39d09f7ae18275245856e1ad66dcaa3d33bfd 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -122,7 +122,7 @@ TEST(Problem, Installers)
ASSERT_TRUE(P->getProcessorMotion());
// check motion processor is correct
- ASSERT_EQ(P->getProcessorMotion(), pm);
+ ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorMotion()), pm);
}
TEST(Problem, SetOrigin_PO_2D)
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 2aef08f59cfcb594a34dce910a9f7f54eb389671..4d3dfffbf09b9d1582c330b1d3d37f7067fc8d3c 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -31,6 +31,41 @@ WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
} // namespace wolf
+TEST(ProcessorBase, IsMotion)
+{
+ using namespace wolf;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using Eigen::Vector2d;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ double dt = 0.01;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ // Install tracker (sensor and processor)
+ auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorDummy",
+ nullptr,
+ nullptr,
+ nullptr,
+ 2);
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
+
+ // Install odometer (sensor and processor)
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2D", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
+ proc_odo_params->time_tolerance = dt/2;
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2D", "odom processor", sens_odo, proc_odo_params);
+
+ ASSERT_FALSE(proc_trk->isMotion());
+ ASSERT_TRUE (proc_odo->isMotion());
+}
+
+
TEST(ProcessorBase, KeyFrameCallback)
{