diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 3ba49645c9c81613e3cc32855ca16786dd17dcd9..3d0eba08b8e234a1adcd3ab4bd00a149e10e6f65 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -178,10 +178,8 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
IsMotionPtr getProcessorIsMotion();
- std::map<int,IsMotionPtr> getProcessorIsMotionMap();
-
-
-
+ std::map<int,IsMotionPtr>& getProcessorIsMotionMap();
+ const std::map<int,IsMotionPtr>& getProcessorIsMotionMap() const;
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
@@ -437,8 +435,12 @@ inline IsMotionPtr Problem::getProcessorIsMotion()
return nullptr;
}
+inline std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap()
+{
+ return processor_is_motion_map_;
+}
-inline std::map<int,IsMotionPtr> Problem::getProcessorIsMotionMap()
+inline const std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap() const
{
return processor_is_motion_map_;
}
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
index 4309f6a68fb889ac2151b5e33c6429d80ce23b9b..b3a510472087094a38682081e7d27cd3b00a07b6 100644
--- a/src/processor/is_motion.cpp
+++ b/src/processor/is_motion.cpp
@@ -14,9 +14,11 @@ void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
_prb_ptr->addProcessorIsMotion(_motion_ptr);
}
-void IsMotion::setOdometry(const VectorComposite& _zero_odom)
+void IsMotion::setOdometry(const VectorComposite& _odom)
{
+ assert(_odom.includesStructure(state_structure_) && "IsMotion::setOdometry(): provided odom has not any key.");
+
for (auto key : state_structure_)
- if (_zero_odom.count(key) != 0)
- odometry_[key] = _zero_odom.at(key); //overwrite or insert only keys of the state_structure_
+ if (_odom.count(key) != 0)
+ odometry_[key] = _odom.at(key); //overwrite or insert only keys of the state_structure_
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 885c1f53a456deb4b74392d011d560cb7216bafe..8a5eb8fa3a068dca72974a86bdcdee1fd9453baf 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -84,6 +84,10 @@ target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
+# IsMotion classes test
+wolf_add_gtest(gtest_is_motion gtest_is_motion.cpp)
+target_link_libraries(gtest_is_motion ${PLUGIN_NAME})
+
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
target_link_libraries(gtest_local_param ${PLUGIN_NAME})
diff --git a/test/dummy/processor_is_motion_dummy.h b/test/dummy/processor_is_motion_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..dca4808c90602d550156a9e486514480b2c7f6b1
--- /dev/null
+++ b/test/dummy/processor_is_motion_dummy.h
@@ -0,0 +1,76 @@
+/*
+ * processor_is_motion_dummy.h
+ *
+ * Created on: Mar 8, 2021
+ * Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+
+#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorIsMotionDummy);
+
+struct ParamsProcessorIsMotionDummy : public ParamsProcessorBase, public ParamsIsMotion
+{
+ std::string state_structure = "PO";
+
+ ParamsProcessorIsMotionDummy() = default;
+ ParamsProcessorIsMotionDummy(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server),
+ ParamsIsMotion(_unique_name, _server)
+ {
+
+ };
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorIsMotionDummy);
+
+class ProcessorIsMotionDummy : public ProcessorBase, public IsMotion
+{
+ public:
+ ProcessorIsMotionDummy(ParamsProcessorIsMotionDummyPtr _params) :
+ ProcessorBase("ProcessorIsMotionDummy", 2, _params),
+ IsMotion(_params->state_structure, _params)
+ {}
+ ~ProcessorIsMotionDummy(){};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorIsMotionDummy, ParamsProcessorIsMotionDummy);
+
+ void configure(SensorBasePtr _sensor) override {};
+ void processCapture(CaptureBasePtr) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
+ bool triggerInCapture(CaptureBasePtr) const override { return false; };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override { return false; };
+ bool storeKeyFrame(FrameBasePtr) override { return false; };
+ bool storeCapture(CaptureBasePtr) override { return false; };
+ bool voteForKeyFrame() const override { return false; };
+ TimeStamp getTimeStamp() const override {return TimeStamp(0);};
+
+ VectorComposite getState(const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorIsMotionDummy);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorIsMotionDummy);
+} // namespace wolf
+
+#endif /* TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ */
diff --git a/test/gtest_is_motion.cpp b/test/gtest_is_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0c8b8fa620a1f8b5261d9a5ca13a6299c896d6d9
--- /dev/null
+++ b/test/gtest_is_motion.cpp
@@ -0,0 +1,185 @@
+//Wolf
+#include "core/utils/utils_gtest.h"
+
+#include "core/processor/is_motion.h"
+#include "dummy/processor_is_motion_dummy.h"
+#include "core/sensor/sensor_odom_2d.h"
+
+#include "core/problem/problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+class IsMotionTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sen;
+ ProcessorBasePtr prc1, prc2, prc3;
+ IsMotionPtr im1, im2, im3;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ double dt = 0.01;
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("POV", 2);
+
+ // Install odom sensor
+ sen = problem->installSensor("SensorOdom2d",
+ "odometer",
+ Vector3d(0,0,0),
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+ // Install 3 odom processors
+ ParamsProcessorIsMotionDummyPtr prc1_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc1_params->time_tolerance = dt/2;
+ prc1_params->state_structure = "PO";
+ prc1_params->state_getter = false;
+ prc1 = problem->installProcessor("ProcessorIsMotionDummy",
+ "not getter processor",
+ sen,
+ prc1_params);
+ im1 = std::dynamic_pointer_cast<IsMotion>(prc1);
+
+ ParamsProcessorIsMotionDummyPtr prc2_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc2_params->time_tolerance = dt/2;
+ prc2_params->state_structure = "PO";
+ prc2_params->state_getter = true;
+ prc2_params->state_priority = 1;
+ prc2 = problem->installProcessor("ProcessorIsMotionDummy",
+ "getter processor",
+ sen,
+ prc2_params);
+ im2 = std::dynamic_pointer_cast<IsMotion>(prc2);
+
+ ParamsProcessorIsMotionDummyPtr prc3_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ prc3_params->time_tolerance = dt/2;
+ prc3_params->state_structure = "POV";
+ prc3_params->state_getter = true;
+ prc3_params->state_priority = 1;
+ prc3 = problem->installProcessor("ProcessorIsMotionDummy",
+ "getter processor low priority",
+ sen,
+ prc3_params);
+ im3 = std::dynamic_pointer_cast<IsMotion>(prc3);
+ }
+};
+
+/*
+ * Things to be tested:
+ * - state_getter
+ * - state_priority
+ * - Duplicated priority (Problem handles this)
+ * - setOdometry (stateStructures)
+ * - getOdomtry
+ * - Problem::getState/getOdometry (state_getter, state_priority)
+ */
+
+TEST_F(IsMotionTest, install)
+{
+ // All isMotion() = true
+ ASSERT_TRUE (prc1->isMotion());
+ ASSERT_TRUE (prc2->isMotion());
+ ASSERT_TRUE (prc3->isMotion());
+ ASSERT_TRUE (im1 != nullptr);
+ ASSERT_TRUE (im2 != nullptr);
+ ASSERT_TRUE (im3 != nullptr);
+
+ // well configured
+ ASSERT_FALSE(im1->isStateGetter());
+ ASSERT_TRUE(im2->isStateGetter());
+ ASSERT_TRUE(im3->isStateGetter());
+ ASSERT_EQ(im2->getStatePriority(), 1);
+ ASSERT_EQ(im3->getStatePriority(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
+ ASSERT_EQ(im1->getStateStructure(), "PO");
+ ASSERT_EQ(im2->getStateStructure(), "PO");
+ ASSERT_EQ(im3->getStateStructure(), "POV");
+
+ // Only 2 and 3 in problem::processor_is_motion_map_
+ ASSERT_EQ(problem->getProcessorIsMotionMap().size(), 2);
+ ASSERT_EQ(problem->getProcessorIsMotionMap().begin()->second, im2);
+ ASSERT_EQ(std::next(problem->getProcessorIsMotionMap().begin())->second, im3);
+}
+
+TEST_F(IsMotionTest, odometry)
+{
+ VectorComposite odom_p("P"); // missing P key
+ odom_p['P'] = Vector2d::Zero();
+ VectorComposite odom_pov("POV"); // key V not needed by prc1 and prc2
+ odom_pov['P'] = Vector2d::Zero();
+ odom_pov['O'] = Vector1d::Zero();
+ odom_pov['V'] = Vector2d::Zero();
+
+ // Error: required PO keys to be added
+ ASSERT_DEATH({im1->setOdometry(odom_p);},"");
+ im1->setOdometry(odom_pov);
+ im2->setOdometry(odom_pov);
+ im3->setOdometry(odom_pov);
+
+ // im1 ->set odom = 0, 0, 0
+ VectorComposite odom1("PO");
+ odom1['P'] = Vector2d::Zero();
+ odom1['O'] = Vector1d::Zero();
+ im1->setOdometry(odom1);
+ auto odom1_get = im1->getOdometry();
+ EXPECT_TRUE(odom1_get.count('P') == 1);
+ EXPECT_TRUE(odom1_get.count('O') == 1);
+ EXPECT_MATRIX_APPROX(odom1_get.at('P'), odom1.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
+
+ // im1 ->set odom = 1, 1, 1
+ VectorComposite odom2("PO");
+ odom2['P'] = Vector2d::Ones();
+ odom2['O'] = Vector1d::Ones();
+ im2->setOdometry(odom2);
+ auto odom2_get = im2->getOdometry();
+ EXPECT_TRUE(odom2_get.count('P') == 1);
+ EXPECT_TRUE(odom2_get.count('O') == 1);
+ EXPECT_MATRIX_APPROX(odom2_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
+
+ // im1 ->set odom = 2, 2, 2, 2, 2
+ VectorComposite odom3("POV");
+ odom3['P'] = 2 * Vector2d::Ones();
+ odom3['O'] = 2 * Vector1d::Ones();
+ odom3['V'] = 2 * Vector2d::Ones();
+ im3->setOdometry(odom3);
+ auto odom3_get = im3->getOdometry();
+ EXPECT_TRUE(odom3_get.count('P') == 1);
+ EXPECT_TRUE(odom3_get.count('O') == 1);
+ EXPECT_TRUE(odom3_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(odom3_get.at('P'), odom3.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom3_get.at('O'), odom3.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom3_get.at('V'), odom3.at('V'), 1e-9);
+
+ // problem::getOdometry(): by priority P and O should come from im2 and V from im3
+ auto odom_get = problem->getOdometry();
+ EXPECT_TRUE(odom_get.count('P') == 1);
+ EXPECT_TRUE(odom_get.count('O') == 1);
+ EXPECT_TRUE(odom_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(odom_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom_get.at('O'), odom2.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(odom_get.at('V'), odom3.at('V'), 1e-9);
+
+ // problem::getState(): by priority P and O should come from im2 and V from im3
+ auto state_get = problem->getState();
+ EXPECT_TRUE(state_get.count('P') == 1);
+ EXPECT_TRUE(state_get.count('O') == 1);
+ EXPECT_TRUE(state_get.count('V') == 1);
+ EXPECT_MATRIX_APPROX(state_get.at('P'), odom2.at('P'), 1e-9);
+ EXPECT_MATRIX_APPROX(state_get.at('O'), odom2.at('O'), 1e-9);
+ EXPECT_MATRIX_APPROX(state_get.at('V'), odom3.at('V'), 1e-9);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index c3277078df9300439a6dee3721f0084df9751522..4497954899991b26100d23af38130e8218ed4a14 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -48,10 +48,16 @@ TEST(ProcessorBase, IsMotion)
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");
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d",
+ "odometer",
+ Vector3d(0,0,0),
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d",
+ "odom processor",
+ sens_odo,
+ proc_odo_params);
ASSERT_FALSE(proc_trk->isMotion());
ASSERT_TRUE (proc_odo->isMotion());