Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////////////////////////
50template <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//////////////////////////////////////////////////////////////////////////////////////////////
143template <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
void computePointIntensityGradient(const pcl::PointCloud< PointInT > &cloud, const pcl::Indices &indices, const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
Estimate the intensity gradient around a given point based on its spatial neighborhood of points.
void computeFeature(PointCloudOut &output) override
Estimate the intensity gradients for a set of points given in <setInputCloud (), setIndices ()> using...
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
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