41#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
42#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
44#include <pcl/common/eigen.h>
48namespace registration {
50template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
59 PCL_ERROR(
"[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number "
60 "or points in source (%zu) differs than target (%zu)!\n",
61 static_cast<std::size_t
>(nr_points),
71template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
80 PCL_ERROR(
"[pcl::TransformationSVD::estimateRigidTransformation] Number or points "
81 "in source (%zu) differs than target (%zu)!\n",
92template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
102 PCL_ERROR(
"[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number "
103 "or points in source (%zu) differs than target (%zu)!\n",
114template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
127template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
141 for (
int i = 0; i <
npts; ++i) {
183template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
187 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>&
cloud_src_demean,
189 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>&
cloud_tgt_demean,
196 Eigen::Matrix<Scalar, 3, 3> H =
200 Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3>>
svd(
201 H, Eigen::ComputeFullU | Eigen::ComputeFullV);
202 Eigen::Matrix<Scalar, 3, 3> u =
svd.matrixU();
203 Eigen::Matrix<Scalar, 3, 3> v =
svd.matrixV();
206 if (u.determinant() * v.determinant() < 0) {
207 for (
int x = 0; x < 3; ++x)
211 Eigen::Matrix<Scalar, 3, 3>
R = v * u.transpose();
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Eigen::internal::umeyama_transform_matrix_type< Derived, OtherDerived >::type umeyama(const Eigen::MatrixBase< Derived > &src, const Eigen::MatrixBase< OtherDerived > &dst, bool with_scaling=false)
Returns the transformation between two point sets.