Point Cloud Library (PCL)  1.14.1-dev
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 
44 namespace 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  {
64  CovarianceMatrix cov;
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 everything
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 
91  float focallength_;
93  float sqr_radius_;
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 
151  int width_;
152  int height_;
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);
173  CovarianceMatrix cov;
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 
196  float sqr_radius_;
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