Point Cloud Library (PCL) 1.12.0
linear_least_squares_normal.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 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 */
38
39#ifndef PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_
40#define PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_
41#define EIGEN_II_METHOD 1
42
43#include <pcl/features/linear_least_squares_normal.h>
44
45//////////////////////////////////////////////////////////////////////////////////////////////
46template <typename PointInT, typename PointOutT>
48{
49}
50
51//////////////////////////////////////////////////////////////////////////////////////////////
52template <typename PointInT, typename PointOutT> void
54 const int pos_x, const int pos_y, PointOutT &normal)
55{
56 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
57
58 const int width = input_->width;
59 const int height = input_->height;
60
61 const int x = pos_x;
62 const int y = pos_y;
63
64 const int index = y * width + x;
65
66 const float px = (*input_)[index].x;
67 const float py = (*input_)[index].y;
68 const float pz = (*input_)[index].z;
69
70 if (std::isnan (px))
71 {
72 normal.normal_x = bad_point;
73 normal.normal_y = bad_point;
74 normal.normal_z = bad_point;
75 normal.curvature = bad_point;
76
77 return;
78 }
79
80 float smoothingSize = normal_smoothing_size_;
81 if (use_depth_dependent_smoothing_) smoothingSize = smoothingSize*(pz+0.5f);
82
83 const int smoothingSizeInt = static_cast<int> (smoothingSize);
84
85 float matA0 = 0.0f;
86 float matA1 = 0.0f;
87 float matA3 = 0.0f;
88
89 float vecb0 = 0.0f;
90 float vecb1 = 0.0f;
91
92 for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
93 {
94 for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
95 {
96 if (u < 0 || u >= width || v < 0 || v >= height) continue;
97
98 const int index2 = v * width + u;
99
100 const float qx = (*input_)[index2].x;
101 const float qy = (*input_)[index2].y;
102 const float qz = (*input_)[index2].z;
103
104 if (std::isnan (qx)) continue;
105
106 const float delta = qz - pz;
107 const float i = qx - px;
108 const float j = qy - py;
109
110 float depthChangeThreshold = pz*pz * 0.05f * max_depth_change_factor_;
111 if (use_depth_dependent_smoothing_) depthChangeThreshold *= pz;
112
113 const float f = std::fabs (delta) > depthChangeThreshold ? 0 : 1;
114
115 matA0 += f * i * i;
116 matA1 += f * i * j;
117 matA3 += f * j * j;
118 vecb0 += f * i * delta;
119 vecb1 += f * j * delta;
120 }
121 }
122
123 const float det = matA0 * matA3 - matA1 * matA1;
124 const float ddx = matA3 * vecb0 - matA1 * vecb1;
125 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
126
127 const float nx = ddx;
128 const float ny = ddy;
129 const float nz = -det * pz;
130
131 const float length = nx * nx + ny * ny + nz * nz;
132
133 if (length <= 0.01f)
134 {
135 normal.normal_x = bad_point;
136 normal.normal_y = bad_point;
137 normal.normal_z = bad_point;
138 normal.curvature = bad_point;
139 }
140 else
141 {
142 const float normInv = 1.0f / std::sqrt (length);
143
144 normal.normal_x = -nx * normInv;
145 normal.normal_y = -ny * normInv;
146 normal.normal_z = -nz * normInv;
147 normal.curvature = bad_point;
148 }
149
150 return;
151}
152
153//////////////////////////////////////////////////////////////////////////////////////////////
154template <typename PointInT, typename PointOutT> void
156{
157 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
158
159 const int width = input_->width;
160 const int height = input_->height;
161
162 // we compute the normals as follows:
163 // ----------------------------------
164 //
165 // for the depth-gradient you can make the following first-order Taylor approximation:
166 // D(x + dx) - D(x) = dx^T \Delta D + h.o.t.
167 //
168 // build linear system by stacking up equation for 8 neighbor points:
169 // Y = X \Delta D
170 //
171 // => \Delta D = (X^T X)^{-1} X^T Y
172 // => \Delta D = (A)^{-1} b
173
174 //const float smoothingSize = 30.0f;
175 for (int y = 0; y < height; ++y)
176 {
177 for (int x = 0; x < width; ++x)
178 {
179 const int index = y * width + x;
180
181 const float px = (*input_)[index].x;
182 const float py = (*input_)[index].y;
183 const float pz = (*input_)[index].z;
184
185 if (std::isnan(px)) continue;
186
187 //float depthDependentSmoothingSize = smoothingSize + pz / 10.0f;
188
189 float smoothingSize = normal_smoothing_size_;
190 //if (use_depth_dependent_smoothing_) smoothingSize *= pz;
191 //if (use_depth_dependent_smoothing_) smoothingSize += pz*5;
192 //if (smoothingSize < 1.0f) smoothingSize += 1.0f;
193
194 const int smoothingSizeInt = static_cast<int>(smoothingSize);
195
196 float matA0 = 0.0f;
197 float matA1 = 0.0f;
198 float matA3 = 0.0f;
199
200 float vecb0 = 0.0f;
201 float vecb1 = 0.0f;
202
203 for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
204 {
205 for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
206 {
207 if (u < 0 || u >= width || v < 0 || v >= height) continue;
208
209 const int index2 = v * width + u;
210
211 const float qx = (*input_)[index2].x;
212 const float qy = (*input_)[index2].y;
213 const float qz = (*input_)[index2].z;
214
215 if (std::isnan(qx)) continue;
216
217 const float delta = qz - pz;
218 const float i = qx - px;
219 const float j = qy - py;
220
221 const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (pz) + 1.0f) * 2.0f);
222 const float f = std::fabs(delta) > depthDependendDepthChange ? 0 : 1;
223
224 //float f = std::abs(delta) > (pz * 0.05f - 0.3f) ? 0 : 1;
225 //const float f = std::abs(delta) > (pz*pz * 0.05f * max_depth_change_factor_) ? 0 : 1;
226 //float f = Math.Abs(delta) > (depth * Math.Log(depth + 1.0) * 0.02f - 0.2f) ? 0 : 1;
227
228 matA0 += f * i * i;
229 matA1 += f * i * j;
230 matA3 += f * j * j;
231 vecb0 += f * i * delta;
232 vecb1 += f * j * delta;
233 }
234 }
235
236 const float det = matA0 * matA3 - matA1 * matA1;
237 const float ddx = matA3 * vecb0 - matA1 * vecb1;
238 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
239
240 const float nx = ddx;
241 const float ny = ddy;
242 const float nz = -det * pz;
243
244 const float length = nx * nx + ny * ny + nz * nz;
245
246 if (length <= 0.0f)
247 {
248 output[index].normal_x = bad_point;
249 output[index].normal_y = bad_point;
250 output[index].normal_z = bad_point;
251 output[index].curvature = bad_point;
252 }
253 else
254 {
255 const float normInv = 1.0f / std::sqrt (length);
256
257 output[index].normal_x = nx * normInv;
258 output[index].normal_y = ny * normInv;
259 output[index].normal_z = nz * normInv;
260 output[index].curvature = bad_point;
261 }
262 }
263 }
264}
265
266#define PCL_INSTANTIATE_LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation<T,NT>;
267//#define LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation<T,NT>;
268
269#endif
270
void computePointNormal(const int pos_x, const int pos_y, PointOutT &normal)
Computes the normal at the specified position.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud.