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/eigen.h>
49 template <
typename Po
intT>
bool
52 if (samples.size () != sample_size_)
54 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
60 PointT p10 = input_->points[samples[1]] - input_->points[samples[0]];
61 PointT p20 = input_->points[samples[2]] - input_->points[samples[0]];
62 PointT p21 = input_->points[samples[2]] - input_->points[samples[1]];
64 return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
65 (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
66 (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
70 template <
typename Po
intT>
bool
75 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
79 if (samples.size () != sample_size_)
85 for (
int i = 0; i < 3; ++i)
87 indices_tgt[i] = correspondences_.at (samples[i]);
90 estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
95 template <
typename Po
intT>
void
98 if (indices_->size () != indices_tgt_->size ())
100 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
106 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
110 if (!isModelValid (model_coefficients))
115 distances.resize (indices_->size ());
118 Eigen::Matrix4f transform;
119 transform.row (0).matrix () = model_coefficients.segment<4>(0);
120 transform.row (1).matrix () = model_coefficients.segment<4>(4);
121 transform.row (2).matrix () = model_coefficients.segment<4>(8);
122 transform.row (3).matrix () = model_coefficients.segment<4>(12);
124 for (std::size_t i = 0; i < indices_->size (); ++i)
126 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
127 input_->points[(*indices_)[i]].y,
128 input_->points[(*indices_)[i]].z, 1.0f);
129 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
130 target_->points[(*indices_tgt_)[i]].y,
131 target_->points[(*indices_tgt_)[i]].z, 1.0f);
133 Eigen::Vector4f p_tr (transform * pt_src);
136 distances[i] = (p_tr - pt_tgt).norm ();
141 template <
typename Po
intT>
void
144 if (indices_->size () != indices_tgt_->size ())
146 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
152 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
156 double thresh = threshold * threshold;
159 if (!isModelValid (model_coefficients))
166 error_sqr_dists_.clear ();
167 inliers.reserve (indices_->size ());
168 error_sqr_dists_.reserve (indices_->size ());
170 Eigen::Matrix4f transform;
171 transform.row (0).matrix () = model_coefficients.segment<4>(0);
172 transform.row (1).matrix () = model_coefficients.segment<4>(4);
173 transform.row (2).matrix () = model_coefficients.segment<4>(8);
174 transform.row (3).matrix () = model_coefficients.segment<4>(12);
176 for (std::size_t i = 0; i < indices_->size (); ++i)
178 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
179 input_->points[(*indices_)[i]].y,
180 input_->points[(*indices_)[i]].z, 1);
181 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
182 target_->points[(*indices_tgt_)[i]].y,
183 target_->points[(*indices_tgt_)[i]].z, 1);
185 Eigen::Vector4f p_tr (transform * pt_src);
187 float distance = (p_tr - pt_tgt).squaredNorm ();
191 inliers.push_back ((*indices_)[i]);
192 error_sqr_dists_.push_back (
static_cast<double> (
distance));
198 template <
typename Po
intT> std::size_t
200 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
202 if (indices_->size () != indices_tgt_->size ())
204 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
209 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
213 double thresh = threshold * threshold;
216 if (!isModelValid (model_coefficients))
221 Eigen::Matrix4f transform;
222 transform.row (0).matrix () = model_coefficients.segment<4>(0);
223 transform.row (1).matrix () = model_coefficients.segment<4>(4);
224 transform.row (2).matrix () = model_coefficients.segment<4>(8);
225 transform.row (3).matrix () = model_coefficients.segment<4>(12);
227 std::size_t nr_p = 0;
228 for (std::size_t i = 0; i < indices_->size (); ++i)
230 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
231 input_->points[(*indices_)[i]].y,
232 input_->points[(*indices_)[i]].z, 1);
233 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
234 target_->points[(*indices_tgt_)[i]].y,
235 target_->points[(*indices_tgt_)[i]].z, 1);
237 Eigen::Vector4f p_tr (transform * pt_src);
239 if ((p_tr - pt_tgt).squaredNorm () < thresh)
246 template <
typename Po
intT>
void
249 if (indices_->size () != indices_tgt_->size ())
251 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
252 optimized_coefficients = model_coefficients;
257 if (!isModelValid (model_coefficients) || !target_)
259 optimized_coefficients = model_coefficients;
263 Indices indices_src (inliers.size ());
264 Indices indices_tgt (inliers.size ());
265 for (std::size_t i = 0; i < inliers.size (); ++i)
267 indices_src[i] = inliers[i];
268 indices_tgt[i] = correspondences_.at (indices_src[i]);
271 estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
275 template <
typename Po
intT>
void
281 Eigen::VectorXf &transform)
const
283 transform.resize (16);
285 Eigen::Matrix<double, 3, Eigen::Dynamic> src (3, indices_src.size ());
286 Eigen::Matrix<double, 3, Eigen::Dynamic> tgt (3, indices_tgt.size ());
288 for (std::size_t i = 0; i < indices_src.size (); ++i)
290 src (0, i) = cloud_src[indices_src[i]].x;
291 src (1, i) = cloud_src[indices_src[i]].y;
292 src (2, i) = cloud_src[indices_src[i]].z;
294 tgt (0, i) = cloud_tgt[indices_tgt[i]].x;
295 tgt (1, i) = cloud_tgt[indices_tgt[i]].y;
296 tgt (2, i) = cloud_tgt[indices_tgt[i]].z;
300 Eigen::Matrix4d transformation_matrix =
pcl::umeyama (src, tgt,
false);
303 transform.segment<4> (0).matrix () = transformation_matrix.cast<
float> ().row (0);
304 transform.segment<4> (4).matrix () = transformation_matrix.cast<
float> ().row (1);
305 transform.segment<4> (8).matrix () = transformation_matrix.cast<
float> ().row (2);
306 transform.segment<4> (12).matrix () = transformation_matrix.cast<
float> ().row (3);
309 #define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
PointCloud represents the base class in PCL for storing collections of 3D points.
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Compute a 4x4 rigid transformation matrix from the samples given.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the transformed points to their correspondences.
void estimateRigidTransformationSVD(const pcl::PointCloud< PointT > &cloud_src, const Indices &indices_src, const pcl::PointCloud< PointT > &cloud_tgt, const Indices &indices_tgt, Eigen::VectorXf &transform) const
Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form so...
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 4x4 transformation using the given inlier set.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
Defines all the PCL implemented PointT point type structures.
float distance(const PointT &p1, const PointT &p2)
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.
std::vector< index_t > Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.