38 #ifndef PCL_KINFU_INTERNAL_HPP_
39 #define PCL_KINFU_INTERNAL_HPP_
41 #include <pcl/gpu/containers/device_array.h>
43 #include "safe_call.hpp"
71 Intr (
float fx_,
float fy_,
float cx_,
float cy_) : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {}
75 int div = 1 << level_index;
76 return (
Intr (fx / div, fy / div, cx / div, cy / div));
110 pyrDown (
const DepthMap& src, DepthMap& dst);
118 createVMap (
const Intr& intr,
const DepthMap& depth, MapArr& vmap);
125 createNMap (
const MapArr& vmap, MapArr& nmap);
143 tranformMaps (
const MapArr& vmap_src,
const MapArr& nmap_src,
const Mat33& Rmat,
const float3& tvec, MapArr& vmap_dst, MapArr& nmap_dst);
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);
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,
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,
288 raycast (
const Intr& intr,
const Mat33& Rcurr,
const float3& tcurr,
float tranc_dist,
const float3& volume_size,
308 generateDepth (
const Mat33& R_inv,
const float3& t,
const MapArr& vmap, DepthMap& dst);
323 resizeVMap (
const MapArr& input, MapArr& output);
330 resizeNMap (
const MapArr& input, MapArr& output);
350 template<
typename NormalType>
366 struct float12 {
float x,
y,
z,
w,
normal_x,
normal_y,
normal_z,
n4,
c1,
c2,
c3,
c4; };
390 return *
reinterpret_cast<int*
>(&value) != 0x7fffffff;
396 sync () { cudaSafeCall (cudaDeviceSynchronize ()); }
399 template<
class D,
class Matx> D&
402 return (*reinterpret_cast<D*>(matx.data ()));
410 bindTextures(
const int *edgeBuf,
const int *triBuf,
const int *numVertsBuf);
422 getOccupiedVoxels(
const PtrStep<short2>& volume, DeviceArray2D<int>& occupied_voxels);
438 generateTriangles(
const PtrStep<short2>& volume,
const DeviceArray2D<int>& occupied_voxels,
const float3& volume_size, DeviceArray<PointType>& output);
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_)
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.
This file defines compatibility wrappers for low level I/O functions.
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.
void unbindTextures()
Unbinds.
void convert(const MapArr &vmap, DeviceArray2D< T > &output)
Conversion from SOA to AOS.
DeviceArray2D< float > MapArr
void tranformMaps(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.
bool valid_host(float value)
Check for qnan (unused now)
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.
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 generateImage(const MapArr &vmap, const MapArr &nmap, const LightSource &light, PtrStepSz< uchar3 > dst)
Renders 3D image of the scene.
int getOccupiedVoxels(const PtrStep< short2 > &volume, DeviceArray2D< int > &occupied_voxels)
Scans tsdf volume and retrieves occupied voxels.
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)
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's extra code) Computation Ax=b for ICP iteration
void bilateralFilter(const DepthMap &src, DepthMap &dst)
Performs 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
void sync()
synchronizes CUDA execution
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.
Intr operator()(int level_index) const
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 resizeVMap(const MapArr &input, MapArr &output)
Performs resize of vertex map to next pyramid level by averaging each four points.
DeviceArray2D< ushort > DepthMap
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's extra code) Computes corespondances map