bug fixed

Author: zihaomu

modules/rgbd/samples/large_kinfu_LCD.cpp

@@ -58,7 +58,7 @@ static const char* keys = {
     " in coarse mode points and normals are displayed }"
     "{idle   | | Do not run LargeKinfu, just display depth frames }"
     "{record | | Write depth frames to specified file list (the same format as for the 'depth' key) }"
-    "{modelBin | | Path to a binary .caffemodel file contains trained network.}"
+    "{modelBin | | Path to a binary .caffemodel file contains trained network which can be download at URL=http://udel.edu/~nmerrill/calc.tar.gz.}"
     "{modelTxt | | Path to a .prototxt file contains the model definition of trained network.}"
     "{width  | 160 | Preprocess input image by resizing to a specific width. It should be multiple by 32. }"
     "{height | 120 | Preprocess input image by resizing to a specific height. It should be multiple by 32. }"
@@ -79,7 +79,8 @@ static const std::string message =
     "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
     "\nto demonstrate Submap based large environment reconstruction"
     "\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger "
-    "reconstructions by default\n";
+    "reconstructions by default\n"
+    "\n The DNN model can be downdload at URL=http://udel.edu/~nmerrill/calc.tar.gz .\n";
 
 int main(int argc, char** argv)
 {

modules/rgbd/src/large_kinfu.cpp

@@ -323,8 +323,8 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth, const Mat& _img)
             if(ifLoop && tarSubmapID != -1 && currentSubmapId != tarSubmapID)
             {
                 // Adding Loop Edge for optimize. If the Edge is duplicate, then skip.
-                submapMgr->addEdgeToCurrentSubmap(currentSubmapId, tarSubmapID);
-                CV_LOG_INFO(NULL, "There is Loop Closure!!!! New edge was added.");
+                if(submapMgr->addEdgeToCurrentSubmap(currentSubmapId, tarSubmapID))
+                    CV_LOG_INFO(NULL, "There is Loop Closure!!!! New edge was added from Submap :"<<currentSubmapId<<" to Submap:"<<tarSubmapID);
             }
         }
     }

modules/rgbd/src/loop_closure_detection.cpp

@@ -4,7 +4,7 @@
 
 #include "precomp.hpp"
 #include "loop_closure_detection.hpp"
-
+#include "keyframe.cpp"
 namespace cv{
 namespace large_kinfu{
 
@@ -73,10 +73,24 @@ 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();
 
+    // If the candidate frame does not belong to the same submapID,
+    // it means that it is impossible to specify the target SubmapID.
     while (iter != candidateKFs.end() ) {
         Ptr<KeyFrame> keyFrameDB = KFDataBase->getKeyFrameByID(*iter);
         if(tempSubmapID == -1)
@@ -96,21 +110,25 @@ bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID)
 
     if(!candidateKFs.empty())
         bestLoopFrame = KFDataBase->getKeyFrameByID(candidateKFs[0]);
+    else
+        return false;
 
     // find target submap ID
-    if(bestLoopFrame->submapID == -1)
+    if(bestLoopFrame->submapID == -1 || bestLoopFrame->submapID == currentSubmapID)
         return false;
     else
     {
         tarSubmapID = bestLoopFrame->submapID;
         preLoopedKFID = currentFrameID;
         currentFrameID = -1;
+
         return true;
     }
 }
 
 void LoopClosureDetectionImpl::addFrame(InputArray _img, const int frameID, const int submapID, int& tarSubmapID, bool& ifLoop)
 {
+
     CV_Assert(!_img.empty());
     currentFrameID = frameID;
     currentSubmapID = submapID;
@@ -145,14 +163,16 @@ void LoopClosureDetectionImpl::reset()
     KFDataBase->reset();
 }
 
-void LoopClosureDetectionImpl::processFrame(InputArray img, OutputArray output)
+void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& output)
 {
-    Mat blob = dnn::blobFromImage(img, 1.0/255.0, inputSize);
+    Mat imgBlur, outMat;
+    cv::GaussianBlur(img, imgBlur, cv::Size(7, 7), 0);
+    Mat blob = dnn::blobFromImage(imgBlur, 1.0/255.0, inputSize);
     net->setInput(blob);
-    net->forward(output);
+    net->forward(outMat);
 
-    Mat outputFeature = output.getMat();
-    outputFeature /= norm(outputFeature);
+    outMat /= norm(outMat);
+    output = outMat.clone();
 
     //! Add ORB feature.
 }

modules/rgbd/src/loop_closure_detection.hpp

@@ -22,7 +22,7 @@ class LoopClosureDetectionImpl : public LoopClosureDetection
 
     void reset() CV_OVERRIDE;
 
-    void processFrame(InputArray img, OutputArray DNNfeature);
+    void processFrame(InputArray img, Mat& DNNfeature);
 
     bool newFrameCheck();
 
@@ -43,7 +43,7 @@ class LoopClosureDetectionImpl : public LoopClosureDetection
     int preLoopedKFID = -1;
 
     double similarityHigh = 0.94;
-    double similarityLow = 0.92;
+    double similarityLow = 0.91;
 
 };
 

modules/rgbd/src/submap.hpp

@@ -67,7 +67,7 @@ class Submap
     }
 
     // Adding new Edge for LCD. Return true or false to indicate whether adding success.
-    void addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose);
+    bool addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose);
 
     //! TODO: Possibly useless
     virtual void setStartFrameId(int _startFrameId) { startFrameId = _startFrameId; };
@@ -122,7 +122,7 @@ void Submap<MatType>::updatePyrPointsNormals(const int pyramidLevels)
 }
 
 template<typename MatType>
-void Submap<MatType>::addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose)
+bool Submap<MatType>::addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose)
 {
     // duplicate check.
     auto iter = constraints.find(tarSubmapID);
@@ -136,6 +136,11 @@ void Submap<MatType>::addEdgeToSubmap(const int tarSubmapID, const Affine3f& tar
         // Create new Edge.
         PoseConstraint& preConstrain = getConstraint(tarSubmapID);
         preConstrain.accumulatePose(estimatePose, 1);
+
+        return true;
+    } else
+    {
+        return false;
     }
 }
 
@@ -187,7 +192,7 @@ class SubmapManager
 
     int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose);
 
-    void addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID);
+    bool addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID);
 
     bool updateMap(int _frameId, std::vector<MatType> _framePoints, std::vector<MatType> _frameNormals);
 
@@ -419,13 +424,12 @@ bool SubmapManager<MatType>::shouldChangeCurrSubmap(int _frameId, int toSubmapId
 }
 
 template<typename MatType>
-void SubmapManager<MatType>::addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID)
+bool SubmapManager<MatType>::addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID)
 {
     Ptr<SubmapT> currentSubmap = getSubmap(currentSubmapID);
     Ptr<SubmapT> tarSubmap = getSubmap(tarSubmapID);
 
-    currentSubmap->addEdgeToSubmap(tarSubmapID, tarSubmap->pose);
-
+    return currentSubmap->addEdgeToSubmap(tarSubmapID, tarSubmap->pose);
 }
 
 template<typename MatType>