39 #ifndef PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_
40 #define PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_
41 #define EIGEN_II_METHOD 1
43 #include <pcl/features/linear_least_squares_normal.h>
46 template <
typename Po
intInT,
typename Po
intOutT>
52 template <
typename Po
intInT,
typename Po
intOutT>
void
54 const int pos_x,
const int pos_y, PointOutT &normal)
56 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
58 const int width = input_->width;
59 const int height = input_->height;
64 const int index = y * width + x;
66 const float px = (*input_)[index].x;
67 const float py = (*input_)[index].y;
68 const float pz = (*input_)[index].z;
72 normal.normal_x = bad_point;
73 normal.normal_y = bad_point;
74 normal.normal_z = bad_point;
75 normal.curvature = bad_point;
80 float smoothingSize = normal_smoothing_size_;
81 if (use_depth_dependent_smoothing_) smoothingSize = smoothingSize*(pz+0.5f);
83 const int smoothingSizeInt =
static_cast<int> (smoothingSize);
92 for (
int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
94 for (
int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
96 if (u < 0 || u >= width || v < 0 || v >= height)
continue;
98 const int index2 = v * width + u;
100 const float qx = (*input_)[index2].x;
101 const float qy = (*input_)[index2].y;
102 const float qz = (*input_)[index2].z;
104 if (std::isnan (qx))
continue;
106 const float delta = qz - pz;
107 const float i = qx - px;
108 const float j = qy - py;
110 float depthChangeThreshold = pz*pz * 0.05f * max_depth_change_factor_;
111 if (use_depth_dependent_smoothing_) depthChangeThreshold *= pz;
113 const float f = std::fabs (delta) > depthChangeThreshold ? 0 : 1;
118 vecb0 += f * i * delta;
119 vecb1 += f * j * delta;
123 const float det = matA0 * matA3 - matA1 * matA1;
124 const float ddx = matA3 * vecb0 - matA1 * vecb1;
125 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
127 const float nx = ddx;
128 const float ny = ddy;
129 const float nz = -det * pz;
131 const float length = nx * nx + ny * ny + nz * nz;
135 normal.normal_x = bad_point;
136 normal.normal_y = bad_point;
137 normal.normal_z = bad_point;
138 normal.curvature = bad_point;
142 const float normInv = 1.0f / std::sqrt (length);
144 normal.normal_x = -nx * normInv;
145 normal.normal_y = -ny * normInv;
146 normal.normal_z = -nz * normInv;
147 normal.curvature = bad_point;
154 template <
typename Po
intInT,
typename Po
intOutT>
void
157 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
159 const int width = input_->width;
160 const int height = input_->height;
175 for (
int y = 0; y < height; ++y)
177 for (
int x = 0; x < width; ++x)
179 const int index = y * width + x;
181 const float px = (*input_)[index].x;
182 const float py = (*input_)[index].y;
183 const float pz = (*input_)[index].z;
185 if (std::isnan(px))
continue;
189 float smoothingSize = normal_smoothing_size_;
194 const int smoothingSizeInt =
static_cast<int>(smoothingSize);
203 for (
int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
205 for (
int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
207 if (u < 0 || u >= width || v < 0 || v >= height)
continue;
209 const int index2 = v * width + u;
211 const float qx = (*input_)[index2].x;
212 const float qy = (*input_)[index2].y;
213 const float qz = (*input_)[index2].z;
215 if (std::isnan(qx))
continue;
217 const float delta = qz - pz;
218 const float i = qx - px;
219 const float j = qy - py;
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;
231 vecb0 += f * i * delta;
232 vecb1 += f * j * delta;
236 const float det = matA0 * matA3 - matA1 * matA1;
237 const float ddx = matA3 * vecb0 - matA1 * vecb1;
238 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
240 const float nx = ddx;
241 const float ny = ddy;
242 const float nz = -det * pz;
244 const float length = nx * nx + ny * ny + nz * nz;
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;
255 const float normInv = 1.0f / std::sqrt (length);
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;
266 #define PCL_INSTANTIATE_LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation<T,NT>;
void computePointNormal(const int pos_x, const int pos_y, PointOutT &normal)
Computes the normal at the specified position.
~LinearLeastSquaresNormalEstimation()
Destructor.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud.