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 (); } }