52 const pcl::PointCloud <PointInT> &cloud,
const pcl::Indices &indices,
53 const Eigen::Vector3f &point,
float mean_intensity,
const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
55 if (indices.size () < 3)
57 gradient[0] = gradient[1] = gradient[2] = std::numeric_limits<float>::quiet_NaN ();
61 Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
62 Eigen::Vector3f b = Eigen::Vector3f::Zero ();
64 for (
const auto &nn_index : indices)
66 PointInT p = cloud[nn_index];
67 if (!std::isfinite (p.x) ||
68 !std::isfinite (p.y) ||
69 !std::isfinite (p.z) ||
70 !std::isfinite (intensity_ (p)))
76 intensity_.demean (p, mean_intensity);
78 A (0, 0) += p.x * p.x;
79 A (0, 1) += p.x * p.y;
80 A (0, 2) += p.x * p.z;
82 A (1, 1) += p.y * p.y;
83 A (1, 2) += p.y * p.z;
85 A (2, 2) += p.z * p.z;
87 b[0] += p.x * intensity_ (p);
88 b[1] += p.y * intensity_ (p);
89 b[2] += p.z * intensity_ (p);
98 Eigen::Vector3f eigen_values;
99 Eigen::Matrix3f eigen_vectors;
100 eigen33 (A, eigen_vectors, eigen_values);
102 b = eigen_vectors.transpose () * b;
104 if ( eigen_values (0) != 0)
105 b (0) /= eigen_values (0);
109 if ( eigen_values (1) != 0)
110 b (1) /= eigen_values (1);
114 if ( eigen_values (2) != 0)
115 b (2) /= eigen_values (2);
120 Eigen::Vector3f x = eigen_vectors * b;
139 gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x;
149 std::vector<float> nn_dists (k_);
150 output.is_dense =
true;
154 threads_ = omp_get_num_procs();
155 PCL_DEBUG (
"[pcl::IntensityGradientEstimation::computeFeature] Setting number of threads to %u.\n", threads_);
160 if (surface_->is_dense)
162#pragma omp parallel for \
165 firstprivate(nn_indices, nn_dists) \
166 num_threads(threads_)
168 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
170 PointOutT &p_out = output[idx];
172 if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
174 p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
175 output.is_dense =
false;
179 Eigen::Vector3f centroid;
180 float mean_intensity = 0;
183 for (
const auto &nn_index : nn_indices)
185 centroid += (*surface_)[nn_index].getVector3fMap ();
186 mean_intensity += intensity_ ((*surface_)[nn_index]);
188 centroid /=
static_cast<float> (nn_indices.size ());
189 mean_intensity /=
static_cast<float> (nn_indices.size ());
191 Eigen::Vector3f normal = Eigen::Vector3f::Map ((*normals_)[(*indices_) [idx]].normal);
192 Eigen::Vector3f gradient;
193 computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
195 p_out.gradient[0] = gradient[0];
196 p_out.gradient[1] = gradient[1];
197 p_out.gradient[2] = gradient[2];
202#pragma omp parallel for \
205 firstprivate(nn_indices, nn_dists) \
206 num_threads(threads_)
208 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
210 PointOutT &p_out = output[idx];
211 if (!
isFinite ((*surface_) [(*indices_)[idx]]) ||
212 !this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
214 p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
215 output.is_dense =
false;
218 Eigen::Vector3f centroid;
219 float mean_intensity = 0;
223 for (
const auto &nn_index : nn_indices)
226 if (!
isFinite ((*surface_) [nn_index]))
229 centroid += surface_->points [nn_index].getVector3fMap ();
230 mean_intensity += intensity_ (surface_->points [nn_index]);
233 centroid /=
static_cast<float> (cp);
234 mean_intensity /=
static_cast<float> (cp);
235 Eigen::Vector3f normal = Eigen::Vector3f::Map ((*normals_)[(*indices_) [idx]].normal);
236 Eigen::Vector3f gradient;
237 computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
239 p_out.gradient[0] = gradient[0];
240 p_out.gradient[1] = gradient[1];
241 p_out.gradient[2] = gradient[2];
void computePointIntensityGradient(const pcl::PointCloud< PointInT > &cloud, const pcl::Indices &indices, const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
Estimate the intensity gradient around a given point based on its spatial neighborhood of points.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...