38 #ifndef PCL_CUDA_NORMAL_3D_H_
39 #define PCL_CUDA_NORMAL_3D_H_
41 #include <pcl/pcl_exports.h>
43 #include <pcl/cuda/common/eigen.h>
50 template <
template <
typename>
class Storage>
53 typedef boost::shared_ptr <const PointCloudAOS <Storage> >
CloudConstPtr;
55 :
points_ (thrust::raw_pointer_cast(&input->points[0]))
57 ,
search_ (input, focallength, sqr_radius)
62 inline __host__ __device__
67 if (!isnan (query_pt.x))
71 return make_float4(query_pt.x);
77 return make_float4(0);
81 float curvature = evals.x / (query_pt.z * (0.2f / 4.0f) * query_pt.z * (0.2f / 4.0f));
83 float3 mc = normalize (evecs.
data[0]);
86 if ( dot (query_pt, mc) > 0 )
88 return make_float4 (mc.x, mc.y, mc.z, curvature);
98 template <
template <
typename>
class Storage>
105 inline __host__ __device__
108 float3 query_pt =
points_[idx];
109 if (isnan(query_pt.z))
110 return make_float4 (0.0f,0.0f,0.0f,0.0f);
116 bool west_valid = (xIdx > 1) && !isnan (
points_[idx-1].z) && fabs (
points_[idx-1].z - query_pt.
z) < 200;
117 bool east_valid = (xIdx <
width_-1) && !isnan (
points_[idx+1].z) && fabs (
points_[idx+1].z - query_pt.
z) < 200;
122 if (west_valid & east_valid)
124 if (west_valid & !east_valid)
125 horiz = points_[idx] - points_[idx-1];
126 if (!west_valid & east_valid)
127 horiz = points_[idx+1] - points_[idx];
128 if (!west_valid & !east_valid)
129 return make_float4 (0.0f,0.0f,0.0f,1.0f);
131 if (south_valid & north_valid)
133 if (south_valid & !north_valid)
134 vert = points_[idx] - points_[idx+
width_];
135 if (!south_valid & north_valid)
136 vert = points_[idx-
width_] - points_[idx];
137 if (!south_valid & !north_valid)
138 return make_float4 (0.0f,0.0f,0.0f,1.0f);
140 float3 normal = cross (horiz, vert);
142 float curvature = length (normal);
143 curvature = fabs(horiz.z) > 0.04 | fabs(vert.z) > 0.04 | !west_valid | !east_valid | !north_valid | !south_valid;
145 float3 mc = normalize (normal);
146 if ( dot (query_pt, mc) > 0 )
148 return make_float4 (mc.x, mc.y, mc.z, curvature);
156 template <
template <
typename>
class Storage>
161 :
points_ (thrust::raw_pointer_cast(&input->points[0]))
163 ,
search_ (input, focallength, sqr_radius)
168 template <
typename Tuple>
169 inline __host__ __device__
172 float3 query_pt = thrust::get<0>(t);
173 float4 normal = thrust::get<1>(t);
176 if (!isnan (query_pt.x))
179 return make_float4(query_pt.x);
181 float proj = normal.x * (query_pt.x - centroid.x) / sqrt(
sqr_radius_) +
182 normal.y * (query_pt.y - centroid.y) / sqrt(
sqr_radius_) +
183 normal.z * (query_pt.z - centroid.z) / sqrt(
sqr_radius_) ;
NormalDeviationKernel(const boost::shared_ptr< const PointCloudAOS< Storage > > &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
OrganizedRadiusSearch< CloudConstPtr > search_
misnamed class holding a 3x3 matrix
This file defines compatibility wrappers for low level I/O functions.
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing...
const PointXYZRGB * points_
__host__ __device__ void eigen33(const CovarianceMatrix &mat, CovarianceMatrix &evecs, float3 &evals)
float sqrt_desired_nr_neighbors_
FastNormalEstimationKernel(const boost::shared_ptr< const PointCloudAOS< Storage > > &input, int width, int height)
float sqrt_desired_nr_neighbors_
__host__ __device__ float4 operator()(int idx)
__host__ __device__ float3 computeCentroid(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
__host__ __device__ float4 operator()(const Tuple &t)
boost::shared_ptr< const PointCloudAOS< Storage > > CloudConstPtr
__host__ __device__ int computeCovarianceOnline(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
NormalEstimationKernel(const boost::shared_ptr< const PointCloudAOS< Storage > > &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
const PointXYZRGB * points_
OrganizedRadiusSearch< CloudConstPtr > search_
boost::shared_ptr< const PointCloudAOS< Storage > > CloudConstPtr
Default point xyz-rgb structure.
__host__ __device__ float4 operator()(float3 query_pt)
const PointXYZRGB * points_