diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index aa8a6a93886..7251b196aed 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 OPTIONAL opencv_features2d opencv_viz WRAP python) \ No newline at end of file diff --git a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp index 9154ecb095e..1c9bbd5af46 100644 --- a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -138,7 +138,29 @@ 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; + + CV_WRAP virtual void setDBOW(const String& dbowPath, double simThreshold=3.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". + @see DBOWTrainer for more information. +*/ +class CV_EXPORTS_W LoopClosureDetection { +public: + // Load DBoW from the given path and set similarity threshold. + CV_WRAP static Ptr create(const String& dbowPath, double simThreshold=3.0); + + virtual ~LoopClosureDetection() = default; + + // Add frame and check if a loop exist, set tarSubmapID is so. + 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_dbow_demo.cpp b/modules/rgbd/samples/large_kinfu_dbow_demo.cpp new file mode 100644 index 00000000000..bca739fb4ab --- /dev/null +++ b/modules/rgbd/samples/large_kinfu_dbow_demo.cpp @@ -0,0 +1,293 @@ +// 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 + +#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 depth.txt file listing a set of depth 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) }" + "{rgb | | Path to rgb.txt file listing a set of RGB images }" + "{dbow | | Path to trained DBoW file }" +}; + +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"; + +int main(int argc, char** argv) +{ + bool coarse = false; + bool idle = false; + std::string recordPath, dbowPath; + + 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("dbow")) + { + dbowPath = parser.get("dbow"); + } + + Ptr ds; + Ptr rgb; + + if (parser.has("depth")) + ds = makePtr(parser.get("depth")); + else + ds = makePtr(parser.get("camera")); + + if (parser.has("rgb")) + rgb = makePtr(parser.get("rgb")); + else + rgb = makePtr(parser.get("camera")); + + if (ds->empty()) + { + std::cerr << "Failed to open depth source\n"; + parser.printMessage(); + return -1; + } + + if (rgb->empty()) + { + std::cerr << "Failed to open RGB source\n"; + parser.printMessage(); + return -1; + } + + + Ptr depthWriter; + Ptr rgbWriter; + + if (!recordPath.empty()) + { + depthWriter = makePtr(recordPath); + rgbWriter = 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 (!dbowPath.empty()) + largeKinfu->setDBOW(dbowPath); + +#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(), rgb_frame = rgb->getRGB(); !frame.empty() && !rgb_frame.empty(); frame = ds->getDepth(), rgb_frame = rgb->getRGB()) + { + if (depthWriter) + depthWriter->append(frame); + + Vec3i volResolution(volParams.resolutionX, + volParams.resolutionY, + volParams.resolutionZ); + Affine3f volPose(Matx44f(volParams.pose)); + +#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); + imshow("RGB", rgb_frame); + + if (!largeKinfu->update(frame, rgb_frame)) + { + largeKinfu->reset(); + std::cout << "reset\n"; + } +#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/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index a3ae9adbcd3..a2c384f25fc 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 { @@ -123,9 +124,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); + bool updateT(const MatType& depth, const Mat& img); + + void setDBOW(const String& dbowPath, double simThreshold) CV_OVERRIDE; private: Params params; @@ -136,6 +139,8 @@ class LargeKinfuImpl : public LargeKinfu int frameCounter; Affine3f pose; + + Ptr lcd; }; template @@ -161,6 +166,8 @@ void LargeKinfuImpl::reset() frameCounter = 0; pose = Affine3f::Identity(); submapMgr->reset(); + + if (lcd) lcd->reset(); } template @@ -181,42 +188,46 @@ 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 depth, 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; + 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 +237,15 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) else depth = _depth; + Mat gray; + if (!_img.empty()) + { + if (_img.channels() == 3) + cvtColor(_img, gray, COLOR_BGR2GRAY); + else if (_img.channels() == 1) + gray = _img; + } + cv::Ptr newFrame = icp->makeOdometryFrame(noArray(), depth, noArray()); icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC); @@ -283,6 +303,28 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) //4. Update map bool isMapUpdated = submapMgr->updateMap(frameCounter, newFrame); + //5. Loop Closure Detection + if (isMapUpdated && !gray.empty() && lcd) + { + int currentSubmapID = submapMgr->getCurrentSubmap()->id; + if (currentSubmapID != -1) + { + int tarSubmapID = -1; + bool ifLoop = lcd->addFrame(gray, frameCounter, currentSubmapID, tarSubmapID); + + if (ifLoop && 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: " << currentSubmapID << " to Submap: " << tarSubmapID << std::endl; + } + else + { + CV_LOG_INFO(NULL, "LCD: No Loop."); + } + } + } + if(isMapUpdated) { // TODO: Convert constraints to posegraph @@ -352,6 +394,13 @@ void LargeKinfuImpl::getNormals(InputArray points, OutputArray normals) currSubmap->volume->fetchNormals(points, normals); } +template +void LargeKinfuImpl::setDBOW(const String& dbowPath, double simThreshold) +{ + CV_Assert(!dbowPath.empty()); + lcd = makePtr(dbowPath, simThreshold); +} + // 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..dd7f54edbe4 --- /dev/null +++ b/modules/rgbd/src/loop_closure_detection.cpp @@ -0,0 +1,73 @@ +// 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& _dbowPath, double _simThreshold) +{ + std::cout << "Loading DBoW from " << _dbowPath << "...\n"; + dbow.load(_dbowPath); + simThreshold = _simThreshold; + reset(); +} + +bool LoopClosureDetectionImpl::addFrame(InputArray _img, const int frameID, const int submapID, int& tarSubmapID) +{ + CV_Assert(!_img.empty()); + + Mat img; + if (_img.isUMat()) + _img.copyTo(img); + else + img = _img.getMat(); + + // Detect and transform features + detector->detectAndCompute(img, cv::Mat(), keypoints, descriptors); + dbow.transform(descriptors, bowVector); + bowVectors.push_back(bowVector); + frameIDs.push_back(frameID); + submapIDs.push_back(submapID); + + if (bowVectors.size() > 2) + { + double score, maxScore = INT_MIN; + double priorSimilarity = dbow.score(bowVector, bowVectors[(int)bowVectors.size() - 2]); + + for (int i = 0; i < (int)bowVectors.size() - 2; i++) + { + score = dbow.score(bowVector, bowVectors[i]) / priorSimilarity; + if (score > maxScore) + { + maxScore = score; + if (maxScore >= simThreshold) + tarSubmapID = submapIDs[i]; + } + } + } + + if (tarSubmapID != -1) + return true; + return false; +} + +void LoopClosureDetectionImpl::reset() +{ + bowVectors.clear(); + frameIDs.clear(); + submapIDs.clear(); +} + +Ptr LoopClosureDetection::create(const String& _dbowPath, double _simThreshold) +{ + return makePtr(_dbowPath, _simThreshold); +} + +} // 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..671552268bd --- /dev/null +++ b/modules/rgbd/src/loop_closure_detection.hpp @@ -0,0 +1,40 @@ +// 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_HPP__ +#define __OPENCV_LOOP_CLOSURE_DETECTION_HPP__ + +#include + +namespace cv +{ +namespace large_kinfu +{ + +class LoopClosureDetectionImpl : public LoopClosureDetection +{ +public: + LoopClosureDetectionImpl(const String& _dbowPath, double _simThreshold); + + bool addFrame(InputArray img, const int frameID, const int submapID, int& tarSubmapID) CV_OVERRIDE; + + void reset() CV_OVERRIDE; + +private: + int nfeatures = 20; + Ptr detector = ORB::create(nfeatures); + std::vector keypoints; + Mat descriptors; + + DBOWTrainer dbow = DBOWTrainer(10, 5); + std::vector bowVectors; + std::vector frameIDs; + std::vector submapIDs; + DBOWTrainer::BOWVector bowVector; + double simThreshold; +}; + +} +} +#endif