40 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
41 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
43 #include <pcl/common/eigen.h>
49 namespace registration
52 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
56 Matrix4 &transformation_matrix)
const
58 std::size_t nr_points = cloud_src.
points.size ();
59 if (cloud_tgt.
points.size () != nr_points)
61 PCL_ERROR (
"[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.
points.size ());
67 estimateRigidTransformation (source_it, target_it, transformation_matrix);
71 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
74 const std::vector<int> &indices_src,
76 Matrix4 &transformation_matrix)
const
78 if (indices_src.size () != cloud_tgt.
points.size ())
80 PCL_ERROR (
"[pcl::TransformationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
86 estimateRigidTransformation (source_it, target_it, transformation_matrix);
90 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
93 const std::vector<int> &indices_src,
95 const std::vector<int> &indices_tgt,
96 Matrix4 &transformation_matrix)
const
98 if (indices_src.size () != indices_tgt.size ())
100 PCL_ERROR (
"[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
106 estimateRigidTransformation (source_it, target_it, transformation_matrix);
110 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
115 Matrix4 &transformation_matrix)
const
119 estimateRigidTransformation (source_it, target_it, transformation_matrix);
123 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
127 Matrix4 &transformation_matrix)
const
129 const int npts =
static_cast <int> (source_it.
size ());
131 transformation_matrix.setIdentity ();
134 Eigen::Matrix<Scalar,4,4> C1 = Eigen::Matrix<Scalar,4,4>::Zero();
135 Eigen::Matrix<Scalar,4,4> C2 = Eigen::Matrix<Scalar,4,4>::Zero();
136 Scalar *c1 = C1.data();
137 Scalar *c2 = C2.data();
139 for(
int i=0; i<npts; i++ ) {
140 const PointSource &a = *source_it;
141 const PointTarget &b = *target_it;
142 const Scalar axbx = a.x*b.x;
143 const Scalar ayby = a.y*b.y;
144 const Scalar azbz = a.z*b.z;
145 const Scalar axby = a.x*b.y;
146 const Scalar aybx = a.y*b.x;
147 const Scalar axbz = a.x*b.z;
148 const Scalar azbx = a.z*b.x;
149 const Scalar aybz = a.y*b.z;
150 const Scalar azby = a.z*b.y;
151 c1[0] += axbx - azbz - ayby;
152 c1[5] += ayby - azbz - axbx;
153 c1[10]+= azbz - axbx - ayby;
154 c1[15]+= axbx + ayby + azbz;
155 c1[1] += axby + aybx;
156 c1[2] += axbz + azbx;
157 c1[3] += aybz - azby;
158 c1[6] += azby + aybz;
159 c1[7] += azbx - axbz;
160 c1[11]+= axby - aybx;
188 const Eigen::Matrix<Scalar,4,4> A = (0.25f/float(npts))*C2.transpose()*C2 - C1;
190 const Eigen::EigenSolver< Eigen::Matrix<Scalar,4,4> > es(A);
193 es.eigenvalues().real().maxCoeff(&i);
194 const Eigen::Matrix<Scalar,4,1> qmat = es.eigenvectors().col(i).real();
195 const Eigen::Matrix<Scalar,4,1> smat = -(0.5f/float(npts))*C2*qmat;
197 const Eigen::Quaternion<Scalar> q( qmat(3), qmat(0), qmat(1), qmat(2) );
198 const Eigen::Quaternion<Scalar> s( smat(3), smat(0), smat(1), smat(2) );
200 const Eigen::Quaternion<Scalar> t = s*q.conjugate();
202 const Eigen::Matrix<Scalar,3,3> R( q.toRotationMatrix() );
204 for(
int i=0; i<3; ++i )
205 for(
int j=0; j<3; ++j)
206 transformation_matrix(i,j) = R(i,j);
208 transformation_matrix(0,3) = -t.x();
209 transformation_matrix(1,3) = -t.y();
210 transformation_matrix(2,3) = -t.z();