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 "precomp.hpp"
8 #include "fast_icp.hpp"
9 #include "tsdf.hpp"
10 #include "hash_tsdf.hpp"
11 #include "kinfu_frame.hpp"
12
13 namespace cv {
14 namespace kinfu {
15
setInitialVolumePose(Matx33f R,Vec3f t)16 void Params::setInitialVolumePose(Matx33f R, Vec3f t)
17 {
18 setInitialVolumePose(Affine3f(R,t).matrix);
19 }
20
setInitialVolumePose(Matx44f homogen_tf)21 void Params::setInitialVolumePose(Matx44f homogen_tf)
22 {
23 Params::volumePose.matrix = homogen_tf;
24 }
25
defaultParams()26 Ptr<Params> Params::defaultParams()
27 {
28 Params p;
29
30 p.frameSize = Size(640, 480);
31
32 p.volumeType = VolumeType::TSDF;
33
34 float fx, fy, cx, cy;
35 fx = fy = 525.f;
36 cx = p.frameSize.width/2 - 0.5f;
37 cy = p.frameSize.height/2 - 0.5f;
38 p.intr = Matx33f(fx, 0, cx,
39 0, fy, cy,
40 0, 0, 1);
41
42 // 5000 for the 16-bit PNG files
43 // 1 for the 32-bit float images in the ROS bag files
44 p.depthFactor = 5000;
45
46 // sigma_depth is scaled by depthFactor when calling bilateral filter
47 p.bilateral_sigma_depth = 0.04f; //meter
48 p.bilateral_sigma_spatial = 4.5; //pixels
49 p.bilateral_kernel_size = 7; //pixels
50
51 p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians
52 p.icpDistThresh = 0.1f; // meters
53
54 p.icpIterations = {10, 5, 4};
55 p.pyramidLevels = (int)p.icpIterations.size();
56
57 p.tsdf_min_camera_movement = 0.f; //meters, disabled
58
59 p.volumeDims = Vec3i::all(512); //number of voxels
60
61 float volSize = 3.f;
62 p.voxelSize = volSize/512.f; //meters
63
64 // default pose of volume cube
65 p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f));
66 p.tsdf_trunc_dist = 7 * p.voxelSize; // about 0.04f in meters
67 p.tsdf_max_weight = 64; //frames
68
69 p.raycast_step_factor = 0.25f; //in voxel sizes
70 // gradient delta factor is fixed at 1.0f and is not used
71 //p.gradient_delta_factor = 0.5f; //in voxel sizes
72
73 //p.lightPose = p.volume_pose.translation()/4; //meters
74 p.lightPose = Vec3f::all(0.f); //meters
75
76 // depth truncation is not used by default but can be useful in some scenes
77 p.truncateThreshold = 0.f; //meters
78
79 return makePtr<Params>(p);
80 }
81
coarseParams()82 Ptr<Params> Params::coarseParams()
83 {
84 Ptr<Params> p = defaultParams();
85
86 p->icpIterations = {5, 3, 2};
87 p->pyramidLevels = (int)p->icpIterations.size();
88
89 float volSize = 3.f;
90 p->volumeDims = Vec3i::all(128); //number of voxels
91 p->voxelSize = volSize/128.f;
92 p->tsdf_trunc_dist = 2 * p->voxelSize; // 0.04f in meters
93
94 p->raycast_step_factor = 0.75f; //in voxel sizes
95
96 return p;
97 }
hashTSDFParams(bool isCoarse)98 Ptr<Params> Params::hashTSDFParams(bool isCoarse)
99 {
100 Ptr<Params> p;
101 if(isCoarse)
102 p = coarseParams();
103 else
104 p = defaultParams();
105 p->volumeType = VolumeType::HASHTSDF;
106 p->truncateThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH();
107 return p;
108 }
109
coloredTSDFParams(bool isCoarse)110 Ptr<Params> Params::coloredTSDFParams(bool isCoarse)
111 {
112 Ptr<Params> p;
113 if (isCoarse)
114 p = coarseParams();
115 else
116 p = defaultParams();
117 p->volumeType = VolumeType::COLOREDTSDF;
118
119 return p;
120 }
121
122 // MatType should be Mat or UMat
123 template< typename MatType>
124 class KinFuImpl : public KinFu
125 {
126 public:
127 KinFuImpl(const Params& _params);
128 virtual ~KinFuImpl();
129
130 const Params& getParams() const CV_OVERRIDE;
131
132 void render(OutputArray image) const CV_OVERRIDE;
133 void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE;
134
135 virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE;
136 void getPoints(OutputArray points) const CV_OVERRIDE;
137 void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE;
138
139 void reset() CV_OVERRIDE;
140
141 const Affine3f getPose() const CV_OVERRIDE;
142
143 bool update(InputArray depth) CV_OVERRIDE;
144
145 bool updateT(const MatType& depth);
146
147 private:
148 Params params;
149
150 cv::Ptr<ICP> icp;
151 cv::Ptr<Volume> volume;
152
153 int frameCounter;
154 Matx44f pose;
155 std::vector<MatType> pyrPoints;
156 std::vector<MatType> pyrNormals;
157 };
158
159
160 template< typename MatType >
KinFuImpl(const Params & _params)161 KinFuImpl<MatType>::KinFuImpl(const Params &_params) :
162 params(_params),
163 icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)),
164 pyrPoints(), pyrNormals()
165 {
166 volume = makeVolume(params.volumeType, params.voxelSize, params.volumePose.matrix, params.raycast_step_factor,
167 params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold, params.volumeDims);
168 reset();
169 }
170
171 template< typename MatType >
reset()172 void KinFuImpl<MatType >::reset()
173 {
174 frameCounter = 0;
175 pose = Affine3f::Identity().matrix;
176 volume->reset();
177 }
178
179 template< typename MatType >
~KinFuImpl()180 KinFuImpl<MatType>::~KinFuImpl()
181 { }
182
183 template< typename MatType >
getParams() const184 const Params& KinFuImpl<MatType>::getParams() const
185 {
186 return params;
187 }
188
189 template< typename MatType >
getPose() const190 const Affine3f KinFuImpl<MatType>::getPose() const
191 {
192 return pose;
193 }
194
195
196 template<>
update(InputArray _depth)197 bool KinFuImpl<Mat>::update(InputArray _depth)
198 {
199 CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
200
201 Mat depth;
202 if(_depth.isUMat())
203 {
204 _depth.copyTo(depth);
205 return updateT(depth);
206 }
207 else
208 {
209 return updateT(_depth.getMat());
210 }
211 }
212
213
214 template<>
update(InputArray _depth)215 bool KinFuImpl<UMat>::update(InputArray _depth)
216 {
217 CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
218
219 UMat depth;
220 if(!_depth.isUMat())
221 {
222 _depth.copyTo(depth);
223 return updateT(depth);
224 }
225 else
226 {
227 return updateT(_depth.getUMat());
228 }
229 }
230
231
232 template< typename MatType >
updateT(const MatType & _depth)233 bool KinFuImpl<MatType>::updateT(const MatType& _depth)
234 {
235 CV_TRACE_FUNCTION();
236
237 MatType depth;
238 if(_depth.type() != DEPTH_TYPE)
239 _depth.convertTo(depth, DEPTH_TYPE);
240 else
241 depth = _depth;
242
243
244 std::vector<MatType> newPoints, newNormals;
245 makeFrameFromDepth(depth, newPoints, newNormals, params.intr,
246 params.pyramidLevels,
247 params.depthFactor,
248 params.bilateral_sigma_depth,
249 params.bilateral_sigma_spatial,
250 params.bilateral_kernel_size,
251 params.truncateThreshold);
252 if(frameCounter == 0)
253 {
254 // use depth instead of distance
255 volume->integrate(depth, params.depthFactor, pose, params.intr);
256 pyrPoints = newPoints;
257 pyrNormals = newNormals;
258 }
259 else
260 {
261 Affine3f affine;
262 bool success = icp->estimateTransform(affine, pyrPoints, pyrNormals, newPoints, newNormals);
263 if(!success)
264 return false;
265
266 pose = (Affine3f(pose) * affine).matrix;
267
268 float rnorm = (float)cv::norm(affine.rvec());
269 float tnorm = (float)cv::norm(affine.translation());
270 // We do not integrate volume if camera does not move
271 if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement)
272 {
273 // use depth instead of distance
274 volume->integrate(depth, params.depthFactor, pose, params.intr);
275 }
276 MatType& points = pyrPoints [0];
277 MatType& normals = pyrNormals[0];
278 volume->raycast(pose, params.intr, params.frameSize, points, normals);
279 buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals,
280 params.pyramidLevels);
281 }
282
283 frameCounter++;
284 return true;
285 }
286
287
288 template< typename MatType >
render(OutputArray image) const289 void KinFuImpl<MatType>::render(OutputArray image) const
290 {
291 CV_TRACE_FUNCTION();
292
293 renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose);
294 }
295
296
297 template< typename MatType >
render(OutputArray image,const Matx44f & _cameraPose) const298 void KinFuImpl<MatType>::render(OutputArray image, const Matx44f& _cameraPose) const
299 {
300 CV_TRACE_FUNCTION();
301
302 Affine3f cameraPose(_cameraPose);
303 MatType points, normals;
304 volume->raycast(_cameraPose, params.intr, params.frameSize, points, normals);
305 renderPointsNormals(points, normals, image, params.lightPose);
306 }
307
308
309 template< typename MatType >
getCloud(OutputArray p,OutputArray n) const310 void KinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
311 {
312 volume->fetchPointsNormals(p, n);
313 }
314
315
316 template< typename MatType >
getPoints(OutputArray points) const317 void KinFuImpl<MatType>::getPoints(OutputArray points) const
318 {
319 volume->fetchPointsNormals(points, noArray());
320 }
321
322
323 template< typename MatType >
getNormals(InputArray points,OutputArray normals) const324 void KinFuImpl<MatType>::getNormals(InputArray points, OutputArray normals) const
325 {
326 volume->fetchNormals(points, normals);
327 }
328
329 // importing class
330
331 #ifdef OPENCV_ENABLE_NONFREE
332
create(const Ptr<Params> & params)333 Ptr<KinFu> KinFu::create(const Ptr<Params>& params)
334 {
335 CV_Assert((int)params->icpIterations.size() == params->pyramidLevels);
336 CV_Assert(params->intr(0,1) == 0 && params->intr(1,0) == 0 && params->intr(2,0) == 0 && params->intr(2,1) == 0 && params->intr(2,2) == 1);
337 #ifdef HAVE_OPENCL
338 if(cv::ocl::useOpenCL())
339 return makePtr< KinFuImpl<UMat> >(*params);
340 #endif
341 return makePtr< KinFuImpl<Mat> >(*params);
342 }
343
344 #else
create(const Ptr<Params> &)345 Ptr<KinFu> KinFu::create(const Ptr<Params>& /* params */)
346 {
347 CV_Error(Error::StsNotImplemented,
348 "This algorithm is patented and is excluded in this configuration; "
349 "Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library");
350 }
351 #endif
352
~KinFu()353 KinFu::~KinFu() {}
354
355 } // namespace kinfu
356 } // namespace cv
357