template <typename PointT> void
pcl::SampleConsensusModelRegistration2D<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
inliers.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n");
return;
}
double thresh = threshold * threshold;
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
Eigen::Matrix4f transform;
transform.row (0).matrix () = model_coefficients.segment<4>(0);
transform.row (1).matrix () = model_coefficients.segment<4>(4);
transform.row (2).matrix () = model_coefficients.segment<4>(8);
transform.row (3).matrix () = model_coefficients.segment<4>(12);
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f p_tr (transform * pt_src);
// Project the point on the image plane
Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
Eigen::Vector3f uv (projection_matrix_ * p_tr3);
if (uv[2] < 0)
continue;
uv /= uv[2];
double distance = ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
(uv[0] - target_->points[(*indices_tgt_)[i]].u) +
(uv[1] - target_->points[(*indices_tgt_)[i]].v) *
(uv[1] - target_->points[(*indices_tgt_)[i]].v));
// Calculate the distance from the transformed point to its correspondence
if (distance < thresh)
{
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = distance;
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
PCL_INFO ("[pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel]\n");
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
distances.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n");
return;
}
distances.resize (indices_->size ());
// Get the 4x4 transformation
Eigen::Matrix4f transform;
transform.row (0).matrix () = model_coefficients.segment<4>(0);
transform.row (1).matrix () = model_coefficients.segment<4>(4);
transform.row (2).matrix () = model_coefficients.segment<4>(8);
transform.row (3).matrix () = model_coefficients.segment<4>(12);
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f p_tr (transform * pt_src);
// Project the point on the image plane
Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
Eigen::Vector3f uv (projection_matrix_ * p_tr3);
if (uv[2] < 0)
continue;
uv /= uv[2];
// Calculate the distance from the transformed point to its correspondence
// need to compute the real norm here to keep MSAC and friends general
distances[i] = std::sqrt ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
(uv[0] - target_->points[(*indices_tgt_)[i]].u) +
(uv[1] - target_->points[(*indices_tgt_)[i]].v) *
(uv[1] - target_->points[(*indices_tgt_)[i]].v));
}
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
inliers.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
return;
}
double thresh = threshold * threshold;
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
inliers.resize (indices_->size ());
Eigen::Matrix4f transform;
transform.row (0) = model_coefficients.segment<4>(0);
transform.row (1) = model_coefficients.segment<4>(4);
transform.row (2) = model_coefficients.segment<4>(8);
transform.row (3) = model_coefficients.segment<4>(12);
int nr_p = 0;
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
pt_src[3] = 1;
Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
pt_tgt[3] = 1;
Eigen::Vector4f p_tr (transform * pt_src);
// Calculate the distance from the transformed point to its correspondence
if ((p_tr - pt_tgt).squaredNorm () < thresh)
inliers[nr_p++] = (*indices_)[i];
}
inliers.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
distances.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
// Get the 4x4 transformation
Eigen::Matrix4f transform;
transform.row (0) = model_coefficients.segment<4>(0);
transform.row (1) = model_coefficients.segment<4>(4);
transform.row (2) = model_coefficients.segment<4>(8);
transform.row (3) = model_coefficients.segment<4>(12);
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
pt_src[3] = 1;
Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
pt_tgt[3] = 1;
Eigen::Vector4f p_tr (transform * pt_src);
// Calculate the distance from the transformed point to its correspondence
// need to compute the real norm here to keep MSAC and friends general
distances[i] = (p_tr - pt_tgt).norm ();
}
}
