41 #ifndef PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
42 #define PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
44 #include <pcl/filters/covariance_sampling.h>
46 #include <Eigen/Eigenvalues>
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 (%lu)\n",
58 num_samples_, indices_->size ());
64 Eigen::Vector3f centroid (0.f, 0.f, 0.f);
65 for (std::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 (std::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 (std::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 return computeConditionNumber (covariance_matrix);
96 template<
typename Po
intT,
typename Po
intNT>
double
99 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> > solver (covariance_matrix, Eigen::EigenvaluesOnly);
100 const double max_ev = solver.eigenvalues (). maxCoeff ();
101 const double min_ev = solver.eigenvalues (). minCoeff ();
102 return (max_ev / min_ev);
107 template<
typename Po
intT,
typename Po
intNT>
bool
115 Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
116 for (std::size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
118 f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
119 (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).
template cast<double> ();
120 f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
124 covariance_matrix = f_mat * f_mat.transpose ();
129 template<
typename Po
intT,
typename Po
intNT>
void
132 Eigen::Matrix<double, 6, 6> c_mat;
137 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> > solver (c_mat);
138 const Eigen::Matrix<double, 6, 6> x = solver.eigenvectors ();
142 std::vector<std::size_t> candidate_indices;
143 candidate_indices.resize (indices_->size ());
144 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
145 candidate_indices[p_i] = p_i;
148 using Vector6d = Eigen::Matrix<double, 6, 1>;
149 std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
150 v.resize (candidate_indices.size ());
151 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
153 v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
154 (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).
template cast<double> ();
155 v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
160 std::vector<std::list<std::pair<int, double> > > L;
163 for (std::size_t i = 0; i < 6; ++i)
165 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
166 L[i].push_back (std::make_pair (p_i, std::abs (v[p_i].dot (x.block<6, 1> (0, i)))));
169 L[i].sort (sort_dot_list_function);
173 std::vector<double> t (6, 0.0);
175 sampled_indices.resize (num_samples_);
176 std::vector<bool> point_sampled (candidate_indices.size (),
false);
178 for (std::size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
181 std::size_t min_t_i = 0;
182 for (std::size_t i = 0; i < 6; ++i)
184 if (t[min_t_i] > t[i])
189 while (point_sampled [L[min_t_i].front ().first])
190 L[min_t_i].pop_front ();
192 sampled_indices[sample_i] = L[min_t_i].front ().first;
193 point_sampled[L[min_t_i].front ().first] =
true;
194 L[min_t_i].pop_front ();
197 for (std::size_t i = 0; i < 6; ++i)
199 double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
205 for (
auto &sampled_index : sampled_indices)
206 sampled_index = (*indices_)[candidate_indices[sampled_index]];
211 template<
typename Po
intT,
typename Po
intNT>
void
215 applyFilter (sampled_indices);
217 output.resize (sampled_indices.size ());
218 output.header = input_->header;
220 output.width = output.size ();
221 output.is_dense =
true;
222 for (std::size_t i = 0; i < sampled_indices.size (); ++i)
223 output[i] = (*input_)[sampled_indices[i]];
227 #define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling<T,NT>;