diff --git a/include/laser/feature/feature_scene_falko.h b/include/laser/feature/feature_scene_falko.h index 69d449fe653d7b3003d61b4230f553ba2bdf4b90..1d584c999059ad60385ef51476dd6ae1d8cdde17 100644 --- a/include/laser/feature/feature_scene_falko.h +++ b/include/laser/feature/feature_scene_falko.h @@ -43,6 +43,7 @@ template <typename D> class FeatureSceneFalko : public FeatureBase ~FeatureSceneFalko() override{}; std::shared_ptr<laserscanutils::SceneFalko<D>> getScene() { return scene_; } + std::shared_ptr<const laserscanutils::SceneFalko<D>> getScene() const { return scene_; } private: std::shared_ptr<laserscanutils::SceneFalko<D>> scene_; diff --git a/include/laser/processor/processor_loop_closure_falko.h b/include/laser/processor/processor_loop_closure_falko.h index d05b1f5d083aee9bf1fb05bf1abfd5492d6bda28..cf9bfb58bfa78df8ecb470c0a71972f0170ee2c5 100644 --- a/include/laser/processor/processor_loop_closure_falko.h +++ b/include/laser/processor/processor_loop_closure_falko.h @@ -51,7 +51,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosureFalko); */ struct MatchLoopClosureFalko : public MatchLoopClosure { - laserscanutils::MatchLoopClosureScenePtr match_falko_; + laserscanutils::MatchLoopClosureScenePtr match_falko_; }; WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureFalko); @@ -255,8 +255,6 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure ~ProcessorLoopClosureFalko() { outdata.close(); }; - std::list<MatchLoopClosurePtr> match_list_; - protected: /** \brief Function not implemented **/ @@ -539,8 +537,10 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure return match_lc_map_robust; } - void printOutData(std::list<laserscanutils::sceneBasePtr> &list_scene_reference, int num_ref_scenes, - std::map<double, MatchLoopClosurePtr> &match_lc_map, std::vector<falkolib::FALKO> &keypoints_list, + void printOutData(const std::list<laserscanutils::sceneBasePtr> &list_scene_reference, + int num_ref_scenes, + const std::map<double, MatchLoopClosurePtr> &match_lc_map, + const std::vector<falkolib::FALKO> &keypoints_list, int acum_score) { if (init_outData_ == 1) @@ -596,12 +596,24 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure std::chrono::duration<long, std::micro> duration_validation_nok; std::chrono::duration<long, std::micro> duration_extract_scene; + std::list<MatchLoopClosurePtr> match_list_; + int num_ok_; + int num_nok_; + int init_outData_; + public: - unsigned int getNStoredFrames() { return buffer_frame_.getContainer().size(); } - unsigned int getNStoredCaptures() { return buffer_capture_.getContainer().size(); } - int num_ok_; - int num_nok_; - int init_outData_; + SizeStd getNStoredFrames() const { return buffer_frame_.size(); } + SizeStd getNStoredCaptures() const { return buffer_capture_.size(); } + int getNumOk() const {return num_ok_;} + int getNumNOk() const {return num_nok_;} + int getInitOutData() const {return init_outData_;} + std::list<MatchLoopClosureConstPtr> getMatchList() const + { + std::list<MatchLoopClosureConstPtr> const_list; + for (auto el : match_list_) + const_list.push_back(el); + return const_list; + } }; WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnBsc); diff --git a/test/gtest_processor_loop_closure_falko.cpp b/test/gtest_processor_loop_closure_falko.cpp index 87d7868972e2be0f9e4c88e5a9ab869a0907408b..13b2338a8737c6c8ed9296a3d32ab40df531d72e 100644 --- a/test/gtest_processor_loop_closure_falko.cpp +++ b/test/gtest_processor_loop_closure_falko.cpp @@ -409,17 +409,17 @@ TEST_F(ProcessorLoopClosureFalkoTest, findLoopClosures) // captureCallback processor->captureCallback(cap1); - EXPECT_EQ(processor->match_list_.size(), 0); + EXPECT_EQ(processor->getMatchList().size(), 0); processor->captureCallback(cap2); - EXPECT_EQ(processor->match_list_.size(), 1); - EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1); - EXPECT_EQ(processor->match_list_.back()->capture_target, cap2); + EXPECT_EQ(processor->getMatchList().size(), 1); + EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1); + EXPECT_EQ(processor->getMatchList().back()->capture_target, cap2); processor->captureCallback(cap3); - EXPECT_EQ(processor->match_list_.size(), 3); - EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1); - EXPECT_EQ(processor->match_list_.back()->capture_target, cap3); + EXPECT_EQ(processor->getMatchList().size(), 3); + EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1); + EXPECT_EQ(processor->getMatchList().back()->capture_target, cap3); // Verify factors EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 0); @@ -474,7 +474,7 @@ TEST_F(ProcessorLoopClosureFalkoTest, TestExtractScene) std::vector<double> x_ref, x_target; std::vector<double> y_ref, y_target; - auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front()); + auto match_falko = std::static_pointer_cast<const MatchLoopClosureFalko>(processor->getMatchList().front()); auto associations = match_falko->match_falko_->associations; @@ -677,7 +677,7 @@ TEST_F(ProcessorLoopClosureFalkoTest, TestProcessCapture) // std::cout << "reference keypoints size : " << key_ref.size() << std::endl; // std::cout << "target keypoints size : " << key_target.size() << std::endl; - // std::cout << "processor->match_list_.size() : " << processor->match_list_.size() << std::endl; + // std::cout << "processor->getMatchList().size() : " << processor->getMatchList().size() << std::endl; std::vector<double> x_ref_all, x_target_all; std::vector<double> y_ref_all, y_target_all; @@ -698,7 +698,7 @@ TEST_F(ProcessorLoopClosureFalkoTest, TestProcessCapture) std::vector<double> x_ref, x_target; std::vector<double> y_ref, y_target; - auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front()); + auto match_falko = std::static_pointer_cast<const MatchLoopClosureFalko>(processor->getMatchList().front()); auto associations = match_falko->match_falko_->associations; for (auto asso : associations) diff --git a/test/gtest_processor_loop_closure_falko_icp.cpp b/test/gtest_processor_loop_closure_falko_icp.cpp index a12e6ecd45e89bc7f6e92021e28a5800d14e97b0..a6e4baf9bf24842fe69b34385905677c32a03b61 100644 --- a/test/gtest_processor_loop_closure_falko_icp.cpp +++ b/test/gtest_processor_loop_closure_falko_icp.cpp @@ -101,17 +101,17 @@ TEST_F(ProcessorLoopClosureFalkoIcpTest, validatefindLoopClosures) // captureCallback processor->captureCallback(cap1); - EXPECT_EQ(processor->match_list_.size(), 0); + EXPECT_EQ(processor->getMatchList().size(), 0); processor->captureCallback(cap2); - EXPECT_EQ(processor->match_list_.size(), 1); - EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1); - EXPECT_EQ(processor->match_list_.back()->capture_target, cap2); + EXPECT_EQ(processor->getMatchList().size(), 1); + EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1); + EXPECT_EQ(processor->getMatchList().back()->capture_target, cap2); processor->captureCallback(cap3); - EXPECT_EQ(processor->match_list_.size(), 3); - EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1); - EXPECT_EQ(processor->match_list_.back()->capture_target, cap3); + EXPECT_EQ(processor->getMatchList().size(), 3); + EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1); + EXPECT_EQ(processor->getMatchList().back()->capture_target, cap3); } int main(int argc, char **argv)