Point Cloud Library (PCL) 1.13.0
normal_3d_kernels.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id: normal_3d.h 1370 2011-06-19 01:06:01Z jspricke $
35 *
36 */
37
38#pragma once
39
40#include <pcl/pcl_exports.h>
41
42#include <pcl/cuda/common/eigen.h>
43
44namespace pcl
45{
46 namespace cuda
47 {
48
49 template <template <typename> class Storage>
51 {
53 NormalEstimationKernel (const typename PointCloudAOS<Storage>::ConstPtr &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
54 : points_ (thrust::raw_pointer_cast(&input->points[0]))
55 , focallength_ (focallength)
56 , search_ (input, focallength, sqr_radius)
57 , sqr_radius_(sqr_radius)
58 , sqrt_desired_nr_neighbors_ (sqrt_desired_nr_neighbors)
59 {}
60
61 inline __host__ __device__
62 float4 operator () (float3 query_pt)
63 {
65 int nnn = 0;
66 if (!isnan (query_pt.x))
67 nnn =
69 else
70 return make_float4(query_pt.x);
71
72 CovarianceMatrix evecs;
73 float3 evals;
74 // compute eigenvalues and -vectors
75 if (nnn <= 1)
76 return make_float4(0);
77
78 eigen33 (cov, evecs, evals);
79 //float curvature = evals.x / (evals.x + evals.y + evals.z);
80 float curvature = evals.x / (query_pt.z * (0.2f / 4.0f) * query_pt.z * (0.2f / 4.0f));
81
82 float3 mc = normalize (evecs.data[0]);
83 // TODO: this should be an optional step, as it slows down eveything
84 // btw, this flips the normals to face the origin (assumed to be the view point)
85 if ( dot (query_pt, mc) > 0 )
86 mc = -mc;
87 return make_float4 (mc.x, mc.y, mc.z, curvature);
88 }
89
95 };
96
97 template <template <typename> class Storage>
99 {
100 FastNormalEstimationKernel (const typename PointCloudAOS<Storage>::ConstPtr &input, int width, int height)
101 : points_ (thrust::raw_pointer_cast(&input->points[0])), width_(width), height_(height)
102 {}
103
104 inline __host__ __device__
105 float4 operator () (int idx)
106 {
107 float3 query_pt = points_[idx];
108 if (isnan(query_pt.z))
109 return make_float4 (0.0f,0.0f,0.0f,0.0f);
110
111 int xIdx = idx % width_;
112 int yIdx = idx / width_;
113
114 // are we at a border? are our neighbor valid points?
115 bool west_valid = (xIdx > 1) && !isnan (points_[idx-1].z) && std::abs (points_[idx-1].z - query_pt.z) < 200;
116 bool east_valid = (xIdx < width_-1) && !isnan (points_[idx+1].z) && std::abs (points_[idx+1].z - query_pt.z) < 200;
117 bool north_valid = (yIdx > 1) && !isnan (points_[idx-width_].z) && std::abs (points_[idx-width_].z - query_pt.z) < 200;
118 bool south_valid = (yIdx < height_-1) && !isnan (points_[idx+width_].z) && std::abs (points_[idx+width_].z - query_pt.z) < 200;
119
120 float3 horiz, vert;
121 if (west_valid & east_valid)
122 horiz = points_[idx+1] - points_[idx-1];
123 if (west_valid & !east_valid)
124 horiz = points_[idx] - points_[idx-1];
125 if (!west_valid & east_valid)
126 horiz = points_[idx+1] - points_[idx];
127 if (!west_valid & !east_valid)
128 return make_float4 (0.0f,0.0f,0.0f,1.0f);
129
130 if (south_valid & north_valid)
131 vert = points_[idx-width_] - points_[idx+width_];
132 if (south_valid & !north_valid)
133 vert = points_[idx] - points_[idx+width_];
134 if (!south_valid & north_valid)
135 vert = points_[idx-width_] - points_[idx];
136 if (!south_valid & !north_valid)
137 return make_float4 (0.0f,0.0f,0.0f,1.0f);
138
139 float3 normal = cross (horiz, vert);
140
141 float curvature = length (normal);
142 curvature = std::abs(horiz.z) > 0.04 | std::abs(vert.z) > 0.04 | !west_valid | !east_valid | !north_valid | !south_valid;
143
144 float3 mc = normalize (normal);
145 if ( dot (query_pt, mc) > 0 )
146 mc = -mc;
147 return make_float4 (mc.x, mc.y, mc.z, curvature);
148 }
149
153 };
154
155 template <template <typename> class Storage>
157 {
159 NormalDeviationKernel (const typename PointCloudAOS<Storage>::ConstPtr &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
160 : points_ (thrust::raw_pointer_cast(&input->points[0]))
161 , focallength_ (focallength)
162 , search_ (input, focallength, sqr_radius)
163 , sqr_radius_(sqr_radius)
164 , sqrt_desired_nr_neighbors_ (sqrt_desired_nr_neighbors)
165 {}
166
167 template <typename Tuple>
168 inline __host__ __device__
169 float4 operator () (const Tuple &t)
170 {
171 float3 query_pt = thrust::get<0>(t);
172 float4 normal = thrust::get<1>(t);
174 float3 centroid;
175 if (!isnan (query_pt.x))
176 centroid = search_.computeCentroid (query_pt, cov, sqrt_desired_nr_neighbors_);
177 else
178 return make_float4(query_pt.x);
179
180 float proj = normal.x * (query_pt.x - centroid.x) / sqrt(sqr_radius_) +
181 normal.y * (query_pt.y - centroid.y) / sqrt(sqr_radius_) +
182 normal.z * (query_pt.z - centroid.z) / sqrt(sqr_radius_) ;
183
184
185 //return make_float4 (normal.x*proj, normal.y*proj, normal.z*proj, clamp (std::abs (proj), 0.0f, 1.0f));
186 return make_float4 (
187 (centroid.x - query_pt.x) / sqrt(sqr_radius_) ,
188 (centroid.y - query_pt.y) / sqrt(sqr_radius_) ,
189 (centroid.z - query_pt.z) / sqrt(sqr_radius_) ,
190 0);
191 }
192
198 };
199
200 } // namespace
201} // namespace
__host__ __device__ float3 computeCentroid(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition: eigen.h:705
__host__ __device__ int computeCovarianceOnline(const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
Definition: eigen.h:622
shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:200
__host__ __device__ void eigen33(const CovarianceMatrix &mat, CovarianceMatrix &evecs, float3 &evals)
Definition: eigen.h:227
misnamed class holding a 3x3 matrix
Definition: point_cloud.h:50
__host__ __device__ float4 operator()(int idx)
FastNormalEstimationKernel(const typename PointCloudAOS< Storage >::ConstPtr &input, int width, int height)
OrganizedRadiusSearch< CloudConstPtr > search_
typename PointCloudAOS< Storage >::ConstPtr CloudConstPtr
__host__ __device__ float4 operator()(const Tuple &t)
NormalDeviationKernel(const typename PointCloudAOS< Storage >::ConstPtr &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
OrganizedRadiusSearch< CloudConstPtr > search_
NormalEstimationKernel(const typename PointCloudAOS< Storage >::ConstPtr &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
__host__ __device__ float4 operator()(float3 query_pt)
typename PointCloudAOS< Storage >::ConstPtr CloudConstPtr
Default point xyz-rgb structure.
Definition: point_types.h:49