diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index ff61e98421bee4da4c49d6a96b535286802a8d69..6ba1c65f79007633d68d9cef636ff6bf4c9de2c6 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -144,7 +144,7 @@ int main() // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN =================================================== TimeStamp t(0.0); FrameBasePtr KF1 = problem->applyPriorOptions(t); - std::static_pointer_cast<ProcessorMotion>(problem->getMotionProvider())->setOrigin(KF1); + std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1); // SELF CALIBRATION =================================================== // These few lines control whether we calibrate some sensor parameters or not. diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml index c35ecb64e9c79a5e9108284e1d4134f4cf7b37f1..c46580effcef4d12a9e4829d6a1c9cfdb8794da3 100644 --- a/demos/hello_wolf/yaml/hello_wolf_config.yaml +++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml @@ -13,7 +13,7 @@ config: $sigma: P: [0.31, 0.31] O: [0.31] - time_tolerance: 0.1 + time_tolerance: 0.5 tree_manager: type: "none" diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 258f3255af666bef03a6658f4ac1f2de801eb145..5e35324c2ebc2e84838a4ba23ecf0ac208bd84a5 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -198,7 +198,7 @@ class Problem : public std::enable_shared_from_this<Problem> void removeMotionProvider(MotionProviderPtr proc); public: - MotionProviderPtr getMotionProvider(); +// MotionProviderPtr getMotionProvider(); std::map<int,MotionProviderPtr>& getMotionProviderMap(); const std::map<int,MotionProviderPtr>& getMotionProviderMap() const; @@ -449,12 +449,12 @@ inline bool Problem::isPriorSet() const return prior_options_ == nullptr; } -inline MotionProviderPtr Problem::getMotionProvider() -{ - if (not motion_provider_map_.empty()) - return motion_provider_map_.begin()->second; - return nullptr; -} +//inline MotionProviderPtr Problem::getMotionProvider() +//{ +// if (not motion_provider_map_.empty()) +// return motion_provider_map_.begin()->second; +// return nullptr; +//} inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() { diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index e5b0fa3872c25fbaa1741df9a84646588d3fdc04..461e7c130cc1dba7a08d9b0cadffaf7c0ea7bc7b 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -85,7 +85,7 @@ TEST(Problem, Processor) ProblemPtr P = Problem::create("PO", 3); // check motion processor is null - ASSERT_FALSE(P->getMotionProvider()); + ASSERT_TRUE(P->getMotionProviderMap().empty()); // add a motion sensor and processor auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d()); @@ -94,7 +94,7 @@ TEST(Problem, Processor) auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>()); // check motion processor IS NOT by emplace - ASSERT_EQ(P->getMotionProvider(), Pm); + ASSERT_EQ(P->getMotionProviderMap().begin()->second, Pm); } TEST(Problem, Installers) @@ -109,16 +109,16 @@ TEST(Problem, Installers) auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer"); // check motion processor IS NOT set - ASSERT_FALSE(P->getMotionProvider()); + ASSERT_TRUE(P->getMotionProviderMap().empty()); // install processor motion ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml"); // check motion processor is set - ASSERT_TRUE(P->getMotionProvider() != nullptr); + ASSERT_FALSE(P->getMotionProviderMap().empty()); // check motion processor is correct - ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getMotionProvider()), pm); + ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getMotionProviderMap().begin()->second), pm); } TEST(Problem, SetOrigin_PO_2d)