38 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
39 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
41 #include <pcl/sample_consensus/sac_model_registration_2d.h>
42 #include <pcl/common/eigen.h>
45 template <
typename Po
intT>
bool
48 if (samples.size () != sample_size_)
50 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
67 template <
typename Po
intT>
void
70 PCL_INFO (
"[pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel]\n");
71 if (indices_->size () != indices_tgt_->size ())
73 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
79 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n");
83 distances.resize (indices_->size ());
86 Eigen::Matrix4f transform;
87 transform.row (0).matrix () = model_coefficients.segment<4>(0);
88 transform.row (1).matrix () = model_coefficients.segment<4>(4);
89 transform.row (2).matrix () = model_coefficients.segment<4>(8);
90 transform.row (3).matrix () = model_coefficients.segment<4>(12);
92 for (std::size_t i = 0; i < indices_->size (); ++i)
94 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
95 input_->points[(*indices_)[i]].y,
96 input_->points[(*indices_)[i]].z, 1.0f);
98 Eigen::Vector4f p_tr (transform * pt_src);
101 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
102 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
113 distances[i] = std::sqrt ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
114 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
115 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
116 (uv[1] - target_->points[(*indices_tgt_)[i]].v));
121 template <
typename Po
intT>
void
124 if (indices_->size () != indices_tgt_->size ())
126 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
132 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n");
136 double thresh = threshold * threshold;
139 error_sqr_dists_.clear ();
140 inliers.reserve (indices_->size ());
141 error_sqr_dists_.reserve (indices_->size ());
143 Eigen::Matrix4f transform;
144 transform.row (0).matrix () = model_coefficients.segment<4>(0);
145 transform.row (1).matrix () = model_coefficients.segment<4>(4);
146 transform.row (2).matrix () = model_coefficients.segment<4>(8);
147 transform.row (3).matrix () = model_coefficients.segment<4>(12);
149 for (std::size_t i = 0; i < indices_->size (); ++i)
151 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
152 input_->points[(*indices_)[i]].y,
153 input_->points[(*indices_)[i]].z, 1.0f);
155 Eigen::Vector4f p_tr (transform * pt_src);
158 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
159 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
166 double distance = ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
167 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
168 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
169 (uv[1] - target_->points[(*indices_tgt_)[i]].v));
172 if (distance < thresh)
174 inliers.push_back ((*indices_)[i]);
175 error_sqr_dists_.push_back (distance);
181 template <
typename Po
intT> std::size_t
183 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
185 if (indices_->size () != indices_tgt_->size ())
187 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
192 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] No target dataset given!\n");
196 double thresh = threshold * threshold;
198 Eigen::Matrix4f transform;
199 transform.row (0).matrix () = model_coefficients.segment<4>(0);
200 transform.row (1).matrix () = model_coefficients.segment<4>(4);
201 transform.row (2).matrix () = model_coefficients.segment<4>(8);
202 transform.row (3).matrix () = model_coefficients.segment<4>(12);
204 std::size_t nr_p = 0;
206 for (std::size_t i = 0; i < indices_->size (); ++i)
208 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
209 input_->points[(*indices_)[i]].y,
210 input_->points[(*indices_)[i]].z, 1.0f);
212 Eigen::Vector4f p_tr (transform * pt_src);
215 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
216 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
226 if (((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
227 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
228 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
229 (uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh)
237 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
Select all the points which respect the given model coefficients as inliers.
bool isSampleGood(const Indices &samples) const
Check if a sample of indices results in a good sample of points indices.
virtual std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the transformed points to their correspondences.