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/point_operators.h>
43 #include <pcl/common/eigen.h>
46 template <
typename Po
intT>
bool
63 template <
typename Po
intT>
void
66 PCL_INFO (
"[pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel]\n");
67 if (indices_->size () != indices_tgt_->size ())
69 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
75 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n");
79 distances.resize (indices_->size ());
82 Eigen::Matrix4f transform;
83 transform.row (0).matrix () = model_coefficients.segment<4>(0);
84 transform.row (1).matrix () = model_coefficients.segment<4>(4);
85 transform.row (2).matrix () = model_coefficients.segment<4>(8);
86 transform.row (3).matrix () = model_coefficients.segment<4>(12);
88 for (
size_t i = 0; i < indices_->size (); ++i)
90 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
91 input_->points[(*indices_)[i]].y,
92 input_->points[(*indices_)[i]].z, 1);
93 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
94 target_->points[(*indices_tgt_)[i]].y,
95 target_->points[(*indices_tgt_)[i]].z, 1);
97 Eigen::Vector4f p_tr (transform * pt_src);
100 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
101 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
110 distances[i] = std::sqrt ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
111 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
112 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
113 (uv[1] - target_->points[(*indices_tgt_)[i]].v));
118 template <
typename Po
intT>
void
121 if (indices_->size () != indices_tgt_->size ())
123 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
129 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n");
133 double thresh = threshold * threshold;
136 inliers.resize (indices_->size ());
137 error_sqr_dists_.resize (indices_->size ());
139 Eigen::Matrix4f transform;
140 transform.row (0).matrix () = model_coefficients.segment<4>(0);
141 transform.row (1).matrix () = model_coefficients.segment<4>(4);
142 transform.row (2).matrix () = model_coefficients.segment<4>(8);
143 transform.row (3).matrix () = model_coefficients.segment<4>(12);
145 for (
size_t i = 0; i < indices_->size (); ++i)
147 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
148 input_->points[(*indices_)[i]].y,
149 input_->points[(*indices_)[i]].z, 1);
150 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
151 target_->points[(*indices_tgt_)[i]].y,
152 target_->points[(*indices_tgt_)[i]].z, 1);
154 Eigen::Vector4f p_tr (transform * pt_src);
157 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
158 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
165 double distance = ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
166 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
167 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
168 (uv[1] - target_->points[(*indices_tgt_)[i]].v));
171 if (distance < thresh)
173 inliers[nr_p] = (*indices_)[i];
174 error_sqr_dists_[nr_p] = distance;
178 inliers.resize (nr_p);
179 error_sqr_dists_.resize (nr_p);
183 template <
typename Po
intT>
int
185 const Eigen::VectorXf &model_coefficients,
const double threshold)
187 if (indices_->size () != indices_tgt_->size ())
189 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
194 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] No target dataset given!\n");
198 double thresh = threshold * threshold;
200 Eigen::Matrix4f transform;
201 transform.row (0).matrix () = model_coefficients.segment<4>(0);
202 transform.row (1).matrix () = model_coefficients.segment<4>(4);
203 transform.row (2).matrix () = model_coefficients.segment<4>(8);
204 transform.row (3).matrix () = model_coefficients.segment<4>(12);
208 for (
size_t i = 0; i < indices_->size (); ++i)
210 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
211 input_->points[(*indices_)[i]].y,
212 input_->points[(*indices_)[i]].z, 1);
213 Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
214 target_->points[(*indices_tgt_)[i]].y,
215 target_->points[(*indices_tgt_)[i]].z, 1);
217 Eigen::Vector4f p_tr (transform * pt_src);
220 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
221 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
229 if (((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
230 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
231 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
232 (uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh)
238 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the transformed points to their correspondences.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.