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/utils/safe_call.hpp>
42 #include <pcl/gpu/kinfu_large_scale/device.h>
43 
44 //using namespace pcl::gpu;
45 
46 namespace pcl
47 {
48  namespace device
49  {
50  namespace kinfuLS
51  {
52  /** \brief Light source collection
53  */
54  struct LightSource
55  {
56  float3 pos[1];
57  int number;
58  };
59 
60  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61  // Maps
62 
63  /** \brief Perfoms bilateral filtering of disparity map
64  * \param[in] src soruce map
65  * \param[out] dst output map
66  */
67  void
68  bilateralFilter (const DepthMap& src, DepthMap& dst);
69 
70  /** \brief Computes depth pyramid
71  * \param[in] src source
72  * \param[out] dst destination
73  */
74  void
75  pyrDown (const DepthMap& src, DepthMap& dst);
76 
77  /** \brief Computes vertex map
78  * \param[in] intr depth camera intrinsics
79  * \param[in] depth depth
80  * \param[out] vmap vertex map
81  */
82  void
83  createVMap (const Intr& intr, const DepthMap& depth, MapArr& vmap);
84 
85  /** \brief Computes normal map using cross product
86  * \param[in] vmap vertex map
87  * \param[out] nmap normal map
88  */
89  void
90  createNMap (const MapArr& vmap, MapArr& nmap);
91 
92  /** \brief Computes normal map using Eigen/PCA approach
93  * \param[in] vmap vertex map
94  * \param[out] nmap normal map
95  */
96  void
97  computeNormalsEigen (const MapArr& vmap, MapArr& nmap);
98 
99  /** \brief Performs affine tranform of vertex and normal maps
100  * \param[in] vmap_src source vertex map
101  * \param[in] nmap_src source vertex map
102  * \param[in] Rmat Rotation mat
103  * \param[in] tvec translation
104  * \param[out] vmap_dst destination vertex map
105  * \param[out] nmap_dst destination vertex map
106  */
107  void
108  transformMaps (const MapArr& vmap_src, const MapArr& nmap_src, const Mat33& Rmat, const float3& tvec, MapArr& vmap_dst, MapArr& nmap_dst);
109 
110  /** \brief Performs depth truncation
111  * \param[out] depth depth map to truncation
112  * \param[in] max_distance truncation threshold, values that are higher than the threshold are reset to zero (means not measurement)
113  */
114  void
115  truncateDepth(DepthMap& depth, float max_distance);
116 
117  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
118  // ICP
119 
120  /** \brief (now it's exra code) Computes corespondances map
121  * \param[in] vmap_g_curr current vertex map in global coo space
122  * \param[in] nmap_g_curr current normals map in global coo space
123  * \param[in] Rprev_inv inverse camera rotation at previous pose
124  * \param[in] tprev camera translation at previous pose
125  * \param[in] intr camera intrinsics
126  * \param[in] vmap_g_prev previous vertex map in global coo space
127  * \param[in] nmap_g_prev previous vertex map in global coo space
128  * \param[in] distThres distance filtering threshold
129  * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
130  * \param[out] coresp
131  */
132  void
133  findCoresp (const MapArr& vmap_g_curr, const MapArr& nmap_g_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
134  const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres, PtrStepSz<short2> coresp);
135 
136  /** \brief (now it's exra code) Computation Ax=b for ICP iteration
137  * \param[in] v_dst destination vertex map (previous frame cloud)
138  * \param[in] n_dst destination normal map (previous frame normals)
139  * \param[in] v_src source normal map (current frame cloud)
140  * \param[in] coresp Corespondances
141  * \param[out] gbuf temp buffer for GPU reduction
142  * \param[out] mbuf output GPU buffer for matrix computed
143  * \param[out] matrixA_host A
144  * \param[out] vectorB_host b
145  */
146  void
147  estimateTransform (const MapArr& v_dst, const MapArr& n_dst, const MapArr& v_src, const PtrStepSz<short2>& coresp,
148  DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
149 
150 
151  /** \brief Computation Ax=b for ICP iteration
152  * \param[in] Rcurr Rotation of current camera pose guess
153  * \param[in] tcurr translation of current camera pose guess
154  * \param[in] vmap_curr current vertex map in camera coo space
155  * \param[in] nmap_curr current vertex map in camera coo space
156  * \param[in] Rprev_inv inverse camera rotation at previous pose
157  * \param[in] tprev camera translation at previous pose
158  * \param[in] intr camera intrinsics
159  * \param[in] vmap_g_prev previous vertex map in global coo space
160  * \param[in] nmap_g_prev previous vertex map in global coo space
161  * \param[in] distThres distance filtering threshold
162  * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
163  * \param[out] gbuf temp buffer for GPU reduction
164  * \param[out] mbuf output GPU buffer for matrix computed
165  * \param[out] matrixA_host A
166  * \param[out] vectorB_host b
167  */
168  void
169  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,
170  const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
171  DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
172 
173  /** \brief Computation Ax=b for ICP iteration
174  * \param[in] Rcurr Rotation of current camera pose guess
175  * \param[in] tcurr translation of current camera pose guess
176  * \param[in] vmap_curr current vertex map in camera coo space
177  * \param[in] nmap_curr current vertex map in camera coo space
178  * \param[in] Rprev_inv inverse camera rotation at previous pose
179  * \param[in] tprev camera translation at previous pose
180  * \param[in] intr camera intrinsics
181  * \param[in] vmap_g_prev previous vertex map in global coo space
182  * \param[in] nmap_g_prev previous vertex map in global coo space
183  * \param[in] distThres distance filtering threshold
184  * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
185  * \param[out] gbuf temp buffer for GPU reduction
186  * \param[out] mbuf output GPU buffer for matrix computed
187  * \param[out] matrixA_host A
188  * \param[out] vectorB_host b
189  */
190  void
191  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,
192  const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
193  DeviceArray2D<double>& gbuf, DeviceArray<double>& mbuf, double* matrixA_host, double* vectorB_host);
194 
195  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
196  // TSDF volume functions
197 
198  /** \brief Perform tsdf volume initialization
199  * \param[out] array volume to be initialized
200  */
201  PCL_EXPORTS void
203 
204  //first version
205  /** \brief Performs Tsfg volume uptation (extra obsolete now)
206  * \param[in] depth_raw Kinect depth image
207  * \param[in] intr camera intrinsics
208  * \param[in] volume_size size of volume in mm
209  * \param[in] Rcurr_inv inverse rotation for current camera pose
210  * \param[in] tcurr translation for current camera pose
211  * \param[in] tranc_dist tsdf truncation distance
212  * \param[in] volume tsdf volume to be updated
213  */
214  void
215  integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
216  const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume);
217 
218  //second version
219  /** \brief Function that integrates volume if volume element contains: 2 bytes for round(tsdf*SHORT_MAX) and 2 bytes for integer weight.
220  * \param[in] depth Kinect depth image
221  * \param[in] intr camera intrinsics
222  * \param[in] volume_size size of volume in mm
223  * \param[in] Rcurr_inv inverse rotation for current camera pose
224  * \param[in] tcurr translation for current camera pose
225  * \param[in] tranc_dist tsdf truncation distance
226  * \param[in] volume tsdf volume to be updated
227  * \param[in] buffer cyclical buffer structure
228  * \param[out] depthScaled Buffer for scaled depth along ray
229  */
230  PCL_EXPORTS void
231  integrateTsdfVolume (const PtrStepSz<ushort>& depth, const Intr& intr, const float3& volume_size,
232  const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, DeviceArray2D<float>& depthScaled);
233 
234  /** \brief Function that clears the TSDF values. The clearing takes place from the origin (in indices) to an offset in X,Y,Z values accordingly
235  * \param[in] volume Pointer to TSDF volume in GPU
236  * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
237  * \param[in] shiftX Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginX and stops in OriginX + shiftX
238  * \param[in] shiftY Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginY and stops in OriginY + shiftY
239  * \param[in] shiftZ Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginZ and stops in OriginZ + shiftZ
240  */
241  PCL_EXPORTS void
242  clearTSDFSlice (PtrStep<short2> volume, pcl::gpu::kinfuLS::tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ);
243 
244  /** \brief Initialzied color volume
245  * \param[out] color_volume color volume for initialization
246  */
247  void
248  initColorVolume(PtrStep<uchar4> color_volume);
249 
250  /** \brief Performs integration in color volume
251  * \param[in] intr Depth camera intrionsics structure
252  * \param[in] tranc_dist tsdf truncation distance
253  * \param[in] R_inv Inverse camera rotation
254  * \param[in] t camera translation
255  * \param[in] vmap Raycasted vertex map
256  * \param[in] colors RGB colors for current frame
257  * \param[in] volume_size volume size in meters
258  * \param[in] color_volume color volume to be integrated
259  * \param[in] max_weight max weight for running color average. Zero means not average, one means average with prev value, etc.
260  */
261  void
262  updateColorVolume(const Intr& intr, float tranc_dist, const Mat33& R_inv, const float3& t, const MapArr& vmap,
263  const PtrStepSz<uchar3>& colors, const float3& volume_size, PtrStep<uchar4> color_volume, int max_weight = 1);
264 
265  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
266  // Raycast and view generation
267  /** \brief Generation vertex and normal maps from volume for current camera pose
268  * \param[in] intr camera intrinsices
269  * \param[in] Rcurr current rotation
270  * \param[in] tcurr current translation
271  * \param[in] tranc_dist volume truncation distance
272  * \param[in] volume_size volume size in mm
273  * \param[in] volume tsdf volume
274  * \param[in] buffer cyclical buffer structure
275  * \param[out] vmap output vertex map
276  * \param[out] nmap output normals map
277  */
278  void
279  raycast (const Intr& intr, const Mat33& Rcurr, const float3& tcurr, float tranc_dist, const float3& volume_size,
280  const PtrStep<short2>& volume, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, MapArr& vmap, MapArr& nmap);
281 
282  /** \brief Renders 3D image of the scene
283  * \param[in] vmap vertex map
284  * \param[in] nmap normals map
285  * \param[in] light pose of light source
286  * \param[out] dst buffer where image is generated
287  */
288  void
289  generateImage (const MapArr& vmap, const MapArr& nmap, const LightSource& light, PtrStepSz<uchar3> dst);
290 
291 
292  /** \brief Renders depth image from give pose
293  * \param[in] R_inv inverse camera rotation
294  * \param[in] t camera translation
295  * \param[in] vmap vertex map
296  * \param[out] dst buffer where depth is generated
297  */
298  void
299  generateDepth (const Mat33& R_inv, const float3& t, const MapArr& vmap, DepthMap& dst);
300 
301  /** \brief Paints 3D view with color map
302  * \param[in] colors rgb color frame from OpenNI
303  * \param[out] dst output 3D view
304  * \param[in] colors_weight weight for colors
305  */
306  void
307  paint3DView(const PtrStep<uchar3>& colors, PtrStepSz<uchar3> dst, float colors_weight = 0.5f);
308 
309  /** \brief Performs resize of vertex map to next pyramid level by averaging each four points
310  * \param[in] input vertext map
311  * \param[out] output resized vertex map
312  */
313  void
314  resizeVMap (const MapArr& input, MapArr& output);
315 
316  /** \brief Performs resize of vertex map to next pyramid level by averaging each four normals
317  * \param[in] input normal map
318  * \param[out] output vertex map
319  */
320  void
321  resizeNMap (const MapArr& input, MapArr& output);
322 
323  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
324  // Push data to TSDF
325 
326  /** \brief Loads the values of a tsdf point cloud to the tsdf volume in GPU
327  * \param[in] volume tsdf volume
328  * \param[in] cloud_gpu contains the data to be pushed to the tsdf volume
329  * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
330  */
331  /*PCL_EXPORTS*/ void
333 
334  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
335  // Cloud extraction
336 
337  /** \brief Perform point cloud extraction from tsdf volume
338  * \param[in] volume tsdf volume
339  * \param[in] volume_size size of the volume
340  * \param[out] output buffer large enought to store point cloud
341  * \return number of point stored to passed buffer
342  */
343  PCL_EXPORTS size_t
344  extractCloud (const PtrStep<short2>& volume, const float3& volume_size, PtrSz<PointType> output);
345 
346  /** \brief Perform point cloud extraction of a slice from tsdf volume
347  * \param[in] volume tsdf volume on GPU
348  * \param[in] volume_size size of the volume
349  * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
350  * \param[in] shiftX Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginX and stops in OriginX + shiftX
351  * \param[in] shiftY Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginY and stops in OriginY + shiftY
352  * \param[in] shiftZ Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginZ and stops in OriginZ + shiftZ
353  * \param[out] output_xyz buffer large enought to store point cloud xyz values
354  * \param[out] output_intensities buffer large enought to store point cloud intensity values
355  * \return number of point stored to passed buffer
356  */
357  PCL_EXPORTS size_t
358  extractSliceAsCloud (const PtrStep<short2>& volume, const float3& volume_size, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, const int shiftX, const int shiftY, const int shiftZ, PtrSz<PointType> output_xyz, PtrSz<float> output_intensities);
359 
360  /** \brief Performs normals computation for given poins using tsdf volume
361  * \param[in] volume tsdf volume
362  * \param[in] volume_size volume size
363  * \param[in] input points where normals are computed
364  * \param[out] output normals. Could be float4 or float8. If for a point normal can't be computed, such normal is marked as nan.
365  */
366  template<typename NormalType>
367  void
368  extractNormals (const PtrStep<short2>& volume, const float3& volume_size, const PtrSz<PointType>& input, NormalType* output);
369 
370  /** \brief Performs colors exctraction from color volume
371  * \param[in] color_volume color volume
372  * \param[in] volume_size volume size
373  * \param[in] points points for which color are computed
374  * \param[out] colors output array with colors.
375  */
376  void
377  exctractColors(const PtrStep<uchar4>& color_volume, const float3& volume_size, const PtrSz<PointType>& points, uchar4* colors);
378 
379  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
380  // Utility
381  struct float8 { float x, y, z, w, c1, c2, c3, c4; };
382  struct float12 { float x, y, z, w, normal_x, normal_y, normal_z, n4, c1, c2, c3, c4; };
383 
384  /** \brief Conversion from SOA to AOS
385  * \param[in] vmap SOA map
386  * \param[out] output Array of 3D points. Can be float4 or float8.
387  */
388  template<typename T>
389  void
390  convert (const MapArr& vmap, DeviceArray2D<T>& output);
391 
392  /** \brief Merges pcl::PointXYZ and pcl::Normal to PointNormal
393  * \param[in] cloud points cloud
394  * \param[in] normals normals cloud
395  * \param[out] output array of PointNomals.
396  */
397  void
398  mergePointNormal(const DeviceArray<float4>& cloud, const DeviceArray<float8>& normals, const DeviceArray<float12>& output);
399 
400  /** \brief Check for qnan (unused now)
401  * \param[in] value
402  */
403  inline bool
404  valid_host (float value)
405  {
406  return *reinterpret_cast<int*>(&value) != 0x7fffffff; //QNAN
407  }
408 
409  /** \brief synchronizes CUDA execution */
410  inline
411  void
412  sync () { cudaSafeCall (cudaDeviceSynchronize ()); }
413 
414 
415  template<class D, class Matx> D&
416  device_cast (Matx& matx)
417  {
418  return (*reinterpret_cast<D*>(matx.data ()));
419  }
420 
421  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
422  // Marching cubes implementation
423 
424  /** \brief Binds marching cubes tables to texture references */
425  void
426  bindTextures(const int *edgeBuf, const int *triBuf, const int *numVertsBuf);
427 
428  /** \brief Unbinds */
429  void
430  unbindTextures();
431 
432  /** \brief Scans tsdf volume and retrieves occuped voxes
433  * \param[in] volume tsdf volume
434  * \param[out] occupied_voxels buffer for occuped voxels. The function fulfills first row with voxel ids and second row with number of vertextes.
435  * \return number of voxels in the buffer
436  */
437  int
438  getOccupiedVoxels(const PtrStep<short2>& volume, DeviceArray2D<int>& occupied_voxels);
439 
440  /** \brief Computes total number of vertexes for all voxels and offsets of vertexes in final triangle array
441  * \param[out] occupied_voxels buffer with occuped voxels. The function fulfills 3nd only with offsets
442  * \return total number of vertexes
443  */
444  int
446 
447  /** \brief Generates final triangle array
448  * \param[in] volume tsdf volume
449  * \param[in] occupied_voxels occuped voxel ids (first row), number of vertexes(second row), offsets(third row).
450  * \param[in] volume_size volume size in meters
451  * \param[out] output triangle array
452  */
453  void
454  generateTriangles(const PtrStep<short2>& volume, const DeviceArray2D<int>& occupied_voxels, const float3& volume_size, DeviceArray<PointType>& output);
455  }
456  }
457 }
458 
459 #endif /* PCL_KINFU_INTERNAL_HPP_ */
int getOccupiedVoxels(const PtrStep< short2 > &volume, DeviceArray2D< int > &occupied_voxels)
Scans tsdf volume and retrieves occuped voxes.
Camera intrinsics structure.
Definition: device.h:81
void generateDepth(const Mat33 &R_inv, const float3 &t, const MapArr &vmap, DepthMap &dst)
Renders depth image from give pose.
void generateImage(const MapArr &vmap, const MapArr &nmap, const LightSource &light, PtrStepSz< uchar3 > dst)
Renders 3D image of the scene.
Structure to handle buffer addresses.
Definition: tsdf_buffer.h:51
void convert(const MapArr &vmap, DeviceArray2D< T > &output)
Conversion from SOA to AOS.
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.
void bilateralFilter(const DepthMap &src, DepthMap &dst)
Perfoms bilateral filtering of disparity map.
void generateTriangles(const PtrStep< short2 > &volume, const DeviceArray2D< int > &occupied_voxels, const float3 &volume_size, DeviceArray< PointType > &output)
Generates final triangle array.
void truncateDepth(DepthMap &depth, float max_distance)
Performs depth truncation.
bool valid_host(float value)
Check for qnan (unused now)
Definition: internal.h:404
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
DeviceArray2D class
Definition: device_array.h:154
void paint3DView(const PtrStep< uchar3 > &colors, PtrStepSz< uchar3 > dst, float colors_weight=0.5f)
Paints 3D view with color map.
3x3 Matrix for device code
Definition: device.h:103
PCL_EXPORTS size_t extractSliceAsCloud(const PtrStep< short2 > &volume, const float3 &volume_size, const pcl::gpu::kinfuLS::tsdf_buffer *buffer, const int shiftX, const int shiftY, const int shiftZ, PtrSz< PointType > output_xyz, PtrSz< float > output_intensities)
Perform point cloud extraction of a slice from tsdf volume.
PCL_EXPORTS void clearTSDFSlice(PtrStep< short2 > volume, pcl::gpu::kinfuLS::tsdf_buffer *buffer, int shiftX, int shiftY, int shiftZ)
Function that clears the TSDF values.
void createVMap(const Intr &intr, const DepthMap &depth, MapArr &vmap)
Computes vertex map.
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 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.
void initColorVolume(PtrStep< uchar4 > color_volume)
Initialzied color volume.
DeviceArray class
Definition: device_array.h:57
void transformMaps(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.
void pyrDown(const DepthMap &src, DepthMap &dst)
Computes depth pyramid.
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 bindTextures(const int *edgeBuf, const int *triBuf, const int *numVertsBuf)
Binds marching cubes tables to texture references.
float4 NormalType
Definition: internal.hpp:59
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)
void sync()
synchronizes CUDA execution
Definition: internal.h:412
void pushCloudAsSliceGPU(const PtrStep< short2 > &volume, pcl::gpu::DeviceArray< PointType > cloud_gpu, const pcl::gpu::kinfuLS::tsdf_buffer *buffer)
Loads the values of a tsdf point cloud to the tsdf volume in GPU.
PCL_EXPORTS void initVolume(PtrStep< short2 > array)
Perform tsdf volume initialization.
void raycast(const Intr &intr, const Mat33 &Rcurr, const float3 &tcurr, float tranc_dist, const float3 &volume_size, const PtrStep< short2 > &volume, const pcl::gpu::kinfuLS::tsdf_buffer *buffer, MapArr &vmap, MapArr &nmap)
Generation vertex and normal maps from volume for current camera pose.
void computeNormalsEigen(const MapArr &vmap, MapArr &nmap)
Computes normal map using Eigen/PCA approach.
void createNMap(const MapArr &vmap, MapArr &nmap)
Computes normal map using cross product.
D & device_cast(Matx &matx)
Definition: internal.h:416
void resizeVMap(const MapArr &input, MapArr &output)
Performs resize of vertex map to next pyramid level by averaging each four points.
PCL_EXPORTS size_t extractCloud(const PtrStep< short2 > &volume, const float3 &volume_size, PtrSz< PointType > output)
Perform point cloud extraction from tsdf volume.
void resizeNMap(const MapArr &input, MapArr &output)
Performs resize of vertex map to next pyramid level by averaging each four normals.
void unbindTextures()
Unbinds.
void mergePointNormal(const DeviceArray< float4 > &cloud, const DeviceArray< float8 > &normals, const DeviceArray< float12 > &output)
Merges pcl::PointXYZ and pcl::Normal to PointNormal.
int computeOffsetsAndTotalVertexes(DeviceArray2D< int > &occupied_voxels)
Computes total number of vertexes for all voxels and offsets of vertexes in final triangle array...
Light source collection.
Definition: internal.h:54
void exctractColors(const PtrStep< uchar4 > &color_volume, const float3 &volume_size, const PtrSz< PointType > &points, uchar4 *colors)
Performs colors exctraction from color volume.