set new parameter for HF-Net

Author: zihaomu

.vscode/settings.json

@@ -139,13 +139,15 @@ int KeyFrameDatabase::getLastKeyFrameID()
 double KeyFrameDatabase::score(InputArray feature1, InputArray feature2)
 {
     Mat mat1, mat2;
+
     mat1 = feature1.getMat();
     mat2 = feature2.getMat();
+    CV_Assert(mat1.size() == mat2.size());
     double out = mat2.dot(mat1);
     return out;
 }
 
-std::vector<int> KeyFrameDatabase::getCandidateKF(const Mat& currentFeature, const double& similarityLow, double& bestSimilarity, int& bestId)
+std::vector<int> KeyFrameDatabase::getCandidateKF(const Mat& currentFeature, const int currentSubmapID, const double& similarityLow, double& bestSimilarity, int& bestId )
 {
     std::vector<int> cadidateKFs;
     float similarity;
@@ -154,6 +156,8 @@ std::vector<int> KeyFrameDatabase::getCandidateKF(const Mat& currentFeature, con
 
     for(std::map<int, Ptr<KeyFrame> >::const_iterator iter = DataBase.begin(); iter != DataBase.end(); iter++)
     {
+        if( currentSubmapID != -1 && currentSubmapID == iter->second->submapID)
+            continue;
         similarity = score(currentFeature, iter->second->DNNFeature);
 
         if(similarity > similarityLow)

modules/rgbd/src/keyframe.cpp

@@ -59,7 +59,7 @@ class KeyFrameDatabase
 
     int getLastKeyFrameID();
 
-    std::vector<int> getCandidateKF(const Mat& currentFeature, const double& similarityLow, double& bestSimilarity, int& bestId);
+    std::vector<int> getCandidateKF(const Mat& currentFeature, const int currentSubmapID, const double& similarityLow, double& bestSimilarity, int& bestId);
 
     double score(InputArray feature1, InputArray feature2);
 

modules/rgbd/src/keyframe.hpp

@@ -43,7 +43,9 @@ bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID)
     std::vector<int> candidateKFs;
 
     // Find candidate key frames which similarity are greater than the similarityLow.
-    candidateKFs = KFDataBase->getCandidateKF(currentDNNFeature, similarityLow, maxScore, bestId);
+    candidateKFs = KFDataBase->getCandidateKF(currentDNNFeature, currentSubmapID, similarityLow, maxScore, bestId);
+
+    CV_LOG_INFO(NULL, "LCD: Best Frame ID = " << bestId<<", similarity = "<<maxScore);
 
     if( candidateKFs.empty() || maxScore < similarityHigh)
         return false;
@@ -77,18 +79,6 @@ bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID)
         }
     }
 
-    // Remove the keyframe belonging to the currentSubmap.
-    iter = candidateKFs.begin();
-    while (iter != candidateKFs.end() )
-    {
-        Ptr<KeyFrame> keyFrameDB = KFDataBase->getKeyFrameByID(*iter);
-        if(keyFrameDB->submapID == currentFrameID)
-        {
-            candidateKFs.erase(iter);
-        }
-        iter++;
-    }
-
     // If all candidate KF from the same submap, then return true.
     int tempSubmapID = -1;
     iter = candidateKFs.begin();
@@ -107,7 +97,6 @@ bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID)
         }
         iter++;
     }
-
     // Check whether currentFrame is closed to previous looped Keyframe.
     if(currentFrameID - preLoopedKFID < 20)
         return false;
@@ -157,7 +146,7 @@ bool LoopClosureDetectionImpl::ORBMather(InputArray feature1, InputArray feature
     }
     if(goodMatches.size() < ORBminMathing)
     {
-        CV_LOG_INFO(NULL, "LOOP CLOSURE: There are too few ORB matching pairs.");
+        CV_LOG_INFO(NULL, "LCD: There are too few ORB matching pairs.");
         return false;
     }
     else
@@ -217,6 +206,7 @@ void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& DNNfeature, std
 
     // DNN processing.
     Mat imgBlur, outDNN, outORB;
+    imgBlur = img.getMat();
     Mat blob = dnn::blobFromImage(imgBlur, 1.0, inputSize);
 
     net->setInput(blob);

modules/rgbd/src/loop_closure_detection.cpp

@@ -61,11 +61,11 @@ class LoopClosureDetectionImpl : public LoopClosureDetection
     // Param: HF-Net
     // Github Link: https://github.com/ethz-asl/hfnet
     std::vector<String> outNameDNN;
-    double similarityHigh = 0.64;
-    double similarityLow = 0.60;
+    double similarityHigh = 0.80;
+    double similarityLow = 0.84;
 
 };
 
 }
 }
-#endif
+#endif