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 (%lu) differs than number of target indices (%lu)!\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);
94 Eigen::Vector4f p_tr (transform * pt_src);
97 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
98 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
107 distances[i] = std::sqrt ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
108 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
109 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
110 (uv[1] - target_->points[(*indices_tgt_)[i]].v));
115 template <
typename Po
intT>
void
118 if (indices_->size () != indices_tgt_->size ())
120 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
126 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n");
130 double thresh = threshold * threshold;
133 inliers.resize (indices_->size ());
134 error_sqr_dists_.resize (indices_->size ());
136 Eigen::Matrix4f transform;
137 transform.row (0).matrix () = model_coefficients.segment<4>(0);
138 transform.row (1).matrix () = model_coefficients.segment<4>(4);
139 transform.row (2).matrix () = model_coefficients.segment<4>(8);
140 transform.row (3).matrix () = model_coefficients.segment<4>(12);
142 for (
size_t i = 0; i < indices_->size (); ++i)
144 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
145 input_->points[(*indices_)[i]].y,
146 input_->points[(*indices_)[i]].z, 1);
148 Eigen::Vector4f p_tr (transform * pt_src);
151 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
152 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
159 double distance = ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
160 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
161 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
162 (uv[1] - target_->points[(*indices_tgt_)[i]].v));
167 inliers[nr_p] = (*indices_)[i];
172 inliers.resize (nr_p);
173 error_sqr_dists_.resize (nr_p);
177 template <
typename Po
intT>
int
179 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
181 if (indices_->size () != indices_tgt_->size ())
183 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
188 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] No target dataset given!\n");
192 double thresh = threshold * threshold;
194 Eigen::Matrix4f transform;
195 transform.row (0).matrix () = model_coefficients.segment<4>(0);
196 transform.row (1).matrix () = model_coefficients.segment<4>(4);
197 transform.row (2).matrix () = model_coefficients.segment<4>(8);
198 transform.row (3).matrix () = model_coefficients.segment<4>(12);
202 for (
size_t i = 0; i < indices_->size (); ++i)
204 Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
205 input_->points[(*indices_)[i]].y,
206 input_->points[(*indices_)[i]].z, 1);
208 Eigen::Vector4f p_tr (transform * pt_src);
211 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
212 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
220 if (((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
221 (uv[0] - target_->points[(*indices_tgt_)[i]].u) +
222 (uv[1] - target_->points[(*indices_tgt_)[i]].v) *
223 (uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh)
229 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_