From a39d026acd2ff8bb77c63ed1ff97fe605608444d Mon Sep 17 00:00:00 2001 From: Zihao Mu Date: Thu, 26 Aug 2021 11:23:05 +0800 Subject: [PATCH 1/5] Loop Closure Detection for 3D Large Kinfu --- modules/rgbd/CMakeLists.txt | 2 +- .../rgbd/include/opencv2/rgbd/large_kinfu.hpp | 32 +- modules/rgbd/samples/large_kinfu_LCD_demo.cpp | 312 ++++++++++++++++++ modules/rgbd/src/keyframe.cpp | 210 ++++++++++++ modules/rgbd/src/keyframe.hpp | 86 +++++ modules/rgbd/src/large_kinfu.cpp | 85 ++++- modules/rgbd/src/loop_closure_detection.cpp | 234 +++++++++++++ modules/rgbd/src/loop_closure_detection.hpp | 71 ++++ 8 files changed, 1017 insertions(+), 15 deletions(-) create mode 100644 modules/rgbd/samples/large_kinfu_LCD_demo.cpp create mode 100644 modules/rgbd/src/keyframe.cpp create mode 100644 modules/rgbd/src/keyframe.hpp create mode 100644 modules/rgbd/src/loop_closure_detection.cpp create mode 100644 modules/rgbd/src/loop_closure_detection.hpp diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index aa8a6a93886..6aa20dd1eff 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,3 +1,3 @@ set(the_description "RGBD algorithms") -ocv_define_module(rgbd opencv_core opencv_3d opencv_imgproc OPTIONAL opencv_viz WRAP python) +ocv_define_module(rgbd opencv_core opencv_3d opencv_imgproc opencv_dnn OPTIONAL opencv_features2d opencv_viz WRAP python) diff --git a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp index 9154ecb095e..9e86314090e 100644 --- a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -138,7 +138,37 @@ class CV_EXPORTS_W LargeKinfu virtual const Affine3f getPose() const = 0; - CV_WRAP virtual bool update(InputArray depth) = 0; + CV_WRAP virtual bool update(InputArray depth, InputArray img = noArray()) = 0; + + // Set parameters for the loop closure detection function. + CV_WRAP virtual void setModelForLCD(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId = 0, int targetId = 0) = 0; +}; + + +/** @brief Loop Closing Detection implementation + + This class implements a Loop Closing Detection of 3d reconstruction algorithm for + larger environments using Spatially hashed TSDF volume "Submaps". + + It takes a sequence RGB images and processes each image by HF-Net. + According to the similarity of features extracted by HF-Net, determine whether there is a LOOP. + Original HF-Net was provided by: https://github.com/ethz-asl/hfnet. + Pre-trained model can be found at: https://1drv.ms/u/s!ApQBoiZSe8Evgolqw23hI8D7lP9mKw?e=JKwPHe. + +*/ +class CV_EXPORTS_W LoopClosureDetection { +public: + + CV_WRAP static Ptr create(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId = 0, int targetId = 0); + + virtual ~LoopClosureDetection() = default; + + // Adding Frame. + // If there is loop, function will return TURE, and the tarSubmapID will be set as the target submap ID, otherwise return False. + CV_WRAP virtual bool addFrame(InputArray img, const int frameID, const int submapID, CV_OUT int& tarSubmapID) = 0; + + // Stop run loop closing detection. + CV_WRAP virtual void reset() = 0; }; } // namespace large_kinfu diff --git a/modules/rgbd/samples/large_kinfu_LCD_demo.cpp b/modules/rgbd/samples/large_kinfu_LCD_demo.cpp new file mode 100644 index 00000000000..f80cbde78a1 --- /dev/null +++ b/modules/rgbd/samples/large_kinfu_LCD_demo.cpp @@ -0,0 +1,312 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory + +#include +#include +#include +#include +#include +#include + +#include "io_utils.hpp" + +using namespace cv; +using namespace cv::kinfu; +using namespace cv::large_kinfu; +using namespace cv::io_utils; + +#ifdef HAVE_OPENCV_VIZ +#include +#endif + +#ifdef HAVE_OPENCV_VIZ +const std::string vizWindowName = "cloud"; + +struct PauseCallbackArgs +{ + PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) {} + + LargeKinfu& largeKinfu; +}; + +void pauseCallback(const viz::MouseEvent& me, void* args); +void pauseCallback(const viz::MouseEvent& me, void* args) +{ + if (me.type == viz::MouseEvent::Type::MouseMove || + me.type == viz::MouseEvent::Type::MouseScrollDown || + me.type == viz::MouseEvent::Type::MouseScrollUp) + { + PauseCallbackArgs pca = *((PauseCallbackArgs*)(args)); + viz::Viz3d window(vizWindowName); + UMat rendered; + pca.largeKinfu.render(rendered, window.getViewerPose().matrix); + imshow("render", rendered); + waitKey(1); + } +} +#endif + +static const char* keys = { + "{help h usage ? | | print this message }" + "{depth | | Path to folder with depth.txt and rgb.txt files listing a set of depth and rgb images. }" + "{camera |0| Index of depth camera to be used as a depth source }" + "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," + " 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 .bin file contains trained network which can be download at URL=https://1drv.ms/u/s!ApQBoiZSe8Evgolqw23hI8D7lP9mKw?e=ywHAc5}" + "{modelTxt | | Path to a .xml file contains the model definition of trained network.}" + "{width | 640 | Preprocess input image by resizing to a specific width. }" + "{height | 480 | Preprocess input image by resizing to a specific height. }" + "{backend | 2 | At current stage only openvino available, and other backend will be supported soon." + " Choose one of computation backends: " + "0: automatically (by default), " + "1: Halide language (http://halide-lang.org/), " + "2: Intel's Deep Learning Inference Engine (https://software.intel.com/openvino-toolkit), " + "3: OpenCV implementation }" + "{ target | 0 | Choose one of target computation devices: " + "0: CPU target (by default), " + "1: OpenCL, " + "2: OpenCL fp16 (half-float precision), " + "3: VPU }" +}; + +static const std::string message = + "\nThis demo uses live depth input or RGB-D dataset taken from" + "\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" + "\n The used OpenVINO DNN model can be downdload at URL=https://1drv.ms/u/s!ApQBoiZSe8Evgolqw23hI8D7lP9mKw?e=ywHAc5.\n" + "\n Make sure that OpenVINO DNN backend is available.\n"; + +int main(int argc, char** argv) +{ + bool coarse = false; + bool idle = false; + std::string recordPath, modelBin, modelTxt; + int backend = 0, target = 0, width, height; + + CommandLineParser parser(argc, argv, keys); + parser.about(message); + + if (!parser.check()) + { + parser.printMessage(); + parser.printErrors(); + return -1; + } + + if (parser.has("help")) + { + parser.printMessage(); + return 0; + } + if (parser.has("coarse")) + { + coarse = true; + } + if (parser.has("record")) + { + recordPath = parser.get("record"); + } + if (parser.has("idle")) + { + idle = true; + } + if (parser.has("modelBin")) + { + modelBin = parser.get("modelBin"); + } + if (parser.has("modelTxt")) + { + modelTxt = parser.get("modelTxt"); + } + if (parser.has("width")) + { + width = parser.get("width"); + } + if (parser.has("height")) + { + height = parser.get("height"); + } + if (parser.has("backend")) + { + backend = parser.get("backend"); + } + if (parser.has("target")) + { + target = parser.get("target"); + } + + Ptr ds; + Ptr rgbs; + + if (parser.has("depth")) + ds = makePtr(parser.get("depth") + "/depth.txt"); + else + ds = makePtr(parser.get("camera")); + + //TODO: intrinsics for camera + rgbs = makePtr(parser.get("depth") + "/rgb.txt"); + + if (ds->empty()) + { + std::cerr << "Failed to open depth source" << std::endl; + parser.printMessage(); + return -1; + } + Size inputSize(width, height); + Ptr depthWriter; + if (!recordPath.empty()) + depthWriter = makePtr(recordPath); + + Ptr params; + Ptr largeKinfu; + + params = large_kinfu::Params::hashTSDFParams(coarse); + + // These params can be different for each depth sensor + ds->updateParams(*params); + + cv::setUseOptimized(true); + + if (!idle) + largeKinfu = LargeKinfu::create(params); + + const auto& volParams = largeKinfu->getParams().volumeParams; + + if (!modelBin.empty()) + LargeKinfu->setModelForLCD(modelBin, modelTxt, inputSize, backend, target); + +#ifdef HAVE_OPENCV_VIZ + cv::viz::Viz3d window(vizWindowName); + window.setViewerPose(Affine3f::Identity()); + bool pause = false; +#endif + + UMat rendered; + UMat points; + UMat normals; + + int64 prevTime = getTickCount(); + + for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) + { + if (depthWriter) + depthWriter->append(frame); + + Vec3i volResolution(volParams.resolutionX, + volParams.resolutionY, + volParams.resolutionZ); + Affine3f volPose(Matx44f(volParams.pose)); + + UMat rgb_frame = rgbs->getRGB(); +#ifdef HAVE_OPENCV_VIZ + if (pause) + { + // doesn't happen in idle mode + largeKinfu->getCloud(points, normals); + if (!points.empty() && !normals.empty()) + { + viz::WCloud cloudWidget(points, viz::Color::white()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05, + viz::Color::gray()); + window.showWidget("cloud", cloudWidget); + window.showWidget("normals", cloudNormals); + + Vec3d volSize = volParams.voxelSize * Vec3d(volResolution); + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose); + PauseCallbackArgs pca(*largeKinfu); + window.registerMouseCallback(pauseCallback, (void*)&pca); + window.showWidget("text", + viz::WText(cv::String("Move camera in this window. " + "Close the window or press Q to resume"), + Point())); + window.spin(); + window.removeWidget("text"); + window.removeWidget("cloud"); + window.removeWidget("normals"); + window.registerMouseCallback(0); + } + + pause = false; + } + else +#endif + { + UMat cvt8; + float depthFactor = params->depthFactor; + convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor); + if (!idle) + { + imshow("depth", cvt8); + + if (!largeKinfu->update(frame, rgb_frame)) + { + largeKinfu->reset(); + std::cout << "reset" << std::endl; + } +#ifdef HAVE_OPENCV_VIZ + else + { + if (coarse) + { + largeKinfu->getCloud(points, normals); + if (!points.empty() && !normals.empty()) + { + viz::WCloud cloudWidget(points, viz::Color::white()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, + /*scale*/ 0.05, viz::Color::gray()); + window.showWidget("cloud", cloudWidget); + window.showWidget("normals", cloudNormals); + } + } + + // window.showWidget("worldAxes", viz::WCoordinateSystem()); + Vec3d volSize = volParams.voxelSize * volResolution; + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose); + window.setViewerPose(largeKinfu->getPose()); + window.spinOnce(1, true); + } +#endif + + largeKinfu->render(rendered); + } + else + { + rendered = cvt8; + } + } + + int64 newTime = getTickCount(); + putText(rendered, + cv::format("FPS: %2d press R to reset, P to pause, Q to quit", + (int)(getTickFrequency() / (newTime - prevTime))), + Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); + prevTime = newTime; + imshow("render", rendered); + + int c = waitKey(1); + switch (c) + { + case 'r': + if (!idle) + largeKinfu->reset(); + break; + case 'q': return 0; +#ifdef HAVE_OPENCV_VIZ + case 'p': + if (!idle) + pause = true; +#endif + default: break; + } + } + + return 0; +} diff --git a/modules/rgbd/src/keyframe.cpp b/modules/rgbd/src/keyframe.cpp new file mode 100644 index 00000000000..430809a3a3f --- /dev/null +++ b/modules/rgbd/src/keyframe.cpp @@ -0,0 +1,210 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "precomp.hpp" +#include "keyframe.hpp" + +namespace cv +{ +namespace large_kinfu +{ + +KeyFrame::KeyFrame():submapID(-1), preKeyFrameID(-1) +{ + nextKeyFrameID = -1; +} + +KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID) : DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(-1) +{ + nextKeyFrameID = -1; +}; + +KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID, int _preKeyFrameID):DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(_preKeyFrameID) +{} + +KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID, int _preKeyFrameID, std::vector _keypoints, Mat _ORBFeatures):DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(_preKeyFrameID) +{ + keypoints = _keypoints; + ORBFeatures = _ORBFeatures; +} + +// Using INT_MAX by default. +KeyFrameDatabase::KeyFrameDatabase():maxSizeDB(INT_MAX), lastKeyFrameID(-1) +{ +} + +KeyFrameDatabase::KeyFrameDatabase(int _maxSizeDB):maxSizeDB(_maxSizeDB),lastKeyFrameID(-1) +{ +} + +void KeyFrameDatabase::addKeyFrame( const Mat& _DNNFeature, int _frameID, int _submapID) +{ + std::vector _keypoints; + Mat _ORBFeatures; + addKeyFrameT(_DNNFeature, _frameID, _submapID, _keypoints, _ORBFeatures); +} + +void KeyFrameDatabase::addKeyFrame( const Mat& _DNNFeature, int _frameID, int _submapID, std::vector& _keypoints, const Mat& _ORBFeatures) +{ + addKeyFrameT(_DNNFeature, _frameID, _submapID, _keypoints, _ORBFeatures); +} + +void KeyFrameDatabase::addKeyFrameT(const Mat& DNNFeature, int frameID, int submapID, std::vector& keypoints, const Mat& ORBFeatures) +{ + Ptr kf, preKF; + preKF = getKeyFrameByID(lastKeyFrameID); + + // new start for KeyFrame in different submaps. + if(preKF) + { + kf = makePtr(DNNFeature, submapID, lastKeyFrameID, keypoints, ORBFeatures); + preKF->nextKeyFrameID = frameID; + } + else + { + kf = makePtr(DNNFeature, submapID, -1, keypoints, ORBFeatures); + } + + // Adding new KF to DB + DataBase[frameID] = kf; + + if(int(DataBase.size()) > maxSizeDB) + shrinkDB(); + + // Change the last + lastKeyFrameID = frameID; +} + +bool KeyFrameDatabase::deleteKeyFrameByID(int keyFrameID) +{ + auto keyFrame = DataBase.find(keyFrameID); + + if(keyFrame == DataBase.end()) + { + return false; + } else + { + // Remove nowKF, and link the perKF to nextKF. + Ptr preKF, nextKF, nowKF; + nowKF = keyFrame->second; + preKF = getKeyFrameByID(nowKF->preKeyFrameID); + nextKF = getKeyFrameByID(nowKF->nextKeyFrameID); + + if(preKF) + { + preKF->nextKeyFrameID = nowKF->nextKeyFrameID; + } + + if(nextKF) + { + nextKF->preKeyFrameID = nowKF->preKeyFrameID; + } + + DataBase.erase(keyFrame); + return true; + } +} + +Ptr KeyFrameDatabase::getKeyFrameByID(int keyFrameID) +{ + if(keyFrameID < 0) + return {}; + + auto keyFrame = DataBase.find(keyFrameID); + if(keyFrame == DataBase.end()) + { + return {}; + } else + { + return keyFrame->second; + } +} + +int KeyFrameDatabase::getSize() +{ + return DataBase.size(); +} + +bool KeyFrameDatabase::empty() +{ + return DataBase.empty(); +} + +int KeyFrameDatabase::getLastKeyFrameID() +{ + return lastKeyFrameID; +} + +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 KeyFrameDatabase::getCandidateKF(const Mat& currentFeature, const int currentSubmapID, const double& similarityLow, double& bestSimilarity, int& bestId ) +{ + std::vector cadidateKFs; + float similarity; + + bestSimilarity = 0; + + for(std::map >::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) + { + cadidateKFs.push_back(iter->first); + } + + if(similarity > bestSimilarity) + { + bestSimilarity = similarity; + bestId = iter->first; + } + } + + return cadidateKFs; +} + +// If size of DB is large than the maxSizeDB, then remove some KFs in DB. +void KeyFrameDatabase::shrinkDB() +{ + for(std::map >::const_iterator iter = DataBase.begin(); iter != DataBase.end(); iter++) + { + deleteKeyFrameByID(iter->first); + + iter++; + + if(iter == DataBase.end()) + { + break; + } + } +} + +// Debug Only. +void KeyFrameDatabase::printDB() +{ + for(std::map >::const_iterator iter = DataBase.begin(); iter != DataBase.end(); iter++) + { + std::cout<<"frame Id= "<first<<", feature = "<second->DNNFeature<second->ORBFeatures< keypoints; + Mat ORBFeatures; + + KeyFrame(); + KeyFrame(Mat DNNfeature, int submapID); + KeyFrame(Mat DNNfeature, int submapID, int preKeyFrameID); + KeyFrame(Mat DNNfeature, int submapID, int preKeyFrameID, std::vector keypoints, Mat ORBdescriptors); +}; + +class KeyFrameDatabase +{ +public: + + KeyFrameDatabase(); + + KeyFrameDatabase(int maxSizeDB); + + ~KeyFrameDatabase() = default; + + void addKeyFrame( const Mat& DNNFeature, int frameID, int submapID); + + void addKeyFrame( const Mat& DNNFeature, int frameID, int submapID, std::vector& keypoints, const Mat& ORBFeatures); + + Ptr getKeyFrameByID(int keyFrameID); + + bool deleteKeyFrameByID(int keyFrameID); + + int getSize(); + + bool empty(); + + void reset(); + + int getLastKeyFrameID(); + + std::vector getCandidateKF(const Mat& currentFeature, const int currentSubmapID, const double& similarityLow, double& bestSimilarity, int& bestId); + + double score(InputArray feature1, InputArray feature2); + + // Debug only + void printDB(); + +private: + void addKeyFrameT(const Mat& DNNFeature, int frameID, int submapID, std::vector& keypoints, const Mat& ORBFeatures); + + void shrinkDB(); + + // < keyFrameID, KeyFrame> + std::map > DataBase; + + int maxSizeDB; + int lastKeyFrameID; + + +}; + +}// namespace large_kinfu +}// namespace cv + +#endif diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index a3ae9adbcd3..982a34e42b9 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -123,9 +123,11 @@ class LargeKinfuImpl : public LargeKinfu const Affine3f getPose() const CV_OVERRIDE; - bool update(InputArray depth) CV_OVERRIDE; + bool update(InputArray depth, InputArray img = noArray()) CV_OVERRIDE; - bool updateT(const MatType& depth); + void setModelForLCD(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId = 0, int targetId = 0) CV_OVERRIDE; + + bool updateT(const MatType& depth, const Mat& img); private: Params params; @@ -136,6 +138,8 @@ class LargeKinfuImpl : public LargeKinfu int frameCounter; Affine3f pose; + + Ptr lcd; }; template @@ -161,6 +165,11 @@ void LargeKinfuImpl::reset() frameCounter = 0; pose = Affine3f::Identity(); submapMgr->reset(); + + if( lcd ) + { + lcd->reset(); + } } template @@ -181,42 +190,51 @@ const Affine3f LargeKinfuImpl::getPose() const } template<> -bool LargeKinfuImpl::update(InputArray _depth) +bool LargeKinfuImpl::update(InputArray _depth, InputArray _img) { CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); Mat depth; - if (_depth.isUMat()) + Mat img; + if(_depth.isUMat()) { _depth.copyTo(depth); - return updateT(depth); + _img.copyTo(img); + return updateT(depth, img); } else { - return updateT(_depth.getMat()); + return updateT(_depth.getMat(), _img.getMat()); } } + template<> -bool LargeKinfuImpl::update(InputArray _depth) +bool LargeKinfuImpl::update(InputArray _depth, InputArray _img) { CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); UMat depth; - if (!_depth.isUMat()) + Mat img; + + if(!_depth.isUMat()) { _depth.copyTo(depth); - return updateT(depth); } else - { - return updateT(_depth.getUMat()); - } + depth = _depth.getUMat(); + + if(!_img.isUMat()) + _img.copyTo(img); + else + img = _img.getMat(); + + return updateT(depth, img); } template -bool LargeKinfuImpl::updateT(const MatType& _depth) +bool LargeKinfuImpl::updateT(const MatType& depth, const Mat& img) { CV_TRACE_FUNCTION(); @@ -226,6 +244,15 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) else depth = _depth; + Mat grayImg; + if(!_img.empty()) + { + if(_img.channels() == 3) + cvtColor(_img, grayImg, COLOR_BGR2GRAY); + else + grayImg = _img; + } + cv::Ptr newFrame = icp->makeOdometryFrame(noArray(), depth, noArray()); icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC); @@ -283,6 +310,30 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) //4. Update map bool isMapUpdated = submapMgr->updateMap(frameCounter, newFrame); + //5. Loop Closure Detection + // Before Optimize the PoseGraph, run loop closure first. + if(!grayImg.empty() && lcd) + { + int currentSubmapId = submapMgr->getCurrentSubmap()->id; + + if(currentSubmapId != -1) + { + int tarSubmapID = -1; + + bool ifLoop = lcd->addFrame(grayImg, frameCounter, currentSubmapId, tarSubmapID); + + if(ifLoop && tarSubmapID != -1 && currentSubmapId != tarSubmapID) + { + // Adding Loop Edge for optimize. If the Edge is duplicate, then skip. + if(submapMgr->addEdgeToCurrentSubmap(currentSubmapId, tarSubmapID)) + std::cout<<"LCD: Find a NEW LOOP! from Submap :"<::getNormals(InputArray points, OutputArray normals) currSubmap->volume->fetchNormals(points, normals); } +template +void LargeKinfuImpl::setModelForLCD(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId, int targetId) +{ + CV_Assert(!modelBin.empty()); + + lcd = makePtr(modelBin, modelTxt, input_size, backendId, targetId); +} + // importing class #ifdef OPENCV_ENABLE_NONFREE diff --git a/modules/rgbd/src/loop_closure_detection.cpp b/modules/rgbd/src/loop_closure_detection.cpp new file mode 100644 index 00000000000..259b099f542 --- /dev/null +++ b/modules/rgbd/src/loop_closure_detection.cpp @@ -0,0 +1,234 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "precomp.hpp" +#include "loop_closure_detection.hpp" + +namespace cv{ +namespace large_kinfu{ + +LoopClosureDetectionImpl::LoopClosureDetectionImpl(const String& _modelBin, const String& _modelTxt, const Size& _inputSize, int _backendId, int _targetId) +{ + inputSize = _inputSize; + CV_Assert(!_modelBin.empty()); + if(_modelTxt.empty()) + { + net = makePtr(dnn::readNet(_modelBin)); + } else{ + net = makePtr(dnn::readNet(_modelBin, _modelTxt)); + } + + //Only HF-Net with OpenVINO backend was supported. + // Pre-trained model can be found at https://1drv.ms/u/s!ApQBoiZSe8Evgolqw23hI8D7lP9mKw?e=ywHAc5. + //! TODO: HF-Net with OpenCV DNN backend. + net->setPreferableBackend(_backendId); + net->setPreferableTarget(_targetId); + outNameDNN = net->getUnconnectedOutLayersNames(); + + KFDataBase = makePtr(maxDatabaseSize); +} + +bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID) +{ + //Calculate the similarity with all pictures in the database. + + // If the KFDataBase is too small, then skip. + if(KFDataBase->getSize() < minDatabaseSize ) + return false; + + double maxScore = 0; + int bestId = -1; + + std::vector candidateKFs; + + // Find candidate key frames which similarity are greater than the similarityLow. + candidateKFs = KFDataBase->getCandidateKF(currentDNNFeature, currentSubmapID, similarityLow, maxScore, bestId); + + CV_LOG_INFO(NULL, "LCD: Best Frame ID = " << bestId<<", similarity = "< duplicateKFs; + std::vector::iterator iter = candidateKFs.begin(); + std::vector::iterator iterTemp; + while (iter != candidateKFs.end() ) + { + Ptr keyFrameDB = KFDataBase->getKeyFrameByID(*iter); + + if(keyFrameDB && keyFrameDB->nextKeyFrameID != -1) + { + iterTemp = find(candidateKFs.begin(), candidateKFs.end(), keyFrameDB->nextKeyFrameID); + if( iterTemp != candidateKFs.end() || keyFrameDB->submapID == currentSubmapID ) + { + duplicateKFs.push_back(*iterTemp); + } + } + iter++; + } + + // Delete duplicated KFs. + for(int deleteID : duplicateKFs) + { + iterTemp = find(candidateKFs.begin(), candidateKFs.end(), deleteID); + if(iterTemp != candidateKFs.end()) + { + candidateKFs.erase(iterTemp); + } + } + + // 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 keyFrameDB = KFDataBase->getKeyFrameByID(*iter); + if(tempSubmapID == -1) + { + tempSubmapID = keyFrameDB->submapID; + }else + { + if(tempSubmapID != keyFrameDB->submapID) + return false; + } + iter++; + } + // Check whether currentFrame is closed to previous looped Keyframe. + if(currentFrameID - preLoopedKFID < 20) + return false; + + if(!candidateKFs.empty()) + bestLoopFrame = KFDataBase->getKeyFrameByID(candidateKFs[0]); + else + return false; + + // find target submap ID + if(bestLoopFrame->submapID == -1 || bestLoopFrame->submapID == currentSubmapID) + { + return false; + } + else + { + tarSubmapID = bestLoopFrame->submapID; + preLoopedKFID = currentFrameID; + currentFrameID = -1; + +#ifdef HAVE_OPENCV_FEATURES2D + // ORB Feature Matcher. + return ORBMather(bestLoopFrame->ORBFeatures, currentORBFeature); +#else + return true; +#endif + + } +} + +bool LoopClosureDetectionImpl::ORBMather(InputArray feature1, InputArray feature2) +{ +#ifdef HAVE_OPENCV_FEATURES2D + std::vector matches; + ORBmatcher->match(feature1,feature2, matches); + auto min_max = minmax_element(matches.begin(), matches.end(), + [](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; }); + double minDist = min_max.first->distance; + + std::vector goodMatches; + for (auto &match: matches) + { + if (match.distance <= std::max(2 * minDist, 30.0)) + { + goodMatches.push_back(match); + } + } + if(goodMatches.size() < ORBminMathing) + { + CV_LOG_INFO(NULL, "LCD: There are too few ORB matching pairs."); + return false; + } + else + { + return true; + } + +#else + return true; +#endif +} + +bool LoopClosureDetectionImpl::addFrame(InputArray _img, const int frameID, const int submapID, int& tarSubmapID) +{ + + CV_Assert(!_img.empty()); + currentFrameID = frameID; + currentSubmapID = submapID; + + Mat img; + if (_img.isUMat()) + { + _img.copyTo(img); + } + else + { + img = _img.getMat(); + } + + // feature Extract. + processFrame(img, currentDNNFeature, currentKeypoints, currentORBFeature); + + + // Key frame filtering. + bool ifLoop = loopCheck(tarSubmapID); + + // add Frame to KeyFrameDataset. + if(!ifLoop) + { +#ifdef HAVE_OPENCV_FEATURES2D + KFDataBase->addKeyFrame(currentDNNFeature, frameID, submapID, currentKeypoints, currentORBFeature); +#else + KFDataBase->addKeyFrame(currentDNNFeature, frameID, submapID); +#endif + } + return ifLoop; +} + +void LoopClosureDetectionImpl::reset() +{ + KFDataBase->reset(); +} + +void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& DNNfeature, std::vector& currentKeypoints, Mat& ORBFeature) +{ + std::vector outMats; + + // DNN processing. + Mat imgBlur, outDNN, outORB; + imgBlur = img.getMat(); + Mat blob = dnn::blobFromImage(imgBlur, 1.0, inputSize); + + net->setInput(blob); + net->forward(outMats, outNameDNN); + + outMats[0] /= norm(outMats[0]); + DNNfeature = outMats[0].clone(); + + // ORB process +#ifdef HAVE_OPENCV_FEATURES2D + ORBdetector->detect(img, currentKeypoints); + ORBdescriptor->compute(img,currentKeypoints,outORB); + ORBFeature = outORB.clone(); +#endif + +} + +Ptr LoopClosureDetection::create(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId, int targetId) +{ + CV_Assert(!modelBin.empty()); + return makePtr(modelBin, modelTxt, input_size, backendId, targetId); +} + +} // namespace large_kinfu +}// namespace cv diff --git a/modules/rgbd/src/loop_closure_detection.hpp b/modules/rgbd/src/loop_closure_detection.hpp new file mode 100644 index 00000000000..450b903c392 --- /dev/null +++ b/modules/rgbd/src/loop_closure_detection.hpp @@ -0,0 +1,71 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#ifndef __OPENCV_LOOP_CLOSURE_DETECTION_H__ +#define __OPENCV_LOOP_CLOSURE_DETECTION_H__ + +#include "opencv2/dnn.hpp" +#include "keyframe.hpp" + +#ifdef HAVE_OPENCV_FEATURES2D +#include +#endif + +namespace cv{ +namespace large_kinfu{ + +class LoopClosureDetectionImpl : public LoopClosureDetection +{ +public: + LoopClosureDetectionImpl(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId = 0, int targetId = 0); + + bool addFrame(InputArray img, const int frameID, const int submapID, int& tarSubmapID) CV_OVERRIDE; + + bool loopCheck(int& tarSubmapID); + + void reset() CV_OVERRIDE; + + void processFrame(InputArray img, Mat& DNNfeature,std::vector& currentKeypoints, Mat& ORBFeature); + + bool ORBMather(InputArray feature1, InputArray feature2); + + bool newFrameCheck(); + + void ORBExtract(); + +private: + Ptr KFDataBase; + Ptr net; + Size inputSize; + int currentFrameID; + std::vector currentKeypoints; + Mat currentDNNFeature; + Mat currentORBFeature; + Ptr bestLoopFrame; + + int currentSubmapID = -1; + +#ifdef HAVE_OPENCV_FEATURES2D + Ptr ORBdetector = ORB::create(); + Ptr ORBdescriptor = ORB::create(); + Ptr ORBmatcher = DescriptorMatcher::create("BruteForce-Hamming"); +#endif + size_t ORBminMathing = 10; + + int minDatabaseSize = 50; + int maxDatabaseSize = 2000; + + int preLoopedKFID = -1; + + // Param: HF-Net + // Github Link: https://github.com/ethz-asl/hfnet + std::vector outNameDNN; + double similarityHigh = 0.80; + double similarityLow = 0.84; + +}; + +} +} +#endif From 177ed7c292b4f04cc3208dd9e9a2fbb28a87c31d Mon Sep 17 00:00:00 2001 From: Zihao Mu Date: Thu, 26 Aug 2021 15:40:43 +0800 Subject: [PATCH 2/5] warnning fixed --- modules/rgbd/src/keyframe.cpp | 4 ++-- modules/rgbd/src/loop_closure_detection.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/keyframe.cpp b/modules/rgbd/src/keyframe.cpp index 430809a3a3f..00eac2d9dc5 100644 --- a/modules/rgbd/src/keyframe.cpp +++ b/modules/rgbd/src/keyframe.cpp @@ -121,7 +121,7 @@ Ptr KeyFrameDatabase::getKeyFrameByID(int keyFrameID) } } -int KeyFrameDatabase::getSize() +size_t KeyFrameDatabase::getSize() { return DataBase.size(); } @@ -150,7 +150,7 @@ double KeyFrameDatabase::score(InputArray feature1, InputArray feature2) std::vector KeyFrameDatabase::getCandidateKF(const Mat& currentFeature, const int currentSubmapID, const double& similarityLow, double& bestSimilarity, int& bestId ) { std::vector cadidateKFs; - float similarity; + double similarity; bestSimilarity = 0; diff --git a/modules/rgbd/src/loop_closure_detection.cpp b/modules/rgbd/src/loop_closure_detection.cpp index 259b099f542..ee9f16875f0 100644 --- a/modules/rgbd/src/loop_closure_detection.cpp +++ b/modules/rgbd/src/loop_closure_detection.cpp @@ -34,7 +34,7 @@ bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID) //Calculate the similarity with all pictures in the database. // If the KFDataBase is too small, then skip. - if(KFDataBase->getSize() < minDatabaseSize ) + if(int(KFDataBase->getSize()) < minDatabaseSize ) return false; double maxScore = 0; @@ -200,7 +200,7 @@ void LoopClosureDetectionImpl::reset() KFDataBase->reset(); } -void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& DNNfeature, std::vector& currentKeypoints, Mat& ORBFeature) +void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& DNNfeature, std::vector& Keypoints, Mat& ORBFeature) { std::vector outMats; @@ -217,8 +217,8 @@ void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& DNNfeature, std // ORB process #ifdef HAVE_OPENCV_FEATURES2D - ORBdetector->detect(img, currentKeypoints); - ORBdescriptor->compute(img,currentKeypoints,outORB); + ORBdetector->detect(img, Keypoints); + ORBdescriptor->compute(img, Keypoints, outORB); ORBFeature = outORB.clone(); #endif From b61fe4ee46a2faf30f3454dcc23a48d57fb612d6 Mon Sep 17 00:00:00 2001 From: Zihao Mu Date: Thu, 26 Aug 2021 16:25:24 +0800 Subject: [PATCH 3/5] warning fixed --- modules/rgbd/src/keyframe.hpp | 2 +- modules/rgbd/src/large_kinfu.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/keyframe.hpp b/modules/rgbd/src/keyframe.hpp index 6c4201841f1..c62e833ec52 100644 --- a/modules/rgbd/src/keyframe.hpp +++ b/modules/rgbd/src/keyframe.hpp @@ -51,7 +51,7 @@ class KeyFrameDatabase bool deleteKeyFrameByID(int keyFrameID); - int getSize(); + size_t getSize(); bool empty(); diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 982a34e42b9..40a98f1f0ce 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -6,6 +6,7 @@ // module's directory #include "precomp.hpp" +#include "loop_closure_detection.hpp" namespace cv { @@ -234,7 +235,7 @@ bool LargeKinfuImpl::update(InputArray _depth, InputArray _img) template -bool LargeKinfuImpl::updateT(const MatType& depth, const Mat& img) +bool LargeKinfuImpl::updateT(const MatType& _depth, const Mat& _img) { CV_TRACE_FUNCTION(); From 07d4465e7b172bbd9b8e2ef25def36311c598a4d Mon Sep 17 00:00:00 2001 From: Zihao Mu Date: Mon, 30 Aug 2021 09:37:25 +0800 Subject: [PATCH 4/5] warning fixed. --- modules/rgbd/samples/large_kinfu_LCD_demo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/samples/large_kinfu_LCD_demo.cpp b/modules/rgbd/samples/large_kinfu_LCD_demo.cpp index f80cbde78a1..df319318ed3 100644 --- a/modules/rgbd/samples/large_kinfu_LCD_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_LCD_demo.cpp @@ -181,7 +181,7 @@ int main(int argc, char** argv) const auto& volParams = largeKinfu->getParams().volumeParams; if (!modelBin.empty()) - LargeKinfu->setModelForLCD(modelBin, modelTxt, inputSize, backend, target); + largeKinfu->setModelForLCD(modelBin, modelTxt, inputSize, backend, target); #ifdef HAVE_OPENCV_VIZ cv::viz::Viz3d window(vizWindowName); From 15c4834962b4f20478f401401053d6c68e76ad69 Mon Sep 17 00:00:00 2001 From: Zihao Mu Date: Mon, 30 Aug 2021 10:59:16 +0800 Subject: [PATCH 5/5] warning fixed --- modules/rgbd/samples/large_kinfu_LCD_demo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/samples/large_kinfu_LCD_demo.cpp b/modules/rgbd/samples/large_kinfu_LCD_demo.cpp index df319318ed3..93271b78333 100644 --- a/modules/rgbd/samples/large_kinfu_LCD_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_LCD_demo.cpp @@ -89,7 +89,7 @@ int main(int argc, char** argv) bool coarse = false; bool idle = false; std::string recordPath, modelBin, modelTxt; - int backend = 0, target = 0, width, height; + int backend = 0, target = 0, width = 640, height = 480; CommandLineParser parser(argc, argv, keys); parser.about(message);