diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp index c48b92836c734472d6c9acfd62171644a6c6311a..8bd0c3a7476892c60786b653f4583656f8241e92 100644 --- a/src/corner_falko_2d.cpp +++ b/src/corner_falko_2d.cpp @@ -91,14 +91,18 @@ int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scan int NewSceneAdded=0; - if (scanNumber % scanInterval == 0){ + if (scanNumber % scanInterval == 0 || scanNumber == 1){ - falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); + findLoopClosure(scan, scanParams); - AddNewReferenceScene(scanFALKO); + if (matchingNumber < refSceneAddingTh) { - NewSceneAdded=1; + falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams); + AddNewReferenceScene(scanFALKO); + + NewSceneAdded=1; + } } return NewSceneAdded; @@ -106,5 +110,4 @@ int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scan } - } // laserscanutils namespace diff --git a/src/corner_falko_2d.h b/src/corner_falko_2d.h index bdab93494a13c3a96eae9a81d9170b940621513f..a3a3048c9fd46004e6e789ed87122537031861ea 100644 --- a/src/corner_falko_2d.h +++ b/src/corner_falko_2d.h @@ -108,6 +108,9 @@ public: int matchingPosition=-1; int keypointsNumberTh=2; + + // Max number of matched keypoints between 2 scenes for the candidate scene be considered a good New reference scene + int refSceneAddingTh =6; }; } /* namespace laserscanutils */ diff --git a/src/examples/corner_falko_demo.cpp b/src/examples/corner_falko_demo.cpp index e9af47f7d89e519686f0908b07ad9c84739d40e6..30358e47788c2b982f8b796bd6255c328a2d8b44 100644 --- a/src/examples/corner_falko_demo.cpp +++ b/src/examples/corner_falko_demo.cpp @@ -35,10 +35,14 @@ int main(int argc, char** argv) CornerFalko2d cornerMatching; - cornerMatching.evaluateNewReferenceScene(scan, laserParams, scanInterval); + int sceneAdded = cornerMatching.evaluateNewReferenceScene(scan, laserParams, scanInterval); std::cout << "num keypoints1 extracted: " << cornerMatching.lastKeypointSet.size() << std::endl; + if (sceneAdded==1){ + std::cout << "NewRefSceneAdded!!! " << std::endl; + } + cornerMatching.findLoopClosure(scan, laserParams); std::cout << "matching number : " << cornerMatching.matchingNumber << std::endl;