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 module's directory
6
7 #include <iostream>
8 #include <fstream>
9 #include <opencv2/imgproc.hpp>
10 #include <opencv2/calib3d.hpp>
11 #include <opencv2/highgui.hpp>
12 #include <opencv2/rgbd/colored_kinfu.hpp>
13
14 #include "io_utils.hpp"
15
16 using namespace cv;
17 using namespace cv::kinfu;
18 using namespace cv::colored_kinfu;
19 using namespace cv::io_utils;
20
21 #ifdef HAVE_OPENCV_VIZ
22 #include <opencv2/viz.hpp>
23 #endif
24
25 #ifdef HAVE_OPENCV_VIZ
26 const std::string vizWindowName = "cloud";
27
28 struct PauseCallbackArgs
29 {
PauseCallbackArgsPauseCallbackArgs30 PauseCallbackArgs(ColoredKinFu& _kf) : kf(_kf)
31 { }
32
33 ColoredKinFu& kf;
34 };
35
36 void pauseCallback(const viz::MouseEvent& me, void* args);
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.kf.render(rendered, window.getViewerPose().matrix);
47 imshow("render", rendered);
48 waitKey(1);
49 }
50 }
51 #endif
52
53 static const char* keys =
54 {
55 "{help h usage ? | | print this message }"
56 "{depth | | Path to folder with depth.txt and rgb.txt files listing a set of depth and rgb images }"
57 "{camera |0| Index of depth camera to be used as a depth source }"
58 "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
59 " in coarse mode points and normals are displayed }"
60 "{idle | | Do not run KinFu, just display depth frames }"
61 "{record | | Write depth frames to specified file list"
62 " (the same format as for the 'depth' key) }"
63 };
64
65 static const std::string message =
66 "\nThis demo uses live depth input or RGB-D dataset taken from"
67 "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
68 "\nto demonstrate KinectFusion implementation \n";
69
70
main(int argc,char ** argv)71 int main(int argc, char **argv)
72 {
73 bool coarse = false;
74 bool idle = false;
75 std::string recordPath;
76
77 CommandLineParser parser(argc, argv, keys);
78 parser.about(message);
79
80 if(!parser.check())
81 {
82 parser.printMessage();
83 parser.printErrors();
84 return -1;
85 }
86
87 if(parser.has("help"))
88 {
89 parser.printMessage();
90 return 0;
91 }
92 if(parser.has("coarse"))
93 {
94 coarse = true;
95 }
96 if(parser.has("record"))
97 {
98 recordPath = parser.get<String>("record");
99 }
100 if(parser.has("idle"))
101 {
102 idle = true;
103 }
104
105 Ptr<DepthSource> ds;
106 Ptr<RGBSource> rgbs;
107
108 if (parser.has("depth"))
109 ds = makePtr<DepthSource>(parser.get<String>("depth") + "/depth.txt");
110 else
111 ds = makePtr<DepthSource>(parser.get<int>("camera"));
112
113 //TODO: intrinsics for camera
114 rgbs = makePtr<RGBSource>(parser.get<String>("depth") + "/rgb.txt");
115
116 if (ds->empty())
117 {
118 std::cerr << "Failed to open depth source" << std::endl;
119 parser.printMessage();
120 return -1;
121 }
122
123 Ptr<DepthWriter> depthWriter;
124 Ptr<RGBWriter> rgbWriter;
125
126 if (!recordPath.empty())
127 {
128 depthWriter = makePtr<DepthWriter>(recordPath);
129 rgbWriter = makePtr<RGBWriter>(recordPath);
130 }
131 Ptr<colored_kinfu::Params> params;
132 Ptr<ColoredKinFu> kf;
133
134 params = colored_kinfu::Params::coloredTSDFParams(coarse);
135
136 // These params can be different for each depth sensor
137 ds->updateParams(*params);
138
139 rgbs->updateParams(*params);
140
141 // Enables OpenCL explicitly (by default can be switched-off)
142 cv::setUseOptimized(false);
143
144 // Scene-specific params should be tuned for each scene individually
145 //float cubeSize = 1.f;
146 //params->voxelSize = cubeSize/params->volumeDims[0]; //meters
147 //params->tsdf_trunc_dist = 0.01f; //meters
148 //params->icpDistThresh = 0.01f; //meters
149 //params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters
150 //params->tsdf_max_weight = 16;
151
152 if(!idle)
153 kf = ColoredKinFu::create(params);
154
155 #ifdef HAVE_OPENCV_VIZ
156 cv::viz::Viz3d window(vizWindowName);
157 window.setViewerPose(Affine3f::Identity());
158 bool pause = false;
159 #endif
160
161 UMat rendered;
162 UMat points;
163 UMat normals;
164
165 int64 prevTime = getTickCount();
166
167 for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
168 {
169 if(depthWriter)
170 depthWriter->append(frame);
171 UMat rgb_frame = rgbs->getRGB();
172 #ifdef HAVE_OPENCV_VIZ
173 if(pause)
174 {
175 // doesn't happen in idle mode
176 kf->getCloud(points, normals);
177 if(!points.empty() && !normals.empty())
178 {
179 viz::WCloud cloudWidget(points, viz::Color::white());
180 viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
181 window.showWidget("cloud", cloudWidget);
182 window.showWidget("normals", cloudNormals);
183
184 Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims);
185 window.showWidget("cube", viz::WCube(Vec3d::all(0),
186 volSize),
187 kf->getParams().volumePose);
188 PauseCallbackArgs pca(*kf);
189 window.registerMouseCallback(pauseCallback, (void*)&pca);
190 window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
191 "Close the window or press Q to resume"), Point()));
192 window.spin();
193 window.removeWidget("text");
194 window.removeWidget("cloud");
195 window.removeWidget("normals");
196 window.registerMouseCallback(0);
197 }
198
199 pause = false;
200 }
201 else
202 #endif
203 {
204 UMat cvt8;
205 float depthFactor = params->depthFactor;
206 convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
207 if(!idle)
208 {
209 imshow("depth", cvt8);
210 imshow("rgb", rgb_frame);
211 if(!kf->update(frame, rgb_frame))
212 {
213 kf->reset();
214 }
215 #ifdef HAVE_OPENCV_VIZ
216 else
217 {
218 if(coarse)
219 {
220 kf->getCloud(points, normals);
221 if(!points.empty() && !normals.empty())
222 {
223 viz::WCloud cloudWidget(points, viz::Color::white());
224 viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
225 window.showWidget("cloud", cloudWidget);
226 window.showWidget("normals", cloudNormals);
227 }
228 }
229
230 //window.showWidget("worldAxes", viz::WCoordinateSystem());
231 Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims;
232 window.showWidget("cube", viz::WCube(Vec3d::all(0),
233 volSize),
234 kf->getParams().volumePose);
235 window.setViewerPose(kf->getPose());
236 window.spinOnce(1, true);
237 }
238 #endif
239
240 kf->render(rendered);
241 }
242 else
243 {
244 rendered = cvt8;
245 }
246 }
247
248 int64 newTime = getTickCount();
249 putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
250 (int)(getTickFrequency()/(newTime - prevTime))),
251 Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
252 prevTime = newTime;
253
254 imshow("render", rendered);
255
256 int c = waitKey(1);
257 switch (c)
258 {
259 case 'r':
260 if(!idle)
261 kf->reset();
262 break;
263 case 'q':
264 return 0;
265 #ifdef HAVE_OPENCV_VIZ
266 case 'p':
267 if(!idle)
268 pause = true;
269 #endif
270 default:
271 break;
272 }
273 }
274
275 return 0;
276 }
277