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>
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 (std::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 (std::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 ();
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> std::size_t
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);
219 std::size_t nr_p = 0;
220 for (std::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 (std::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 (std::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_