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