41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
44 #include <pcl/sample_consensus/sac_model_registration.h>
45 #include <pcl/common/point_operators.h>
46 #include <pcl/common/eigen.h>
47 #include <pcl/point_types.h>
50 template <
typename Po
intT>
bool
56 PointT p10 = input_->points[samples[1]] - input_->points[samples[0]];
57 PointT p20 = input_->points[samples[2]] - input_->points[samples[0]];
58 PointT p21 = input_->points[samples[2]] - input_->points[samples[1]];
60 return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
61 (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
62 (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
66 template <
typename Po
intT>
bool
71 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
75 if (samples.size () != 3)
78 std::vector<int> indices_tgt (3);
79 for (
int i = 0; i < 3; ++i)
80 indices_tgt[i] = correspondences_.at (samples[i]);
82 estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
87 template <
typename Po
intT>
void
90 if (indices_->size () != indices_tgt_->size ())
92 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
98 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
102 if (!isModelValid (model_coefficients))
107 distances.resize (indices_->size ());
110 Eigen::Matrix4f transform;
111 transform.row (0).matrix () = model_coefficients.segment<4>(0);
112 transform.row (1).matrix () = model_coefficients.segment<4>(4);
113 transform.row (2).matrix () = model_coefficients.segment<4>(8);
114 transform.row (3).matrix () = model_coefficients.segment<4>(12);
116 for (
size_t i = 0; i < indices_->size (); ++i)
118 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
119 input_->points[(*indices_)[i]].y,
120 input_->points[(*indices_)[i]].z, 1);
121 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
122 target_->points[(*indices_tgt_)[i]].y,
123 target_->points[(*indices_tgt_)[i]].z, 1);
125 Eigen::Vector4f p_tr (transform * pt_src);
128 distances[i] = (p_tr - pt_tgt).norm ();
133 template <
typename Po
intT>
void
136 if (indices_->size () != indices_tgt_->size ())
138 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
144 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
148 double thresh = threshold * threshold;
151 if (!isModelValid (model_coefficients))
158 inliers.resize (indices_->size ());
159 error_sqr_dists_.resize (indices_->size ());
161 Eigen::Matrix4f transform;
162 transform.row (0).matrix () = model_coefficients.segment<4>(0);
163 transform.row (1).matrix () = model_coefficients.segment<4>(4);
164 transform.row (2).matrix () = model_coefficients.segment<4>(8);
165 transform.row (3).matrix () = model_coefficients.segment<4>(12);
167 for (
size_t i = 0; i < indices_->size (); ++i)
169 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
170 input_->points[(*indices_)[i]].y,
171 input_->points[(*indices_)[i]].z, 1);
172 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
173 target_->points[(*indices_tgt_)[i]].y,
174 target_->points[(*indices_tgt_)[i]].z, 1);
176 Eigen::Vector4f p_tr (transform * pt_src);
178 float distance = (p_tr - pt_tgt).squaredNorm ();
180 if (distance < thresh)
182 inliers[nr_p] = (*indices_)[i];
183 error_sqr_dists_[nr_p] =
static_cast<double> (distance);
187 inliers.resize (nr_p);
188 error_sqr_dists_.resize (nr_p);
192 template <
typename Po
intT>
int
194 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
196 if (indices_->size () != indices_tgt_->size ())
198 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
203 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
207 double thresh = threshold * threshold;
210 if (!isModelValid (model_coefficients))
213 Eigen::Matrix4f transform;
214 transform.row (0).matrix () = model_coefficients.segment<4>(0);
215 transform.row (1).matrix () = model_coefficients.segment<4>(4);
216 transform.row (2).matrix () = model_coefficients.segment<4>(8);
217 transform.row (3).matrix () = model_coefficients.segment<4>(12);
220 for (
size_t i = 0; i < indices_->size (); ++i)
222 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
223 input_->points[(*indices_)[i]].y,
224 input_->points[(*indices_)[i]].z, 1);
225 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
226 target_->points[(*indices_tgt_)[i]].y,
227 target_->points[(*indices_tgt_)[i]].z, 1);
229 Eigen::Vector4f p_tr (transform * pt_src);
231 if ((p_tr - pt_tgt).squaredNorm () < thresh)
238 template <
typename Po
intT>
void
241 if (indices_->size () != indices_tgt_->size ())
243 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
244 optimized_coefficients = model_coefficients;
249 if (!isModelValid (model_coefficients) || !target_)
251 optimized_coefficients = model_coefficients;
255 std::vector<int> indices_src (inliers.size ());
256 std::vector<int> indices_tgt (inliers.size ());
257 for (
size_t i = 0; i < inliers.size (); ++i)
259 indices_src[i] = inliers[i];
260 indices_tgt[i] = correspondences_.at (indices_src[i]);
263 estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
267 template <
typename Po
intT>
void
270 const std::vector<int> &indices_src,
272 const std::vector<int> &indices_tgt,
273 Eigen::VectorXf &transform)
const
275 transform.resize (16);
277 Eigen::Matrix<double, 3, Eigen::Dynamic> src (3, indices_src.size ());
278 Eigen::Matrix<double, 3, Eigen::Dynamic> tgt (3, indices_tgt.size ());
280 for (
size_t i = 0; i < indices_src.size (); ++i)
282 src (0, i) = cloud_src[indices_src[i]].x;
283 src (1, i) = cloud_src[indices_src[i]].y;
284 src (2, i) = cloud_src[indices_src[i]].z;
286 tgt (0, i) = cloud_tgt[indices_tgt[i]].x;
287 tgt (1, i) = cloud_tgt[indices_tgt[i]].y;
288 tgt (2, i) = cloud_tgt[indices_tgt[i]].z;
292 Eigen::Matrix4d transformation_matrix =
pcl::umeyama (src, tgt,
false);
295 transform.segment<4> (0).matrix () = transformation_matrix.cast<
float> ().row (0);
296 transform.segment<4> (4).matrix () = transformation_matrix.cast<
float> ().row (1);
297 transform.segment<4> (8).matrix () = transformation_matrix.cast<
float> ().row (2);
298 transform.segment<4> (12).matrix () = transformation_matrix.cast<
float> ().row (3);
301 #define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
303 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
Eigen::internal::umeyama_transform_matrix_type< Derived, OtherDerived >::type umeyama(const Eigen::MatrixBase< Derived > &src, const Eigen::MatrixBase< OtherDerived > &dst, bool with_scaling=false)
Returns the transformation between two point sets.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Compute a 4x4 rigid transformation matrix from the samples given.
void estimateRigidTransformationSVD(const pcl::PointCloud< PointT > &cloud_src, const std::vector< int > &indices_src, const pcl::PointCloud< PointT > &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::VectorXf &transform) const
Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form so...
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the 4x4 transformation using the given inlier set.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the transformed points to their correspondences.