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.
uint32_t height
The point cloud height (if organized as an image-structure).
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.
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.
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
void resize(size_t n)
Resize the cloud.
uint32_t width
The point cloud width (if organized as an image-structure).
double computeConditionNumber()
Compute the condition number of the input point cloud.