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 PointT, typename Scalar> void pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, const pcl::PointIndices& indices, const Eigen::Matrix<Scalar, 4, 1> ¢roid, pcl::PointCloud<PointT> &cloud_out) { return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out)); }
template <typename PointT> void pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD ( const pcl::PointCloud<PointT> &cloud_src, const std::vector<int> &indices_src, const pcl::PointCloud<PointT> &cloud_tgt, const std::vector<int> &indices_tgt, Eigen::VectorXf &transform) { transform.resize (16); 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); // Assemble the correlation matrix H = source * target' Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f u = svd.matrixU (); Eigen::Matrix3f v = svd.matrixV (); // Compute R = V * U' if (u.determinant () * v.determinant () < 0) { for (int x = 0; x < 3; ++x) v (x, 2) *= -1; } Eigen::Matrix3f R = v * u.transpose (); // Return the correct transformation transform.segment<3> (0) = R.row (0); transform[12] = 0; transform.segment<3> (4) = R.row (1); transform[13] = 0; transform.segment<3> (8) = R.row (2); transform[14] = 0; Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> (); transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0; }
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); }
template<typename PointT> bool pcl::PCA<PointT>::initCompute () { if(!Base::initCompute ()) { PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed"); return (false); } if(indices_->size () < 3) { PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3"); return (false); } // Compute mean mean_ = Eigen::Vector4f::Zero (); compute3DCentroid (*input_, *indices_, mean_); // Compute demeanished cloud Eigen::MatrixXf cloud_demean; demeanPointCloud (*input_, *indices_, mean_, cloud_demean); assert (cloud_demean.cols () == int (indices_->size ())); // Compute the product cloud_demean * cloud_demean^T Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ()); // Compute eigen vectors and values Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha); // Organize eigenvectors and eigenvalues in ascendent order for (int i = 0; i < 3; ++i) { eigenvalues_[i] = evd.eigenvalues () [2-i]; eigenvectors_.col (i) = evd.eigenvectors ().col (2-i); } // If not basis only then compute the coefficients if (!basis_only_) coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> (); compute_done_ = true; return (true); }
void SetTgtPoints(std::vector<Point3d>& out_cloud) { compute3DCentroid(out_cloud, centroid_tgt); demeanPointCloud(out_cloud, centroid_tgt, cloud_tgt_demean); }
void SetRefPoints(std::vector<Point3d>& in_cloud) { compute3DCentroid(in_cloud, centroid_ref); demeanPointCloud(in_cloud, centroid_ref, cloud_ref_demean); }