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));
165 if (distance < thresh)
167 inliers[nr_p] = (*indices_)[i];
168 error_sqr_dists_[nr_p] = distance;
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)
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_ 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.