41 #ifndef PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_
42 #define PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_
44 #include <pcl/features/intensity_gradient.h>
47 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename IntensitySelectorT>
void
50 const Eigen::Vector3f &point,
float mean_intensity,
const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
52 if (indices.size () < 3)
54 gradient[0] = gradient[1] = gradient[2] = std::numeric_limits<float>::quiet_NaN ();
58 Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
59 Eigen::Vector3f b = Eigen::Vector3f::Zero ();
61 for (
size_t i_point = 0; i_point < indices.size (); ++i_point)
63 PointInT p = cloud.points[indices[i_point]];
64 if (!pcl_isfinite (p.x) ||
65 !pcl_isfinite (p.y) ||
66 !pcl_isfinite (p.z) ||
67 !pcl_isfinite (intensity_ (p)))
73 intensity_.demean (p, mean_intensity);
75 A (0, 0) += p.x * p.x;
76 A (0, 1) += p.x * p.y;
77 A (0, 2) += p.x * p.z;
79 A (1, 1) += p.y * p.y;
80 A (1, 2) += p.y * p.z;
82 A (2, 2) += p.z * p.z;
84 b[0] += p.x * intensity_ (p);
85 b[1] += p.y * intensity_ (p);
86 b[2] += p.z * intensity_ (p);
94 Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
139 gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x;
143 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename IntensitySelectorT>
void
148 std::vector<int> nn_indices (k_);
149 std::vector<float> nn_dists (k_);
153 if (surface_->is_dense)
156 #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
159 for (
int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
161 PointOutT &p_out = output.
points[idx];
163 if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
165 p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
170 Eigen::Vector3f centroid;
171 float mean_intensity = 0;
174 for (
size_t i = 0; i < nn_indices.size (); ++i)
176 centroid += surface_->points[nn_indices[i]].getVector3fMap ();
177 mean_intensity += intensity_ (surface_->points[nn_indices[i]]);
179 centroid /=
static_cast<float> (nn_indices.size ());
180 mean_intensity /=
static_cast<float> (nn_indices.size ());
182 Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal);
183 Eigen::Vector3f gradient;
184 computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
186 p_out.gradient[0] = gradient[0];
187 p_out.gradient[1] = gradient[1];
188 p_out.gradient[2] = gradient[2];
194 #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
197 for (
int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
199 PointOutT &p_out = output.
points[idx];
200 if (!
isFinite ((*surface_) [(*indices_)[idx]]) ||
201 !this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
203 p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
207 Eigen::Vector3f centroid;
208 float mean_intensity = 0;
212 for (
size_t i = 0; i < nn_indices.size (); ++i)
215 if (!
isFinite ((*surface_) [nn_indices[i]]))
218 centroid += surface_->points [nn_indices[i]].getVector3fMap ();
219 mean_intensity += intensity_ (surface_->points [nn_indices[i]]);
222 centroid /=
static_cast<float> (cp);
223 mean_intensity /=
static_cast<float> (cp);
224 Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal);
225 Eigen::Vector3f gradient;
226 computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
228 p_out.gradient[0] = gradient[0];
229 p_out.gradient[1] = gradient[1];
230 p_out.gradient[2] = gradient[2];
235 #define PCL_INSTANTIATE_IntensityGradientEstimation(InT,NT,OutT) template class PCL_EXPORTS pcl::IntensityGradientEstimation<InT,NT,OutT>;
237 #endif // PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void computeFeature(PointCloudOut &output)
Estimate the intensity gradients for a set of points given in <setInputCloud (), setIndices ()> using...