41 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
42 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
44 #include <pcl/common/eigen.h>
50 namespace registration
53 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
57 Matrix4 &transformation_matrix)
const
59 std::size_t nr_points = cloud_src.
points.size ();
60 if (cloud_tgt.
points.size () != nr_points)
62 PCL_ERROR (
"[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.
points.size ());
68 estimateRigidTransformation (source_it, target_it, transformation_matrix);
72 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
75 const std::vector<int> &indices_src,
77 Matrix4 &transformation_matrix)
const
79 if (indices_src.size () != cloud_tgt.
points.size ())
81 PCL_ERROR (
"[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
87 estimateRigidTransformation (source_it, target_it, transformation_matrix);
91 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
94 const std::vector<int> &indices_src,
96 const std::vector<int> &indices_tgt,
97 Matrix4 &transformation_matrix)
const
99 if (indices_src.size () != indices_tgt.size ())
101 PCL_ERROR (
"[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
107 estimateRigidTransformation (source_it, target_it, transformation_matrix);
111 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
116 Matrix4 &transformation_matrix)
const
124 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
128 Matrix4 &transformation_matrix)
const
131 const int npts =
static_cast <int> (source_it.
size ());
137 Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
138 Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);
140 for (
int i = 0; i < npts; ++i)
142 cloud_src (0, i) = source_it->x;
143 cloud_src (1, i) = source_it->y;
144 cloud_src (2, i) = source_it->z;
147 cloud_tgt (0, i) = target_it->x;
148 cloud_tgt (1, i) = target_it->y;
149 cloud_tgt (2, i) = target_it->z;
154 transformation_matrix =
pcl::umeyama (cloud_src, cloud_tgt,
false);
160 transformation_matrix.setIdentity ();
162 Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
169 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
173 getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
178 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
180 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
181 const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,
182 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
183 const Eigen::Matrix<Scalar, 4, 1> ¢roid_tgt,
184 Matrix4 &transformation_matrix)
const
186 transformation_matrix.setIdentity ();
189 Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
192 Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
193 Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
194 Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();
197 if (u.determinant () * v.determinant () < 0)
199 for (
int x = 0; x < 3; ++x)
203 Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();
206 transformation_matrix.topLeftCorner (3, 3) = R;
207 const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
208 transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;