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