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>
48 template <
typename Po
intT>
bool
51 if (samples.size () != sample_size_)
53 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
59 PointT p10 = (*input_)[samples[1]] - (*input_)[samples[0]];
60 PointT p20 = (*input_)[samples[2]] - (*input_)[samples[0]];
61 PointT p21 = (*input_)[samples[2]] - (*input_)[samples[1]];
63 return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
64 (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
65 (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
69 template <
typename Po
intT>
bool
74 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
78 if (samples.size () != sample_size_)
84 for (
int i = 0; i < 3; ++i)
86 indices_tgt[i] = correspondences_.at (samples[i]);
89 estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
94 template <
typename Po
intT>
void
97 if (indices_->size () != indices_tgt_->size ())
99 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
105 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
109 if (!isModelValid (model_coefficients))
114 distances.resize (indices_->size ());
117 Eigen::Matrix4f transform;
118 transform.row (0).matrix () = model_coefficients.segment<4>(0);
119 transform.row (1).matrix () = model_coefficients.segment<4>(4);
120 transform.row (2).matrix () = model_coefficients.segment<4>(8);
121 transform.row (3).matrix () = model_coefficients.segment<4>(12);
123 for (std::size_t i = 0; i < indices_->size (); ++i)
125 Eigen::Vector4f pt_src ((*input_)[(*indices_)[i]].x,
126 (*input_)[(*indices_)[i]].y,
127 (*input_)[(*indices_)[i]].z, 1.0f);
128 Eigen::Vector4f pt_tgt ((*target_)[(*indices_tgt_)[i]].x,
129 (*target_)[(*indices_tgt_)[i]].y,
130 (*target_)[(*indices_tgt_)[i]].z, 1.0f);
132 Eigen::Vector4f p_tr (transform * pt_src);
135 distances[i] = (p_tr - pt_tgt).norm ();
140 template <
typename Po
intT>
void
143 if (indices_->size () != indices_tgt_->size ())
145 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
151 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
155 double thresh = threshold * threshold;
158 if (!isModelValid (model_coefficients))
165 error_sqr_dists_.clear ();
166 inliers.reserve (indices_->size ());
167 error_sqr_dists_.reserve (indices_->size ());
169 Eigen::Matrix4f transform;
170 transform.row (0).matrix () = model_coefficients.segment<4>(0);
171 transform.row (1).matrix () = model_coefficients.segment<4>(4);
172 transform.row (2).matrix () = model_coefficients.segment<4>(8);
173 transform.row (3).matrix () = model_coefficients.segment<4>(12);
175 for (std::size_t i = 0; i < indices_->size (); ++i)
177 Eigen::Vector4f pt_src ((*input_)[(*indices_)[i]].x,
178 (*input_)[(*indices_)[i]].y,
179 (*input_)[(*indices_)[i]].z, 1);
180 Eigen::Vector4f pt_tgt ((*target_)[(*indices_tgt_)[i]].x,
181 (*target_)[(*indices_tgt_)[i]].y,
182 (*target_)[(*indices_tgt_)[i]].z, 1);
184 Eigen::Vector4f p_tr (transform * pt_src);
186 float distance = (p_tr - pt_tgt).squaredNorm ();
190 inliers.push_back ((*indices_)[i]);
191 error_sqr_dists_.push_back (
static_cast<double> (
distance));
197 template <
typename Po
intT> std::size_t
199 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
201 if (indices_->size () != indices_tgt_->size ())
203 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
208 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
212 double thresh = threshold * threshold;
215 if (!isModelValid (model_coefficients))
220 Eigen::Matrix4f transform;
221 transform.row (0).matrix () = model_coefficients.segment<4>(0);
222 transform.row (1).matrix () = model_coefficients.segment<4>(4);
223 transform.row (2).matrix () = model_coefficients.segment<4>(8);
224 transform.row (3).matrix () = model_coefficients.segment<4>(12);
226 std::size_t nr_p = 0;
227 for (std::size_t i = 0; i < indices_->size (); ++i)
229 Eigen::Vector4f pt_src ((*input_)[(*indices_)[i]].x,
230 (*input_)[(*indices_)[i]].y,
231 (*input_)[(*indices_)[i]].z, 1);
232 Eigen::Vector4f pt_tgt ((*target_)[(*indices_tgt_)[i]].x,
233 (*target_)[(*indices_tgt_)[i]].y,
234 (*target_)[(*indices_tgt_)[i]].z, 1);
236 Eigen::Vector4f p_tr (transform * pt_src);
238 if ((p_tr - pt_tgt).squaredNorm () < thresh)
245 template <
typename Po
intT>
void
248 if (indices_->size () != indices_tgt_->size ())
250 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
251 optimized_coefficients = model_coefficients;
256 if (!isModelValid (model_coefficients) || !target_)
258 optimized_coefficients = model_coefficients;
262 Indices indices_src (inliers.size ());
263 Indices indices_tgt (inliers.size ());
264 for (std::size_t i = 0; i < inliers.size (); ++i)
266 indices_src[i] = inliers[i];
267 indices_tgt[i] = correspondences_.at (indices_src[i]);
270 estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
274 template <
typename Po
intT>
void
280 Eigen::VectorXf &transform)
const
282 transform.resize (16);
284 Eigen::Matrix<double, 3, Eigen::Dynamic> src (3, indices_src.size ());
285 Eigen::Matrix<double, 3, Eigen::Dynamic> tgt (3, indices_tgt.size ());
287 for (std::size_t i = 0; i < indices_src.size (); ++i)
289 src (0, i) = cloud_src[indices_src[i]].x;
290 src (1, i) = cloud_src[indices_src[i]].y;
291 src (2, i) = cloud_src[indices_src[i]].z;
293 tgt (0, i) = cloud_tgt[indices_tgt[i]].x;
294 tgt (1, i) = cloud_tgt[indices_tgt[i]].y;
295 tgt (2, i) = cloud_tgt[indices_tgt[i]].z;
299 Eigen::Matrix4d transformation_matrix =
pcl::umeyama (src, tgt,
false);
302 transform.segment<4> (0).matrix () = transformation_matrix.cast<
float> ().row (0);
303 transform.segment<4> (4).matrix () = transformation_matrix.cast<
float> ().row (1);
304 transform.segment<4> (8).matrix () = transformation_matrix.cast<
float> ().row (2);
305 transform.segment<4> (12).matrix () = transformation_matrix.cast<
float> ().row (3);
308 #define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
310 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_