69 PCL_INFO (
"[pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel]\n");
70 if (indices_->size () != indices_tgt_->size ())
72 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
78 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n");
82 distances.resize (indices_->size ());
85 Eigen::Matrix4f transform;
91 for (std::size_t i = 0; i < indices_->size (); ++i)
93 Eigen::Vector4f
pt_src ((*input_)[(*indices_)[i]].x,
94 (*input_)[(*indices_)[i]].y,
95 (*input_)[(*indices_)[i]].z, 1.0f);
101 Eigen::Vector3f
uv (projection_matrix_ *
p_tr3);
112 distances[i] = std::sqrt ((
uv[0] - (*target_)[(*indices_tgt_)[i]].u) *
113 (
uv[0] - (*target_)[(*indices_tgt_)[i]].u) +
114 (
uv[1] - (*target_)[(*indices_tgt_)[i]].v) *
115 (
uv[1] - (*target_)[(*indices_tgt_)[i]].v));
123 if (indices_->size () != indices_tgt_->size ())
125 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
131 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n");
135 double thresh = threshold * threshold;
138 error_sqr_dists_.clear ();
139 inliers.reserve (indices_->size ());
140 error_sqr_dists_.reserve (indices_->size ());
142 Eigen::Matrix4f transform;
148 for (std::size_t i = 0; i < indices_->size (); ++i)
150 Eigen::Vector4f
pt_src ((*input_)[(*indices_)[i]].x,
151 (*input_)[(*indices_)[i]].y,
152 (*input_)[(*indices_)[i]].z, 1.0f);
158 Eigen::Vector3f
uv (projection_matrix_ *
p_tr3);
165 double distance = ((
uv[0] - (*target_)[(*indices_tgt_)[i]].u) *
166 (
uv[0] - (*target_)[(*indices_tgt_)[i]].u) +
167 (
uv[1] - (*target_)[(*indices_tgt_)[i]].v) *
168 (
uv[1] - (*target_)[(*indices_tgt_)[i]].v));
171 if (distance < thresh)
173 inliers.push_back ((*indices_)[i]);
174 error_sqr_dists_.push_back (distance);
184 if (indices_->size () != indices_tgt_->size ())
186 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
191 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration2D::countWithinDistance] No target dataset given!\n");
195 double thresh = threshold * threshold;
197 Eigen::Matrix4f transform;
203 std::size_t
nr_p = 0;
205 for (std::size_t i = 0; i < indices_->size (); ++i)
207 Eigen::Vector4f
pt_src ((*input_)[(*indices_)[i]].x,
208 (*input_)[(*indices_)[i]].y,
209 (*input_)[(*indices_)[i]].z, 1.0f);
215 Eigen::Vector3f
uv (projection_matrix_ *
p_tr3);
225 if (((
uv[0] - (*target_)[(*indices_tgt_)[i]].u) *
226 (
uv[0] - (*target_)[(*indices_tgt_)[i]].u) +
227 (
uv[1] - (*target_)[(*indices_tgt_)[i]].v) *
228 (
uv[1] - (*target_)[(*indices_tgt_)[i]].v)) < thresh)