template <typename PointSource, typename PointTarget> inline void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation ( const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &indices_src, const pcl::PointCloud<PointTarget> &cloud_tgt, const std::vector<int> &indices_tgt, Eigen::Matrix4f &transformation_matrix) { if (indices_src.size () != indices_tgt.size ()) { PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ()); return; } // <cloud_src,cloud_src> is the source dataset transformation_matrix.setIdentity (); Eigen::Vector4f centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (cloud_src, indices_src, centroid_src); compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt); // Subtract the centroids from source, target Eigen::MatrixXf cloud_src_demean; demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean); Eigen::MatrixXf cloud_tgt_demean; demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean); getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); }
template <typename PointSource, typename PointTarget> inline void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation ( const pcl::PointCloud<PointSource> &cloud_src, const pcl::PointCloud<PointTarget> &cloud_tgt, const pcl::Correspondences &correspondences, Eigen::Matrix4f &transformation_matrix) { std::vector<int> indices_src, indices_tgt; pcl::registration::getQueryIndices (correspondences, indices_src); pcl::registration::getMatchIndices (correspondences, indices_tgt); // <cloud_src,cloud_src> is the source dataset Eigen::Vector4f centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (cloud_src, indices_src, centroid_src); compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt); // Subtract the centroids from source, target Eigen::MatrixXf cloud_src_demean; demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean); Eigen::MatrixXf cloud_tgt_demean; demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean); getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); }
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const { // Convert to Eigen format const int npts = static_cast <int> (source_it.size ()); Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts); Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts); for (int i = 0; i < npts; ++i) { cloud_src (0, i) = source_it->x; cloud_src (1, i) = source_it->y; cloud_src (2, i) = source_it->z; ++source_it; cloud_tgt (0, i) = target_it->x; cloud_tgt (1, i) = target_it->y; cloud_tgt (2, i) = target_it->z; ++target_it; } if (use_umeyama_) { // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); } else { source_it.reset (); target_it.reset (); // <cloud_src,cloud_src> is the source dataset transformation_matrix.setIdentity (); Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (source_it, centroid_src); compute3DCentroid (target_it, centroid_tgt); source_it.reset (); target_it.reset (); // Subtract the centroids from source, target Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean; demeanPointCloud (source_it, centroid_src, cloud_src_demean); demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); } }
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimationTranslation<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const { source_it.reset (); target_it.reset (); // <cloud_src,cloud_src> is the source dataset transformation_matrix.setIdentity (); Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (source_it, centroid_src); compute3DCentroid (target_it, centroid_tgt); source_it.reset (); target_it.reset (); getTransformationFromCorrelation (centroid_src, centroid_tgt, transformation_matrix); }
template <typename PointSource, typename PointTarget> inline void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation ( const pcl::PointCloud<PointSource> &cloud_src, const pcl::PointCloud<PointTarget> &cloud_tgt, Eigen::Matrix4f &transformation_matrix) { // <cloud_src,cloud_src> is the source dataset transformation_matrix.setIdentity (); Eigen::Vector4f centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (cloud_src, centroid_src); compute3DCentroid (cloud_tgt, centroid_tgt); // Subtract the centroids from source, target Eigen::MatrixXf cloud_src_demean; demeanPointCloud (cloud_src, centroid_src, cloud_src_demean); Eigen::MatrixXf cloud_tgt_demean; demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean); getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); }
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const { source_it.reset (); target_it.reset (); Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (source_it, centroid_src); compute3DCentroid (target_it, centroid_tgt); source_it.reset (); target_it.reset (); // ignore z component centroid_src[2] = 0.0f; centroid_tgt[2] = 0.0f; // Subtract the centroids from source, target Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean; demeanPointCloud (source_it, centroid_src, cloud_src_demean); demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); }