Point Cloud Library (PCL)  1.8.1
internal.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_KINFU_INTERNAL_HPP_
39 #define PCL_KINFU_INTERNAL_HPP_
40 
41 #include <pcl/gpu/containers/device_array.h>
42 //#include <pcl/gpu/utils/safe_call.hpp>
43 #include "safe_call.hpp"
44 
45 namespace pcl
46 {
47  namespace device
48  {
49  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50  // Types
51  typedef unsigned short ushort;
54  typedef float4 PointType;
55 
56  //TSDF fixed point divisor (if old format is enabled)
57  const int DIVISOR = 32767; // SHRT_MAX;
58 
59  //Should be multiple of 32
60  enum { VOLUME_X = 512, VOLUME_Y = 512, VOLUME_Z = 512 };
61 
62 
63  const float VOLUME_SIZE = 3.0f; // in meters
64 
65  /** \brief Camera intrinsics structure
66  */
67  struct Intr
68  {
69  float fx, fy, cx, cy;
70  Intr () {}
71  Intr (float fx_, float fy_, float cx_, float cy_) : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {}
72 
73  Intr operator()(int level_index) const
74  {
75  int div = 1 << level_index;
76  return (Intr (fx / div, fy / div, cx / div, cy / div));
77  }
78  };
79 
80  /** \brief 3x3 Matrix for device code
81  */
82  struct Mat33
83  {
84  float3 data[3];
85  };
86 
87  /** \brief Light source collection
88  */
89  struct LightSource
90  {
91  float3 pos[1];
92  int number;
93  };
94 
95  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96  // Maps
97 
98  /** \brief Perfoms bilateral filtering of disparity map
99  * \param[in] src soruce map
100  * \param[out] dst output map
101  */
102  void
103  bilateralFilter (const DepthMap& src, DepthMap& dst);
104 
105  /** \brief Computes depth pyramid
106  * \param[in] src source
107  * \param[out] dst destination
108  */
109  void
110  pyrDown (const DepthMap& src, DepthMap& dst);
111 
112  /** \brief Computes vertex map
113  * \param[in] intr depth camera intrinsics
114  * \param[in] depth depth
115  * \param[out] vmap vertex map
116  */
117  void
118  createVMap (const Intr& intr, const DepthMap& depth, MapArr& vmap);
119 
120  /** \brief Computes normal map using cross product
121  * \param[in] vmap vertex map
122  * \param[out] nmap normal map
123  */
124  void
125  createNMap (const MapArr& vmap, MapArr& nmap);
126 
127  /** \brief Computes normal map using Eigen/PCA approach
128  * \param[in] vmap vertex map
129  * \param[out] nmap normal map
130  */
131  void
132  computeNormalsEigen (const MapArr& vmap, MapArr& nmap);
133 
134  /** \brief Performs affine tranform of vertex and normal maps
135  * \param[in] vmap_src source vertex map
136  * \param[in] nmap_src source vertex map
137  * \param[in] Rmat Rotation mat
138  * \param[in] tvec translation
139  * \param[out] vmap_dst destination vertex map
140  * \param[out] nmap_dst destination vertex map
141  */
142  void
143  tranformMaps (const MapArr& vmap_src, const MapArr& nmap_src, const Mat33& Rmat, const float3& tvec, MapArr& vmap_dst, MapArr& nmap_dst);
144 
145  /** \brief Performs depth truncation
146  * \param[out] depth depth map to truncation
147  * \param[in] max_distance truncation threshold, values that are higher than the threshold are reset to zero (means not measurement)
148  */
149  void
150  truncateDepth(DepthMap& depth, float max_distance);
151 
152  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
153  // ICP
154 
155  /** \brief (now it's exra code) Computes corespondances map
156  * \param[in] vmap_g_curr current vertex map in global coo space
157  * \param[in] nmap_g_curr current normals map in global coo space
158  * \param[in] Rprev_inv inverse camera rotation at previous pose
159  * \param[in] tprev camera translation at previous pose
160  * \param[in] intr camera intrinsics
161  * \param[in] vmap_g_prev previous vertex map in global coo space
162  * \param[in] nmap_g_prev previous vertex map in global coo space
163  * \param[in] distThres distance filtering threshold
164  * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
165  * \param[out] coresp
166  */
167  void
168  findCoresp (const MapArr& vmap_g_curr, const MapArr& nmap_g_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
169  const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres, PtrStepSz<short2> coresp);
170 
171  /** \brief (now it's exra code) Computation Ax=b for ICP iteration
172  * \param[in] v_dst destination vertex map (previous frame cloud)
173  * \param[in] n_dst destination normal map (previous frame normals)
174  * \param[in] v_src source normal map (current frame cloud)
175  * \param[in] coresp Corespondances
176  * \param[out] gbuf temp buffer for GPU reduction
177  * \param[out] mbuf output GPU buffer for matrix computed
178  * \param[out] matrixA_host A
179  * \param[out] vectorB_host b
180  */
181  void
182  estimateTransform (const MapArr& v_dst, const MapArr& n_dst, const MapArr& v_src, const PtrStepSz<short2>& coresp,
183  DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
184 
185 
186  /** \brief Computation Ax=b for ICP iteration
187  * \param[in] Rcurr Rotation of current camera pose guess
188  * \param[in] tcurr translation of current camera pose guess
189  * \param[in] vmap_curr current vertex map in camera coo space
190  * \param[in] nmap_curr current vertex map in camera coo space
191  * \param[in] Rprev_inv inverse camera rotation at previous pose
192  * \param[in] tprev camera translation at previous pose
193  * \param[in] intr camera intrinsics
194  * \param[in] vmap_g_prev previous vertex map in global coo space
195  * \param[in] nmap_g_prev previous vertex map in global coo space
196  * \param[in] distThres distance filtering threshold
197  * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
198  * \param[out] gbuf temp buffer for GPU reduction
199  * \param[out] mbuf output GPU buffer for matrix computed
200  * \param[out] matrixA_host A
201  * \param[out] vectorB_host b
202  */
203  void
204  estimateCombined (const Mat33& Rcurr, const float3& tcurr, const MapArr& vmap_curr, const MapArr& nmap_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
205  const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
206  DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
207 
208 
209  void
210  estimateCombined (const Mat33& Rcurr, const float3& tcurr, const MapArr& vmap_curr, const MapArr& nmap_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
211  const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
212  DeviceArray2D<double>& gbuf, DeviceArray<double>& mbuf, double* matrixA_host, double* vectorB_host);
213 
214 
215  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
216  // TSDF volume functions
217 
218  /** \brief Perform tsdf volume initialization
219  * \param[out] array volume to be initialized
220  */
221  PCL_EXPORTS void
223 
224  //first version
225  /** \brief Performs Tsfg volume uptation (extra obsolete now)
226  * \param[in] depth_raw Kinect depth image
227  * \param[in] intr camera intrinsics
228  * \param[in] volume_size size of volume in mm
229  * \param[in] Rcurr_inv inverse rotation for current camera pose
230  * \param[in] tcurr translation for current camera pose
231  * \param[in] tranc_dist tsdf truncation distance
232  * \param[in] volume tsdf volume to be updated
233  */
234  void
235  integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
236  const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume);
237 
238  //second version
239  /** \brief Function that integrates volume if volume element contains: 2 bytes for round(tsdf*SHORT_MAX) and 2 bytes for integer weight.
240  * \param[in] depth_raw Kinect depth image
241  * \param[in] intr camera intrinsics
242  * \param[in] volume_size size of volume in mm
243  * \param[in] Rcurr_inv inverse rotation for current camera pose
244  * \param[in] tcurr translation for current camera pose
245  * \param[in] tranc_dist tsdf truncation distance
246  * \param[in] volume tsdf volume to be updated
247  * \param[out] depthRawScaled Buffer for scaled depth along ray
248  */
249  PCL_EXPORTS void
250  integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
251  const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume, DeviceArray2D<float>& depthRawScaled);
252 
253  /** \brief Initialzied color volume
254  * \param[out] color_volume color volume for initialization
255  */
256 
257  void
258  initColorVolume(PtrStep<uchar4> color_volume);
259 
260  /** \brief Performs integration in color volume
261  * \param[in] intr Depth camera intrionsics structure
262  * \param[in] tranc_dist tsdf truncation distance
263  * \param[in] R_inv Inverse camera rotation
264  * \param[in] t camera translation
265  * \param[in] vmap Raycasted vertex map
266  * \param[in] colors RGB colors for current frame
267  * \param[in] volume_size volume size in meters
268  * \param[in] color_volume color volume to be integrated
269  * \param[in] max_weight max weight for running color average. Zero means not average, one means average with prev value, etc.
270  */
271  void
272  updateColorVolume(const Intr& intr, float tranc_dist, const Mat33& R_inv, const float3& t, const MapArr& vmap,
273  const PtrStepSz<uchar3>& colors, const float3& volume_size, PtrStep<uchar4> color_volume, int max_weight = 1);
274 
275  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
276  // Raycast and view generation
277  /** \brief Generation vertex and normal maps from volume for current camera pose
278  * \param[in] intr camera intrinsices
279  * \param[in] Rcurr current rotation
280  * \param[in] tcurr current translation
281  * \param[in] tranc_dist volume truncation distance
282  * \param[in] volume_size volume size in mm
283  * \param[in] volume tsdf volume
284  * \param[out] vmap output vertex map
285  * \param[out] nmap output normals map
286  */
287  void
288  raycast (const Intr& intr, const Mat33& Rcurr, const float3& tcurr, float tranc_dist, const float3& volume_size,
289  const PtrStep<short2>& volume, MapArr& vmap, MapArr& nmap);
290 
291  /** \brief Renders 3D image of the scene
292  * \param[in] vmap vetex map
293  * \param[in] nmap normals map
294  * \param[in] light poase of light source
295  * \param[out] dst buffer where image is generated
296  */
297  void
298  generateImage (const MapArr& vmap, const MapArr& nmap, const LightSource& light, PtrStepSz<uchar3> dst);
299 
300 
301  /** \brief Renders depth image from give pose
302  * \param[in] R_inv inverse camera rotation
303  * \param[in] t camera translation
304  * \param[in] vmap vertex map
305  * \param[out] dst buffer where depth is generated
306  */
307  void
308  generateDepth (const Mat33& R_inv, const float3& t, const MapArr& vmap, DepthMap& dst);
309 
310  /** \brief Paints 3D view with color map
311  * \param[in] colors rgb color frame from OpenNI
312  * \param[out] dst output 3D view
313  * \param[in] colors_weight weight for colors
314  */
315  void
316  paint3DView(const PtrStep<uchar3>& colors, PtrStepSz<uchar3> dst, float colors_weight = 0.5f);
317 
318  /** \brief Performs resize of vertex map to next pyramid level by averaging each four points
319  * \param[in] input vertext map
320  * \param[out] output resized vertex map
321  */
322  void
323  resizeVMap (const MapArr& input, MapArr& output);
324 
325  /** \brief Performs resize of vertex map to next pyramid level by averaging each four normals
326  * \param[in] input normal map
327  * \param[out] output vertex map
328  */
329  void
330  resizeNMap (const MapArr& input, MapArr& output);
331 
332  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
333  // Cloud extraction
334 
335  /** \brief Perform point cloud extraction from tsdf volume
336  * \param[in] volume tsdf volume
337  * \param[in] volume_size size of the volume
338  * \param[out] output buffer large enought to store point cloud
339  * \return number of point stored to passed buffer
340  */
341  PCL_EXPORTS size_t
342  extractCloud (const PtrStep<short2>& volume, const float3& volume_size, PtrSz<PointType> output);
343 
344  /** \brief Performs normals computation for given poins using tsdf volume
345  * \param[in] volume tsdf volume
346  * \param[in] volume_size volume size
347  * \param[in] input points where normals are computed
348  * \param[out] output normals. Could be float4 or float8. If for a point normal can't be computed, such normal is marked as nan.
349  */
350  template<typename NormalType>
351  void
352  extractNormals (const PtrStep<short2>& volume, const float3& volume_size, const PtrSz<PointType>& input, NormalType* output);
353 
354  /** \brief Performs colors exctraction from color volume
355  * \param[in] color_volume color volume
356  * \param[in] volume_size volume size
357  * \param[in] points points for which color are computed
358  * \param[out] colors output array with colors.
359  */
360  void
361  exctractColors(const PtrStep<uchar4>& color_volume, const float3& volume_size, const PtrSz<PointType>& points, uchar4* colors);
362 
363  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
364  // Utility
365  struct float8 { float x, y, z, w, c1, c2, c3, c4; };
366  struct float12 { float x, y, z, w, normal_x, normal_y, normal_z, n4, c1, c2, c3, c4; };
367 
368  /** \brief Conversion from SOA to AOS
369  * \param[in] vmap SOA map
370  * \param[out] output Array of 3D points. Can be float4 or float8.
371  */
372  template<typename T>
373  void
374  convert (const MapArr& vmap, DeviceArray2D<T>& output);
375 
376  /** \brief Merges pcl::PointXYZ and pcl::Normal to PointNormal
377  * \param[in] cloud points cloud
378  * \param[in] normals normals cloud
379  * \param[out] output array of PointNomals.
380  */
381  void
382  mergePointNormal(const DeviceArray<float4>& cloud, const DeviceArray<float8>& normals, const DeviceArray<float12>& output);
383 
384  /** \brief Check for qnan (unused now)
385  * \param[in] value
386  */
387  inline bool
388  valid_host (float value)
389  {
390  return *reinterpret_cast<int*>(&value) != 0x7fffffff; //QNAN
391  }
392 
393  /** \brief synchronizes CUDA execution */
394  inline
395  void
396  sync () { cudaSafeCall (cudaDeviceSynchronize ()); }
397 
398 
399  template<class D, class Matx> D&
400  device_cast (Matx& matx)
401  {
402  return (*reinterpret_cast<D*>(matx.data ()));
403  }
404 
405  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
406  // Marching cubes implementation
407 
408  /** \brief Binds marching cubes tables to texture references */
409  void
410  bindTextures(const int *edgeBuf, const int *triBuf, const int *numVertsBuf);
411 
412  /** \brief Unbinds */
413  void
414  unbindTextures();
415 
416  /** \brief Scans tsdf volume and retrieves occuped voxes
417  * \param[in] volume tsdf volume
418  * \param[out] occupied_voxels buffer for occuped voxels. The function fulfills first row with voxel ids and second row with number of vertextes.
419  * \return number of voxels in the buffer
420  */
421  int
422  getOccupiedVoxels(const PtrStep<short2>& volume, DeviceArray2D<int>& occupied_voxels);
423 
424  /** \brief Computes total number of vertexes for all voxels and offsets of vertexes in final triangle array
425  * \param[out] occupied_voxels buffer with occuped voxels. The function fulfills 3nd only with offsets
426  * \return total number of vertexes
427  */
428  int
430 
431  /** \brief Generates final triangle array
432  * \param[in] volume tsdf volume
433  * \param[in] occupied_voxels occuped voxel ids (first row), number of vertexes(second row), offsets(third row).
434  * \param[in] volume_size volume size in meters
435  * \param[out] output triangle array
436  */
437  void
438  generateTriangles(const PtrStep<short2>& volume, const DeviceArray2D<int>& occupied_voxels, const float3& volume_size, DeviceArray<PointType>& output);
439  }
440 }
441 
442 #endif /* PCL_KINFU_INTERNAL_HPP_ */
void paint3DView(const PtrStep< uchar3 > &colors, PtrStepSz< uchar3 > dst, float colors_weight=0.5f)
Paints 3D view with color map.
Intr(float fx_, float fy_, float cx_, float cy_)
Definition: internal.h:71
int computeOffsetsAndTotalVertexes(DeviceArray2D< int > &occupied_voxels)
Computes total number of vertexes for all voxels and offsets of vertexes in final triangle array...
void exctractColors(const PtrStep< uchar4 > &color_volume, const float3 &volume_size, const PtrSz< PointType > &points, uchar4 *colors)
Performs colors exctraction from color volume.
void createVMap(const Intr &intr, const DepthMap &depth, MapArr &vmap)
Computes vertex map.
PCL_EXPORTS size_t extractCloud(const PtrStep< short2 > &volume, const float3 &volume_size, PtrSz< PointType > output)
Perform point cloud extraction from tsdf volume.
unsigned short ushort
Definition: internal.h:51
DeviceArray2D class
Definition: device_array.h:154
void resizeNMap(const MapArr &input, MapArr &output)
Performs resize of vertex map to next pyramid level by averaging each four normals.
void raycast(const Intr &intr, const Mat33 &Rcurr, const float3 &tcurr, float tranc_dist, const float3 &volume_size, const PtrStep< short2 > &volume, MapArr &vmap, MapArr &nmap)
Generation vertex and normal maps from volume for current camera pose.
Intr operator()(int level_index) const
Definition: internal.h:73
void unbindTextures()
Unbinds.
void convert(const MapArr &vmap, DeviceArray2D< T > &output)
Conversion from SOA to AOS.
DeviceArray2D< float > MapArr
Definition: internal.h:52
void tranformMaps(const MapArr &vmap_src, const MapArr &nmap_src, const Mat33 &Rmat, const float3 &tvec, MapArr &vmap_dst, MapArr &nmap_dst)
Performs affine tranform of vertex and normal maps.
bool valid_host(float value)
Check for qnan (unused now)
Definition: internal.h:388
float4 PointType
Definition: internal.hpp:58
DeviceArray class
Definition: device_array.h:57
void updateColorVolume(const Intr &intr, float tranc_dist, const Mat33 &R_inv, const float3 &t, const MapArr &vmap, const PtrStepSz< uchar3 > &colors, const float3 &volume_size, PtrStep< uchar4 > color_volume, int max_weight=1)
Performs integration in color volume.
Camera intrinsics structure.
Definition: internal.h:67
void estimateCombined(const Mat33 &Rcurr, const float3 &tcurr, const MapArr &vmap_curr, const MapArr &nmap_curr, const Mat33 &Rprev_inv, const float3 &tprev, const Intr &intr, const MapArr &vmap_g_prev, const MapArr &nmap_g_prev, float distThres, float angleThres, DeviceArray2D< float > &gbuf, DeviceArray< float > &mbuf, float *matrixA_host, float *vectorB_host)
Computation Ax=b for ICP iteration.
const float VOLUME_SIZE
Definition: internal.h:63
void generateImage(const MapArr &vmap, const MapArr &nmap, const LightSource &light, PtrStepSz< uchar3 > dst)
Renders 3D image of the scene.
const int DIVISOR
Definition: internal.h:57
float4 NormalType
Definition: internal.hpp:59
int getOccupiedVoxels(const PtrStep< short2 > &volume, DeviceArray2D< int > &occupied_voxels)
Scans tsdf volume and retrieves occuped voxes.
void bindTextures(const int *edgeBuf, const int *triBuf, const int *numVertsBuf)
Binds marching cubes tables to texture references.
PCL_EXPORTS void initVolume(PtrStep< short2 > array)
Perform tsdf volume initialization.
void generateDepth(const Mat33 &R_inv, const float3 &t, const MapArr &vmap, DepthMap &dst)
Renders depth image from give pose.
D & device_cast(Matx &matx)
Definition: internal.h:400
Light source collection.
Definition: internal.h:89
void initColorVolume(PtrStep< uchar4 > color_volume)
Initialzied color volume.
void estimateTransform(const MapArr &v_dst, const MapArr &n_dst, const MapArr &v_src, const PtrStepSz< short2 > &coresp, DeviceArray2D< float > &gbuf, DeviceArray< float > &mbuf, float *matrixA_host, float *vectorB_host)
(now it&#39;s exra code) Computation Ax=b for ICP iteration
void bilateralFilter(const DepthMap &src, DepthMap &dst)
Perfoms bilateral filtering of disparity map.
void truncateDepth(DepthMap &depth, float max_distance)
Performs depth truncation.
void createNMap(const MapArr &vmap, MapArr &nmap)
Computes normal map using cross product.
void integrateTsdfVolume(const PtrStepSz< ushort > &depth_raw, const Intr &intr, const float3 &volume_size, const Mat33 &Rcurr_inv, const float3 &tcurr, float tranc_dist, PtrStep< short2 > volume)
Performs Tsfg volume uptation (extra obsolete now)
3x3 Matrix for device code
Definition: internal.h:82
void sync()
synchronizes CUDA execution
Definition: internal.h:396
void computeNormalsEigen(const MapArr &vmap, MapArr &nmap)
Computes normal map using Eigen/PCA approach.
void pyrDown(const DepthMap &src, DepthMap &dst)
Computes depth pyramid.
void generateTriangles(const PtrStep< short2 > &volume, const DeviceArray2D< int > &occupied_voxels, const float3 &volume_size, DeviceArray< PointType > &output)
Generates final triangle array.
void extractNormals(const PtrStep< short2 > &volume, const float3 &volume_size, const PtrSz< PointType > &input, NormalType *output)
Performs normals computation for given poins using tsdf volume.
void resizeVMap(const MapArr &input, MapArr &output)
Performs resize of vertex map to next pyramid level by averaging each four points.
DeviceArray2D< ushort > DepthMap
Definition: internal.h:53
void mergePointNormal(const DeviceArray< float4 > &cloud, const DeviceArray< float8 > &normals, const DeviceArray< float12 > &output)
Merges pcl::PointXYZ and pcl::Normal to PointNormal.
void findCoresp(const MapArr &vmap_g_curr, const MapArr &nmap_g_curr, const Mat33 &Rprev_inv, const float3 &tprev, const Intr &intr, const MapArr &vmap_g_prev, const MapArr &nmap_g_prev, float distThres, float angleThres, PtrStepSz< short2 > coresp)
(now it&#39;s exra code) Computes corespondances map