Point Cloud Library (PCL)  1.14.1-dev
intensity_gradient.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/features/intensity_gradient.h>
44 
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 #include <pcl/common/eigen.h> // for eigen33
47 
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
52  const pcl::PointCloud <PointInT> &cloud, const pcl::Indices &indices,
53  const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
54 {
55  if (indices.size () < 3)
56  {
57  gradient[0] = gradient[1] = gradient[2] = std::numeric_limits<float>::quiet_NaN ();
58  return;
59  }
60 
61  Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
62  Eigen::Vector3f b = Eigen::Vector3f::Zero ();
63 
64  for (const auto &nn_index : indices)
65  {
66  PointInT p = cloud[nn_index];
67  if (!std::isfinite (p.x) ||
68  !std::isfinite (p.y) ||
69  !std::isfinite (p.z) ||
70  !std::isfinite (intensity_ (p)))
71  continue;
72 
73  p.x -= point[0];
74  p.y -= point[1];
75  p.z -= point[2];
76  intensity_.demean (p, mean_intensity);
77 
78  A (0, 0) += p.x * p.x;
79  A (0, 1) += p.x * p.y;
80  A (0, 2) += p.x * p.z;
81 
82  A (1, 1) += p.y * p.y;
83  A (1, 2) += p.y * p.z;
84 
85  A (2, 2) += p.z * p.z;
86 
87  b[0] += p.x * intensity_ (p);
88  b[1] += p.y * intensity_ (p);
89  b[2] += p.z * intensity_ (p);
90  }
91  // Fill in the lower triangle of A
92  A (1, 0) = A (0, 1);
93  A (2, 0) = A (0, 2);
94  A (2, 1) = A (1, 2);
95 
96 // Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
97 
98  Eigen::Vector3f eigen_values;
99  Eigen::Matrix3f eigen_vectors;
100  eigen33 (A, eigen_vectors, eigen_values);
101 
102  b = eigen_vectors.transpose () * b;
103 
104  if ( eigen_values (0) != 0)
105  b (0) /= eigen_values (0);
106  else
107  b (0) = 0;
108 
109  if ( eigen_values (1) != 0)
110  b (1) /= eigen_values (1);
111  else
112  b (1) = 0;
113 
114  if ( eigen_values (2) != 0)
115  b (2) /= eigen_values (2);
116  else
117  b (2) = 0;
118 
119 
120  Eigen::Vector3f x = eigen_vectors * b;
121 
122 // if (A.col (0).squaredNorm () != 0)
123 // x [0] /= A.col (0).squaredNorm ();
124 // b -= x [0] * A.col (0);
125 //
126 //
127 // if (A.col (1).squaredNorm () != 0)
128 // x [1] /= A.col (1).squaredNorm ();
129 // b -= x[1] * A.col (1);
130 //
131 // x [2] = b.dot (A.col (2));
132 // if (A.col (2).squaredNorm () != 0)
133 // x[2] /= A.col (2).squaredNorm ();
134 // // Fit a hyperplane to the data
135 //
136 // std::cout << A << "\n*\n" << bb << "\n=\n" << x << "\nvs.\n" << x2 << "\n\n";
137 // std::cout << A * x << "\nvs.\n" << A * x2 << "\n\n------\n";
138  // Project the gradient vector, x, onto the tangent plane
139  gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x;
140 }
141 
142 //////////////////////////////////////////////////////////////////////////////////////////////
143 template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
145 {
146  // Allocate enough space to hold the results
147  // \note This resize is irrelevant for a radiusSearch ().
148  pcl::Indices nn_indices (k_);
149  std::vector<float> nn_dists (k_);
150  output.is_dense = true;
151 
152 #ifdef _OPENMP
153  if (threads_ == 0) {
154  threads_ = omp_get_num_procs();
155  PCL_DEBUG ("[pcl::IntensityGradientEstimation::computeFeature] Setting number of threads to %u.\n", threads_);
156  }
157 #endif // _OPENMP
158 
159  // If the data is dense, we don't need to check for NaN
160  if (surface_->is_dense)
161  {
162 #pragma omp parallel for \
163  default(none) \
164  shared(output) \
165  firstprivate(nn_indices, nn_dists) \
166  num_threads(threads_)
167  // Iterating over the entire index vector
168  for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
169  {
170  PointOutT &p_out = output[idx];
171 
172  if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
173  {
174  p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
175  output.is_dense = false;
176  continue;
177  }
178 
179  Eigen::Vector3f centroid;
180  float mean_intensity = 0;
181  // Initialize to 0
182  centroid.setZero ();
183  for (const auto &nn_index : nn_indices)
184  {
185  centroid += (*surface_)[nn_index].getVector3fMap ();
186  mean_intensity += intensity_ ((*surface_)[nn_index]);
187  }
188  centroid /= static_cast<float> (nn_indices.size ());
189  mean_intensity /= static_cast<float> (nn_indices.size ());
190 
191  Eigen::Vector3f normal = Eigen::Vector3f::Map ((*normals_)[(*indices_) [idx]].normal);
192  Eigen::Vector3f gradient;
193  computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
194 
195  p_out.gradient[0] = gradient[0];
196  p_out.gradient[1] = gradient[1];
197  p_out.gradient[2] = gradient[2];
198  }
199  }
200  else
201  {
202 #pragma omp parallel for \
203  default(none) \
204  shared(output) \
205  firstprivate(nn_indices, nn_dists) \
206  num_threads(threads_)
207  // Iterating over the entire index vector
208  for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
209  {
210  PointOutT &p_out = output[idx];
211  if (!isFinite ((*surface_) [(*indices_)[idx]]) ||
212  !this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
213  {
214  p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
215  output.is_dense = false;
216  continue;
217  }
218  Eigen::Vector3f centroid;
219  float mean_intensity = 0;
220  // Initialize to 0
221  centroid.setZero ();
222  unsigned cp = 0;
223  for (const auto &nn_index : nn_indices)
224  {
225  // Check if the point is invalid
226  if (!isFinite ((*surface_) [nn_index]))
227  continue;
228 
229  centroid += surface_->points [nn_index].getVector3fMap ();
230  mean_intensity += intensity_ (surface_->points [nn_index]);
231  ++cp;
232  }
233  centroid /= static_cast<float> (cp);
234  mean_intensity /= static_cast<float> (cp);
235  Eigen::Vector3f normal = Eigen::Vector3f::Map ((*normals_)[(*indices_) [idx]].normal);
236  Eigen::Vector3f gradient;
237  computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
238 
239  p_out.gradient[0] = gradient[0];
240  p_out.gradient[1] = gradient[1];
241  p_out.gradient[2] = gradient[2];
242  }
243  }
244 }
245 
246 #define PCL_INSTANTIATE_IntensityGradientEstimation(InT,NT,OutT) template class PCL_EXPORTS pcl::IntensityGradientEstimation<InT,NT,OutT>;
247 
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
void computeFeature(PointCloudOut &output) override
Estimate the intensity gradients for a set of points given in <setInputCloud (), setIndices ()> using...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:295
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133