41 #ifndef PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
42 #define PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
44 #include <pcl/common/eigen.h>
45 #include <pcl/filters/covariance_sampling.h>
49 template<
typename Po
intT,
typename Po
intNT>
bool
55 if (num_samples_ > indices_->size ())
57 PCL_ERROR (
"[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%zu)\n",
58 num_samples_, indices_->size ());
64 Eigen::Vector3f centroid (0.f, 0.f, 0.f);
65 for (
size_t p_i = 0; p_i < indices_->size (); ++p_i)
66 centroid += (*input_)[(*indices_)[p_i]].getVector3fMap ();
67 centroid /= float (indices_->size ());
69 scaled_points_.resize (indices_->size ());
70 double average_norm = 0.0;
71 for (
size_t p_i = 0; p_i < indices_->size (); ++p_i)
73 scaled_points_[p_i] = (*input_)[(*indices_)[p_i]].getVector3fMap () - centroid;
74 average_norm += scaled_points_[p_i].norm ();
76 average_norm /= double (scaled_points_.size ());
77 for (
size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
78 scaled_points_[p_i] /=
float (average_norm);
84 template<
typename Po
intT,
typename Po
intNT>
double
87 Eigen::Matrix<double, 6, 6> covariance_matrix;
91 Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
92 eigen_solver.compute (covariance_matrix,
true);
94 Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues ();
96 double max_ev = std::numeric_limits<double>::min (),
97 min_ev = std::numeric_limits<double>::max ();
98 for (
size_t i = 0; i < 6; ++i)
100 if (real (complex_eigenvalues (i, 0)) > max_ev)
101 max_ev = real (complex_eigenvalues (i, 0));
103 if (real (complex_eigenvalues (i, 0)) < min_ev)
104 min_ev = real (complex_eigenvalues (i, 0));
107 return (max_ev / min_ev);
112 template<
typename Po
intT,
typename Po
intNT>
double
115 Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
116 eigen_solver.compute (covariance_matrix,
true);
118 Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues ();
120 double max_ev = std::numeric_limits<double>::min (),
121 min_ev = std::numeric_limits<double>::max ();
122 for (
size_t i = 0; i < 6; ++i)
124 if (real (complex_eigenvalues (i, 0)) > max_ev)
125 max_ev = real (complex_eigenvalues (i, 0));
127 if (real (complex_eigenvalues (i, 0)) < min_ev)
128 min_ev = real (complex_eigenvalues (i, 0));
131 return (max_ev / min_ev);
136 template<
typename Po
intT,
typename Po
intNT>
bool
144 Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
145 for (
size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
147 f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
148 (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).
template cast<double> ();
149 f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
153 covariance_matrix = f_mat * f_mat.transpose ();
158 template<
typename Po
intT,
typename Po
intNT>
void
166 Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
167 for (
size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
169 f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
170 (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).
template cast<double> ();
171 f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
175 Eigen::Matrix<double, 6, 6> c_mat (f_mat * f_mat.transpose ());
177 Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
178 eigen_solver.compute (c_mat,
true);
179 Eigen::MatrixXcd complex_eigenvectors = eigen_solver.eigenvectors ();
181 Eigen::Matrix<double, 6, 6> x;
182 for (
size_t i = 0; i < 6; ++i)
183 for (
size_t j = 0; j < 6; ++j)
184 x (i, j) = real (complex_eigenvectors (i, j));
189 std::vector<size_t> candidate_indices;
190 candidate_indices.resize (indices_->size ());
191 for (
size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
192 candidate_indices[p_i] = p_i;
195 typedef Eigen::Matrix<double, 6, 1> Vector6d;
196 std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
197 v.resize (candidate_indices.size ());
198 for (
size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
200 v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
201 (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).
template cast<double> ();
202 v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
207 std::vector<std::list<std::pair<int, double> > > L;
210 for (
size_t i = 0; i < 6; ++i)
212 for (
size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
213 L[i].push_back (std::make_pair (p_i, fabs (v[p_i].dot (x.block<6, 1> (0, i)))));
216 L[i].sort (sort_dot_list_function);
220 std::vector<double> t (6, 0.0);
222 sampled_indices.resize (num_samples_);
223 std::vector<bool> point_sampled (candidate_indices.size (),
false);
225 for (
size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
229 for (
size_t i = 0; i < 6; ++i)
231 if (t[min_t_i] > t[i])
236 while (point_sampled [L[min_t_i].front ().first])
237 L[min_t_i].pop_front ();
239 sampled_indices[sample_i] = L[min_t_i].front ().first;
240 point_sampled[L[min_t_i].front ().first] =
true;
241 L[min_t_i].pop_front ();
244 for (
size_t i = 0; i < 6; ++i)
246 double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
252 for (
size_t i = 0; i < sampled_indices.size (); ++i)
253 sampled_indices[i] = (*indices_)[candidate_indices[sampled_indices[i]]];
258 template<
typename Po
intT,
typename Po
intNT>
void
261 std::vector<int> sampled_indices;
262 applyFilter (sampled_indices);
264 output.
resize (sampled_indices.size ());
265 output.
header = input_->header;
267 output.
width = uint32_t (output.
size ());
269 for (
size_t i = 0; i < sampled_indices.size (); ++i)
270 output[i] = (*input_)[sampled_indices[i]];
274 #define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling<T,NT>;
pcl::PCLHeader header
The point cloud header.
void resize(size_t n)
Resize the cloud.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
FilterIndices represents the base class for filters that are about binary point removal.
uint32_t width
The point cloud width (if organized as an image-structure).
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void applyFilter(Cloud &output)
Sample of point indices into a separate PointCloud.
uint32_t height
The point cloud height (if organized as an image-structure).
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
double computeConditionNumber()
Compute the condition number of the input point cloud.
PointCloud represents the base class in PCL for storing collections of 3D points. ...