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>
47 template <
typename Po
intT>
bool
50 if (samples.size () != sample_size_)
52 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
58 PointT p10 = (*input_)[samples[1]] - (*input_)[samples[0]];
59 PointT p20 = (*input_)[samples[2]] - (*input_)[samples[0]];
60 PointT p21 = (*input_)[samples[2]] - (*input_)[samples[1]];
62 return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
63 (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
64 (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
68 template <
typename Po
intT>
bool
73 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
77 if (samples.size () != sample_size_)
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",
89 samples[i], correspondences_.size ());
92 indices_tgt[i] = it->second;
95 estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
100 template <
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");
115 if (!isModelValid (model_coefficients))
120 distances.resize (indices_->size ());
123 Eigen::Matrix4f transform;
124 transform.row (0).matrix () = model_coefficients.segment<4>(0);
125 transform.row (1).matrix () = model_coefficients.segment<4>(4);
126 transform.row (2).matrix () = model_coefficients.segment<4>(8);
127 transform.row (3).matrix () = model_coefficients.segment<4>(12);
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);
138 Eigen::Vector4f p_tr (transform * pt_src);
141 distances[i] = (p_tr - pt_tgt).norm ();
146 template <
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;
164 if (!isModelValid (model_coefficients))
171 error_sqr_dists_.clear ();
172 inliers.reserve (indices_->size ());
173 error_sqr_dists_.reserve (indices_->size ());
175 Eigen::Matrix4f transform;
176 transform.row (0).matrix () = model_coefficients.segment<4>(0);
177 transform.row (1).matrix () = model_coefficients.segment<4>(4);
178 transform.row (2).matrix () = model_coefficients.segment<4>(8);
179 transform.row (3).matrix () = model_coefficients.segment<4>(12);
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);
190 Eigen::Vector4f p_tr (transform * pt_src);
192 float distance = (p_tr - pt_tgt).squaredNorm ();
196 inliers.push_back ((*indices_)[i]);
197 error_sqr_dists_.push_back (
static_cast<double> (
distance));
203 template <
typename Po
intT> std::size_t
205 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
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;
221 if (!isModelValid (model_coefficients))
226 Eigen::Matrix4f transform;
227 transform.row (0).matrix () = model_coefficients.segment<4>(0);
228 transform.row (1).matrix () = model_coefficients.segment<4>(4);
229 transform.row (2).matrix () = model_coefficients.segment<4>(8);
230 transform.row (3).matrix () = model_coefficients.segment<4>(12);
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);
242 Eigen::Vector4f p_tr (transform * pt_src);
244 if ((p_tr - pt_tgt).squaredNorm () < thresh)
251 template <
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 ());
257 optimized_coefficients = model_coefficients;
262 if (!isModelValid (model_coefficients) || !target_)
264 optimized_coefficients = model_coefficients;
268 Indices indices_src (inliers.size ());
269 Indices indices_tgt (inliers.size ());
270 for (std::size_t i = 0; i < inliers.size (); ++i)
272 indices_src[i] = inliers[i];
273 const auto it = correspondences_.find (indices_src[i]);
274 if (it == correspondences_.cend ())
276 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Element with key %i is not in map (map contains %lu elements).\n",
277 indices_src[i], correspondences_.size ());
278 optimized_coefficients = model_coefficients;
281 indices_tgt[i] = it->second;
284 estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
288 template <
typename Po
intT>
void
294 Eigen::VectorXf &transform)
const
296 transform.resize (16);
298 Eigen::Matrix<double, 3, Eigen::Dynamic> src (3, indices_src.size ());
299 Eigen::Matrix<double, 3, Eigen::Dynamic> tgt (3, indices_tgt.size ());
301 for (std::size_t i = 0; i < indices_src.size (); ++i)
303 src (0, i) = cloud_src[indices_src[i]].x;
304 src (1, i) = cloud_src[indices_src[i]].y;
305 src (2, i) = cloud_src[indices_src[i]].z;
307 tgt (0, i) = cloud_tgt[indices_tgt[i]].x;
308 tgt (1, i) = cloud_tgt[indices_tgt[i]].y;
309 tgt (2, i) = cloud_tgt[indices_tgt[i]].z;
313 Eigen::Matrix4d transformation_matrix =
pcl::umeyama (src, tgt,
false);
316 transform.segment<4> (0).matrix () = transformation_matrix.cast<
float> ().row (0);
317 transform.segment<4> (4).matrix () = transformation_matrix.cast<
float> ().row (1);
318 transform.segment<4> (8).matrix () = transformation_matrix.cast<
float> ().row (2);
319 transform.segment<4> (12).matrix () = transformation_matrix.cast<
float> ().row (3);
322 #define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
324 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_