Point Cloud Library (PCL)  1.14.1-dev
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 //////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointInT, typename PointOutT>
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////
50 template <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 //////////////////////////////////////////////////////////////////////////////////////////////
152 template <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.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud.