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>
47template <
typename Po
intT>
bool
52 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n",
samples.
size (), sample_size_);
68template <
typename Po
intT>
bool
73 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
83 for (
int i = 0; i < 3; ++i)
85 const auto it = correspondences_.find (
samples[i]);
86 if (
it == correspondences_.cend ())
88 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeModelCoefficients] Element with key %i is not in map (map contains %lu elements).\n",
100template <
typename Po
intT>
void
103 if (indices_->size () != indices_tgt_->size ())
105 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
111 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
120 distances.resize (indices_->size ());
123 Eigen::Matrix4f transform;
129 for (std::size_t i = 0; i < indices_->size (); ++i)
131 Eigen::Vector4f
pt_src ((*input_)[(*indices_)[i]].x,
132 (*input_)[(*indices_)[i]].y,
133 (*input_)[(*indices_)[i]].z, 1.0f);
134 Eigen::Vector4f
pt_tgt ((*target_)[(*indices_tgt_)[i]].x,
135 (*target_)[(*indices_tgt_)[i]].y,
136 (*target_)[(*indices_tgt_)[i]].z, 1.0f);
146template <
typename Po
intT>
void
149 if (indices_->size () != indices_tgt_->size ())
151 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
157 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
161 double thresh = threshold * threshold;
171 error_sqr_dists_.clear ();
172 inliers.reserve (indices_->size ());
173 error_sqr_dists_.reserve (indices_->size ());
175 Eigen::Matrix4f transform;
181 for (std::size_t i = 0; i < indices_->size (); ++i)
183 Eigen::Vector4f
pt_src ((*input_)[(*indices_)[i]].x,
184 (*input_)[(*indices_)[i]].y,
185 (*input_)[(*indices_)[i]].z, 1);
186 Eigen::Vector4f
pt_tgt ((*target_)[(*indices_tgt_)[i]].x,
187 (*target_)[(*indices_tgt_)[i]].y,
188 (*target_)[(*indices_tgt_)[i]].z, 1);
194 if (distance < thresh)
196 inliers.push_back ((*indices_)[i]);
197 error_sqr_dists_.push_back (
static_cast<double> (distance));
203template <
typename Po
intT> std::size_t
207 if (indices_->size () != indices_tgt_->size ())
209 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
214 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
218 double thresh = threshold * threshold;
226 Eigen::Matrix4f transform;
232 std::size_t
nr_p = 0;
233 for (std::size_t i = 0; i < indices_->size (); ++i)
235 Eigen::Vector4f
pt_src ((*input_)[(*indices_)[i]].x,
236 (*input_)[(*indices_)[i]].y,
237 (*input_)[(*indices_)[i]].z, 1);
238 Eigen::Vector4f
pt_tgt ((*target_)[(*indices_tgt_)[i]].x,
239 (*target_)[(*indices_tgt_)[i]].y,
240 (*target_)[(*indices_tgt_)[i]].z, 1);
251template <
typename Po
intT>
void
254 if (indices_->size () != indices_tgt_->size ())
256 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
274 if (
it == correspondences_.cend ())
276 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Element with key %i is not in map (map contains %lu elements).\n",
288template <
typename Po
intT>
void
294 Eigen::VectorXf &transform)
const
296 transform.resize (16);
322#define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.