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
47
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> 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)50 pcl::IntensityGradientEstimation <PointInT, PointNT, PointOutT, IntensitySelectorT>::computePointIntensityGradient (
51 const pcl::PointCloud <PointInT> &cloud, const pcl::Indices &indices,
52 const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
53 {
54 if (indices.size () < 3)
55 {
56 gradient[0] = gradient[1] = gradient[2] = std::numeric_limits<float>::quiet_NaN ();
57 return;
58 }
59
60 Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
61 Eigen::Vector3f b = Eigen::Vector3f::Zero ();
62
63 for (const auto &nn_index : indices)
64 {
65 PointInT p = cloud[nn_index];
66 if (!std::isfinite (p.x) ||
67 !std::isfinite (p.y) ||
68 !std::isfinite (p.z) ||
69 !std::isfinite (intensity_ (p)))
70 continue;
71
72 p.x -= point[0];
73 p.y -= point[1];
74 p.z -= point[2];
75 intensity_.demean (p, mean_intensity);
76
77 A (0, 0) += p.x * p.x;
78 A (0, 1) += p.x * p.y;
79 A (0, 2) += p.x * p.z;
80
81 A (1, 1) += p.y * p.y;
82 A (1, 2) += p.y * p.z;
83
84 A (2, 2) += p.z * p.z;
85
86 b[0] += p.x * intensity_ (p);
87 b[1] += p.y * intensity_ (p);
88 b[2] += p.z * intensity_ (p);
89 }
90 // Fill in the lower triangle of A
91 A (1, 0) = A (0, 1);
92 A (2, 0) = A (0, 2);
93 A (2, 1) = A (1, 2);
94
95 // Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
96
97 Eigen::Vector3f eigen_values;
98 Eigen::Matrix3f eigen_vectors;
99 eigen33 (A, eigen_vectors, eigen_values);
100
101 b = eigen_vectors.transpose () * b;
102
103 if ( eigen_values (0) != 0)
104 b (0) /= eigen_values (0);
105 else
106 b (0) = 0;
107
108 if ( eigen_values (1) != 0)
109 b (1) /= eigen_values (1);
110 else
111 b (1) = 0;
112
113 if ( eigen_values (2) != 0)
114 b (2) /= eigen_values (2);
115 else
116 b (2) = 0;
117
118
119 Eigen::Vector3f x = eigen_vectors * b;
120
121 // if (A.col (0).squaredNorm () != 0)
122 // x [0] /= A.col (0).squaredNorm ();
123 // b -= x [0] * A.col (0);
124 //
125 //
126 // if (A.col (1).squaredNorm () != 0)
127 // x [1] /= A.col (1).squaredNorm ();
128 // b -= x[1] * A.col (1);
129 //
130 // x [2] = b.dot (A.col (2));
131 // if (A.col (2).squaredNorm () != 0)
132 // x[2] /= A.col (2).squaredNorm ();
133 // // Fit a hyperplane to the data
134 //
135 // std::cout << A << "\n*\n" << bb << "\n=\n" << x << "\nvs.\n" << x2 << "\n\n";
136 // std::cout << A * x << "\nvs.\n" << A * x2 << "\n\n------\n";
137 // Project the gradient vector, x, onto the tangent plane
138 gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x;
139 }
140
141 //////////////////////////////////////////////////////////////////////////////////////////////
142 template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
computeFeature(PointCloudOut & output)143 pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT, IntensitySelectorT>::computeFeature (PointCloudOut &output)
144 {
145 // Allocate enough space to hold the results
146 // \note This resize is irrelevant for a radiusSearch ().
147 pcl::Indices nn_indices (k_);
148 std::vector<float> nn_dists (k_);
149 output.is_dense = true;
150
151 // If the data is dense, we don't need to check for NaN
152 if (surface_->is_dense)
153 {
154 #pragma omp parallel for \
155 default(none) \
156 shared(output) \
157 firstprivate(nn_indices, nn_dists) \
158 num_threads(threads_)
159 // Iterating over the entire index vector
160 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
161 {
162 PointOutT &p_out = output[idx];
163
164 if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
165 {
166 p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
167 output.is_dense = false;
168 continue;
169 }
170
171 Eigen::Vector3f centroid;
172 float mean_intensity = 0;
173 // Initialize to 0
174 centroid.setZero ();
175 for (const auto &nn_index : nn_indices)
176 {
177 centroid += (*surface_)[nn_index].getVector3fMap ();
178 mean_intensity += intensity_ ((*surface_)[nn_index]);
179 }
180 centroid /= static_cast<float> (nn_indices.size ());
181 mean_intensity /= static_cast<float> (nn_indices.size ());
182
183 Eigen::Vector3f normal = Eigen::Vector3f::Map ((*normals_)[(*indices_) [idx]].normal);
184 Eigen::Vector3f gradient;
185 computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
186
187 p_out.gradient[0] = gradient[0];
188 p_out.gradient[1] = gradient[1];
189 p_out.gradient[2] = gradient[2];
190 }
191 }
192 else
193 {
194 #pragma omp parallel for \
195 default(none) \
196 shared(output) \
197 firstprivate(nn_indices, nn_dists) \
198 num_threads(threads_)
199 // Iterating over the entire index vector
200 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
201 {
202 PointOutT &p_out = output[idx];
203 if (!isFinite ((*surface_) [(*indices_)[idx]]) ||
204 !this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
205 {
206 p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
207 output.is_dense = false;
208 continue;
209 }
210 Eigen::Vector3f centroid;
211 float mean_intensity = 0;
212 // Initialize to 0
213 centroid.setZero ();
214 unsigned cp = 0;
215 for (const auto &nn_index : nn_indices)
216 {
217 // Check if the point is invalid
218 if (!isFinite ((*surface_) [nn_index]))
219 continue;
220
221 centroid += surface_->points [nn_index].getVector3fMap ();
222 mean_intensity += intensity_ (surface_->points [nn_index]);
223 ++cp;
224 }
225 centroid /= static_cast<float> (cp);
226 mean_intensity /= static_cast<float> (cp);
227 Eigen::Vector3f normal = Eigen::Vector3f::Map ((*normals_)[(*indices_) [idx]].normal);
228 Eigen::Vector3f gradient;
229 computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
230
231 p_out.gradient[0] = gradient[0];
232 p_out.gradient[1] = gradient[1];
233 p_out.gradient[2] = gradient[2];
234 }
235 }
236 }
237
238 #define PCL_INSTANTIATE_IntensityGradientEstimation(InT,NT,OutT) template class PCL_EXPORTS pcl::IntensityGradientEstimation<InT,NT,OutT>;
239
240