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>
102 :
points_ (thrust::raw_pointer_cast(&input->points[0])), width_(width), height_(height)
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);
112 int xIdx = idx % width_;
113 int yIdx = idx / width_;
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;
118 bool north_valid = (yIdx > 1) && !isnan (
points_[idx-width_].z) && fabs (
points_[idx-width_].z - query_pt.
z) < 200;
119 bool south_valid = (yIdx < height_-1) && !isnan (
points_[idx+width_].z) && fabs (
points_[idx+width_].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)
132 vert = points_[idx-width_] - points_[idx+width_];
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
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__ float3 computeCentroid(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
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_