diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 2d1689e5aff3f510e8d1e8fef029dfa55ec855b3..6089b8aac12432072854ec8c469129133cf4a784 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -105,16 +105,16 @@ public: /** \brief Create and update the scene struct with keypoints and descriptors **/ - sceneFalkoBSCPtr extractScene(LaserScan &scan, LaserScanParams &scanParams) { - auto newScene = std::make_shared<sceneFalko<D>>(); - auto scanFALKO = - loopClosureFalko<D, Extr, M>::convert2LaserScanFALKO(scan, scanParams); + sceneFalkoBSCPtr extractScene(LaserScan &_scan, LaserScanParams &_scan_params) { + auto new_scene = std::make_shared<sceneFalko<D>>(); + auto scan_falko = + convert2LaserScanFALKO(_scan, _scan_params); // Extract keypoints - extract((*scanFALKO), (newScene->keypointsList)); + extract(*scan_falko, (new_scene->keypointsList)); // Compute descriptors - extractor_.compute(*scanFALKO, newScene->keypointsList, - newScene->descriptorsList); - return newScene; + extractor_.compute(*scan_falko, new_scene->keypointsList, + new_scene->descriptorsList); + return new_scene; } /** \brief Convert scans from laserscanutils::LaserScan to @@ -132,11 +132,11 @@ public: /** \brief Create and update a matchLoopClosure struct with the info that is *produced when matching two given scenes **/ - matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1, - sceneFalkoBSCPtr scene2) { - std::vector<std::pair<int, int>> assoNN; + matchLoopClosurePtr matchScene(sceneFalkoBSCPtr _scene1, + sceneFalkoBSCPtr _scene2) { + std::vector<std::pair<int, int>> asso_nn; int matching_number = - matcher_.match(scene1->keypointsList, scene2->keypointsList, assoNN); + matcher_.match(_scene1->keypointsList, _scene2->keypointsList, asso_nn); auto new_match = std::make_shared<matchLoopClosure<D>>(); new_match->keypointsNumberMatch = matching_number; if (matching_number > keypoints_number_th) { @@ -144,7 +144,7 @@ public: } else { new_match->match = false; } - new_match->sceneTuple = std::make_tuple(scene1, scene2); + new_match->sceneTuple = std::make_tuple(_scene1, _scene2); return new_match; } @@ -152,14 +152,14 @@ public: *closures **/ std::list<matchLoopClosurePtr> - findLoopClosure(std::list<sceneFalkoBSCPtr> &l_scenes, - const sceneFalkoBSCPtr &new_scene) { - int number_ref_sc = l_scenes.size(); + findLoopClosure(std::list<sceneFalkoBSCPtr> &_l_scenes, + const sceneFalkoBSCPtr &_new_scene) { + int number_ref_sc = _l_scenes.size(); std::list<matchLoopClosurePtr> matchings; for (int i = 0; i < number_ref_sc; i++) { - auto l_front = l_scenes.begin(); + auto l_front = _l_scenes.begin(); std::advance(l_front, i); - auto new_match = matchScene(*l_front, new_scene); + auto new_match = matchScene(*l_front, _new_scene); matchings.push_back(new_match); } return matchings; diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp index 0373da06354febb8c3970a587df735adde135d6b..ac80971bc8290ac54ce4ffd5bdc7e2ff8c7b8d31 100644 --- a/test/gtest_loop_closure_falko.cpp +++ b/test/gtest_loop_closure_falko.cpp @@ -6,89 +6,51 @@ using namespace laserscanutils; TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { - int scanSize = 1440; + // Initialization + int scan_size = 1440; LaserScan scan; - LaserScanParams laserParams; - - laserParams.angle_min_ = 0; - laserParams.angle_max_ = 2.0 * M_PI; - - for (int i = 0; i < scanSize; i++) { + LaserScanParams laser_params; + laser_params.angle_min_ = 0; + laser_params.angle_max_ = 2.0 * M_PI; + for (int i = 0; i < scan_size; i++) { scan.ranges_raw_.push_back(testRanges1[i]); } - parameterLoopClosureFalko param; - loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko(param); - - - auto new_scene = LCFalko.extractScene(scan, laserParams); + loopClosureFalko<bsc, bscExtractor, NNMatcher> loop_cl_falko(param); - int detectedKeypoints = ((*new_scene).keypointsList).size(); + // Test convert2LaserScanFALKO + std::shared_ptr<falkolib::LaserScan> scan_falko = + loop_cl_falko.convert2LaserScanFALKO(scan, laser_params); + int firstPoint = scan_falko->ranges[0]; + ASSERT_EQ(firstPoint, 250); + + // Test extractScene + auto new_scene = loop_cl_falko.extractScene(scan, laser_params); + int detectedKeypoints = new_scene->keypointsList.size(); int detectedDescriptors = new_scene->descriptorsList.size(); - ASSERT_EQ(detectedKeypoints, 18); - ASSERT_EQ(detectedDescriptors, 18); - // Test matcher - - auto new_match = LCFalko.matchScene(new_scene, new_scene); - + // Test matcheScene + auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); int keypoints_matched = new_match->keypointsNumberMatch; - ASSERT_EQ(keypoints_matched, 18); // TEST findLoopClosure - std::list<std::shared_ptr<sceneFalko<bsc>>> ref_scenes; - ref_scenes.push_back(new_scene); - ref_scenes.push_back(new_scene); - - auto matchings = LCFalko.findLoopClosure(ref_scenes, new_scene); - + auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); int matchings_number = matchings.size(); - ASSERT_EQ(matchings_number, 2); - auto l_front = matchings.begin(); - auto there_is_match = l_front->get()->match; - ASSERT_EQ(there_is_match, true); // PRINTF("All good at TestTest::DummyTestExample !\n"); } -TEST(loop_closure_falko2, convert2laserScanFalko) { - int scanSize = 1440; - - LaserScan scan; - LaserScanParams laserParams; - - laserParams.angle_min_ = 0; - laserParams.angle_max_ = 2.0 * M_PI; - - for (int i = 0; i < scanSize; i++) { - scan.ranges_raw_.push_back(testRanges1[i]); - } - - parameterLoopClosureFalko param; - loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko(param); - - - std::shared_ptr<falkolib::LaserScan> scanFALKO = - LCFalko.convert2LaserScanFALKO(scan, laserParams); - - int firstPoint = ((*scanFALKO).ranges)[0]; - - ASSERT_EQ(firstPoint, 250); - - // PRINTF("All good at TestTest::DummyTestExample !\n"); -} - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS();