Skip to content

Commit a39d026

Browse files
committed
Loop Closure Detection for 3D Large Kinfu
1 parent b9ad0d4 commit a39d026

File tree

8 files changed

+1017
-15
lines changed

8 files changed

+1017
-15
lines changed

modules/rgbd/CMakeLists.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
set(the_description "RGBD algorithms")
22

3-
ocv_define_module(rgbd opencv_core opencv_3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
3+
ocv_define_module(rgbd opencv_core opencv_3d opencv_imgproc opencv_dnn OPTIONAL opencv_features2d opencv_viz WRAP python)

modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp

Lines changed: 31 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -138,7 +138,37 @@ class CV_EXPORTS_W LargeKinfu
138138

139139
virtual const Affine3f getPose() const = 0;
140140

141-
CV_WRAP virtual bool update(InputArray depth) = 0;
141+
CV_WRAP virtual bool update(InputArray depth, InputArray img = noArray()) = 0;
142+
143+
// Set parameters for the loop closure detection function.
144+
CV_WRAP virtual void setModelForLCD(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId = 0, int targetId = 0) = 0;
145+
};
146+
147+
148+
/** @brief Loop Closing Detection implementation
149+
150+
This class implements a Loop Closing Detection of 3d reconstruction algorithm for
151+
larger environments using Spatially hashed TSDF volume "Submaps".
152+
153+
It takes a sequence RGB images and processes each image by HF-Net.
154+
According to the similarity of features extracted by HF-Net, determine whether there is a LOOP.
155+
Original HF-Net was provided by: https://github.com/ethz-asl/hfnet.
156+
Pre-trained model can be found at: https://1drv.ms/u/s!ApQBoiZSe8Evgolqw23hI8D7lP9mKw?e=JKwPHe.
157+
158+
*/
159+
class CV_EXPORTS_W LoopClosureDetection {
160+
public:
161+
162+
CV_WRAP static Ptr<LoopClosureDetection> create(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId = 0, int targetId = 0);
163+
164+
virtual ~LoopClosureDetection() = default;
165+
166+
// Adding Frame.
167+
// If there is loop, function will return TURE, and the tarSubmapID will be set as the target submap ID, otherwise return False.
168+
CV_WRAP virtual bool addFrame(InputArray img, const int frameID, const int submapID, CV_OUT int& tarSubmapID) = 0;
169+
170+
// Stop run loop closing detection.
171+
CV_WRAP virtual void reset() = 0;
142172
};
143173

144174
} // namespace large_kinfu
Lines changed: 312 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,312 @@
1+
// This file is part of OpenCV project.
2+
// It is subject to the license terms in the LICENSE file found in the top-level directory
3+
// of this distribution and at http://opencv.org/license.html
4+
5+
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this
6+
// module's directory
7+
8+
#include <fstream>
9+
#include <iostream>
10+
#include <opencv2/3d.hpp>
11+
#include <opencv2/highgui.hpp>
12+
#include <opencv2/imgproc.hpp>
13+
#include <opencv2/rgbd/large_kinfu.hpp>
14+
15+
#include "io_utils.hpp"
16+
17+
using namespace cv;
18+
using namespace cv::kinfu;
19+
using namespace cv::large_kinfu;
20+
using namespace cv::io_utils;
21+
22+
#ifdef HAVE_OPENCV_VIZ
23+
#include <opencv2/viz.hpp>
24+
#endif
25+
26+
#ifdef HAVE_OPENCV_VIZ
27+
const std::string vizWindowName = "cloud";
28+
29+
struct PauseCallbackArgs
30+
{
31+
PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) {}
32+
33+
LargeKinfu& largeKinfu;
34+
};
35+
36+
void pauseCallback(const viz::MouseEvent& me, void* args);
37+
void pauseCallback(const viz::MouseEvent& me, void* args)
38+
{
39+
if (me.type == viz::MouseEvent::Type::MouseMove ||
40+
me.type == viz::MouseEvent::Type::MouseScrollDown ||
41+
me.type == viz::MouseEvent::Type::MouseScrollUp)
42+
{
43+
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
44+
viz::Viz3d window(vizWindowName);
45+
UMat rendered;
46+
pca.largeKinfu.render(rendered, window.getViewerPose().matrix);
47+
imshow("render", rendered);
48+
waitKey(1);
49+
}
50+
}
51+
#endif
52+
53+
static const char* keys = {
54+
"{help h usage ? | | print this message }"
55+
"{depth | | Path to folder with depth.txt and rgb.txt files listing a set of depth and rgb images. }"
56+
"{camera |0| Index of depth camera to be used as a depth source }"
57+
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
58+
" in coarse mode points and normals are displayed }"
59+
"{idle | | Do not run LargeKinfu, just display depth frames }"
60+
"{record | | Write depth frames to specified file list (the same format as for the 'depth' key) }"
61+
"{modelBin | | Path to a binary .bin file contains trained network which can be download at URL=https://1drv.ms/u/s!ApQBoiZSe8Evgolqw23hI8D7lP9mKw?e=ywHAc5}"
62+
"{modelTxt | | Path to a .xml file contains the model definition of trained network.}"
63+
"{width | 640 | Preprocess input image by resizing to a specific width. }"
64+
"{height | 480 | Preprocess input image by resizing to a specific height. }"
65+
"{backend | 2 | At current stage only openvino available, and other backend will be supported soon."
66+
" Choose one of computation backends: "
67+
"0: automatically (by default), "
68+
"1: Halide language (http://halide-lang.org/), "
69+
"2: Intel's Deep Learning Inference Engine (https://software.intel.com/openvino-toolkit), "
70+
"3: OpenCV implementation }"
71+
"{ target | 0 | Choose one of target computation devices: "
72+
"0: CPU target (by default), "
73+
"1: OpenCL, "
74+
"2: OpenCL fp16 (half-float precision), "
75+
"3: VPU }"
76+
};
77+
78+
static const std::string message =
79+
"\nThis demo uses live depth input or RGB-D dataset taken from"
80+
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
81+
"\nto demonstrate Submap based large environment reconstruction"
82+
"\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger "
83+
"reconstructions by default\n"
84+
"\n The used OpenVINO DNN model can be downdload at URL=https://1drv.ms/u/s!ApQBoiZSe8Evgolqw23hI8D7lP9mKw?e=ywHAc5.\n"
85+
"\n Make sure that OpenVINO DNN backend is available.\n";
86+
87+
int main(int argc, char** argv)
88+
{
89+
bool coarse = false;
90+
bool idle = false;
91+
std::string recordPath, modelBin, modelTxt;
92+
int backend = 0, target = 0, width, height;
93+
94+
CommandLineParser parser(argc, argv, keys);
95+
parser.about(message);
96+
97+
if (!parser.check())
98+
{
99+
parser.printMessage();
100+
parser.printErrors();
101+
return -1;
102+
}
103+
104+
if (parser.has("help"))
105+
{
106+
parser.printMessage();
107+
return 0;
108+
}
109+
if (parser.has("coarse"))
110+
{
111+
coarse = true;
112+
}
113+
if (parser.has("record"))
114+
{
115+
recordPath = parser.get<String>("record");
116+
}
117+
if (parser.has("idle"))
118+
{
119+
idle = true;
120+
}
121+
if (parser.has("modelBin"))
122+
{
123+
modelBin = parser.get<String>("modelBin");
124+
}
125+
if (parser.has("modelTxt"))
126+
{
127+
modelTxt = parser.get<String>("modelTxt");
128+
}
129+
if (parser.has("width"))
130+
{
131+
width = parser.get<int>("width");
132+
}
133+
if (parser.has("height"))
134+
{
135+
height = parser.get<int>("height");
136+
}
137+
if (parser.has("backend"))
138+
{
139+
backend = parser.get<int>("backend");
140+
}
141+
if (parser.has("target"))
142+
{
143+
target = parser.get<int>("target");
144+
}
145+
146+
Ptr<DepthSource> ds;
147+
Ptr<RGBSource> rgbs;
148+
149+
if (parser.has("depth"))
150+
ds = makePtr<DepthSource>(parser.get<String>("depth") + "/depth.txt");
151+
else
152+
ds = makePtr<DepthSource>(parser.get<int>("camera"));
153+
154+
//TODO: intrinsics for camera
155+
rgbs = makePtr<RGBSource>(parser.get<String>("depth") + "/rgb.txt");
156+
157+
if (ds->empty())
158+
{
159+
std::cerr << "Failed to open depth source" << std::endl;
160+
parser.printMessage();
161+
return -1;
162+
}
163+
Size inputSize(width, height);
164+
Ptr<DepthWriter> depthWriter;
165+
if (!recordPath.empty())
166+
depthWriter = makePtr<DepthWriter>(recordPath);
167+
168+
Ptr<large_kinfu::Params> params;
169+
Ptr<LargeKinfu> largeKinfu;
170+
171+
params = large_kinfu::Params::hashTSDFParams(coarse);
172+
173+
// These params can be different for each depth sensor
174+
ds->updateParams(*params);
175+
176+
cv::setUseOptimized(true);
177+
178+
if (!idle)
179+
largeKinfu = LargeKinfu::create(params);
180+
181+
const auto& volParams = largeKinfu->getParams().volumeParams;
182+
183+
if (!modelBin.empty())
184+
LargeKinfu->setModelForLCD(modelBin, modelTxt, inputSize, backend, target);
185+
186+
#ifdef HAVE_OPENCV_VIZ
187+
cv::viz::Viz3d window(vizWindowName);
188+
window.setViewerPose(Affine3f::Identity());
189+
bool pause = false;
190+
#endif
191+
192+
UMat rendered;
193+
UMat points;
194+
UMat normals;
195+
196+
int64 prevTime = getTickCount();
197+
198+
for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
199+
{
200+
if (depthWriter)
201+
depthWriter->append(frame);
202+
203+
Vec3i volResolution(volParams.resolutionX,
204+
volParams.resolutionY,
205+
volParams.resolutionZ);
206+
Affine3f volPose(Matx44f(volParams.pose));
207+
208+
UMat rgb_frame = rgbs->getRGB();
209+
#ifdef HAVE_OPENCV_VIZ
210+
if (pause)
211+
{
212+
// doesn't happen in idle mode
213+
largeKinfu->getCloud(points, normals);
214+
if (!points.empty() && !normals.empty())
215+
{
216+
viz::WCloud cloudWidget(points, viz::Color::white());
217+
viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05,
218+
viz::Color::gray());
219+
window.showWidget("cloud", cloudWidget);
220+
window.showWidget("normals", cloudNormals);
221+
222+
Vec3d volSize = volParams.voxelSize * Vec3d(volResolution);
223+
window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
224+
PauseCallbackArgs pca(*largeKinfu);
225+
window.registerMouseCallback(pauseCallback, (void*)&pca);
226+
window.showWidget("text",
227+
viz::WText(cv::String("Move camera in this window. "
228+
"Close the window or press Q to resume"),
229+
Point()));
230+
window.spin();
231+
window.removeWidget("text");
232+
window.removeWidget("cloud");
233+
window.removeWidget("normals");
234+
window.registerMouseCallback(0);
235+
}
236+
237+
pause = false;
238+
}
239+
else
240+
#endif
241+
{
242+
UMat cvt8;
243+
float depthFactor = params->depthFactor;
244+
convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor);
245+
if (!idle)
246+
{
247+
imshow("depth", cvt8);
248+
249+
if (!largeKinfu->update(frame, rgb_frame))
250+
{
251+
largeKinfu->reset();
252+
std::cout << "reset" << std::endl;
253+
}
254+
#ifdef HAVE_OPENCV_VIZ
255+
else
256+
{
257+
if (coarse)
258+
{
259+
largeKinfu->getCloud(points, normals);
260+
if (!points.empty() && !normals.empty())
261+
{
262+
viz::WCloud cloudWidget(points, viz::Color::white());
263+
viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1,
264+
/*scale*/ 0.05, viz::Color::gray());
265+
window.showWidget("cloud", cloudWidget);
266+
window.showWidget("normals", cloudNormals);
267+
}
268+
}
269+
270+
// window.showWidget("worldAxes", viz::WCoordinateSystem());
271+
Vec3d volSize = volParams.voxelSize * volResolution;
272+
window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
273+
window.setViewerPose(largeKinfu->getPose());
274+
window.spinOnce(1, true);
275+
}
276+
#endif
277+
278+
largeKinfu->render(rendered);
279+
}
280+
else
281+
{
282+
rendered = cvt8;
283+
}
284+
}
285+
286+
int64 newTime = getTickCount();
287+
putText(rendered,
288+
cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
289+
(int)(getTickFrequency() / (newTime - prevTime))),
290+
Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
291+
prevTime = newTime;
292+
imshow("render", rendered);
293+
294+
int c = waitKey(1);
295+
switch (c)
296+
{
297+
case 'r':
298+
if (!idle)
299+
largeKinfu->reset();
300+
break;
301+
case 'q': return 0;
302+
#ifdef HAVE_OPENCV_VIZ
303+
case 'p':
304+
if (!idle)
305+
pause = true;
306+
#endif
307+
default: break;
308+
}
309+
}
310+
311+
return 0;
312+
}

0 commit comments

Comments
 (0)