39 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
40 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_
42 #include <pcl/common/eigen.h>
45 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
49 Matrix4 &transformation_matrix)
const
51 std::size_t nr_points = cloud_src.
points.size ();
52 if (cloud_tgt.
points.size () != nr_points)
54 PCL_ERROR (
"[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.
points.size ());
60 estimateRigidTransformation (source_it, target_it, transformation_matrix);
64 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
67 const std::vector<int> &indices_src,
69 Matrix4 &transformation_matrix)
const
71 if (indices_src.size () != cloud_tgt.
points.size ())
73 PCL_ERROR (
"[pcl::TransformationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
79 estimateRigidTransformation (source_it, target_it, transformation_matrix);
83 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
86 const std::vector<int> &indices_src,
88 const std::vector<int> &indices_tgt,
89 Matrix4 &transformation_matrix)
const
91 if (indices_src.size () != indices_tgt.size ())
93 PCL_ERROR (
"[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
99 estimateRigidTransformation (source_it, target_it, transformation_matrix);
103 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
108 Matrix4 &transformation_matrix)
const
112 estimateRigidTransformation (source_it, target_it, transformation_matrix);
116 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
120 Matrix4 &transformation_matrix)
const
122 const int npts =
static_cast <int> (source_it.
size ());
124 transformation_matrix.setIdentity ();
127 Eigen::Matrix<Scalar,4,4> C1 = Eigen::Matrix<Scalar,4,4>::Zero();
128 Eigen::Matrix<Scalar,4,4> C2 = Eigen::Matrix<Scalar,4,4>::Zero();
129 Scalar *c1 = C1.data();
130 Scalar *c2 = C2.data();
132 for(
int i=0; i<npts; i++ ) {
133 const PointSource &a = *source_it;
134 const PointTarget &b = *target_it;
135 const Scalar axbx = a.x*b.x;
136 const Scalar ayby = a.y*b.y;
137 const Scalar azbz = a.z*b.z;
138 const Scalar axby = a.x*b.y;
139 const Scalar aybx = a.y*b.x;
140 const Scalar axbz = a.x*b.z;
141 const Scalar azbx = a.z*b.x;
142 const Scalar aybz = a.y*b.z;
143 const Scalar azby = a.z*b.y;
144 c1[0] += axbx - azbz - ayby;
145 c1[5] += ayby - azbz - axbx;
146 c1[10]+= azbz - axbx - ayby;
147 c1[15]+= axbx + ayby + azbz;
148 c1[1] += axby + aybx;
149 c1[2] += axbz + azbx;
150 c1[3] += aybz - azby;
151 c1[6] += azby + aybz;
152 c1[7] += azbx - axbz;
153 c1[11]+= axby - aybx;
181 const Eigen::Matrix<Scalar,4,4> A = (0.25f/float(npts))*C2.transpose()*C2 - C1;
183 const Eigen::EigenSolver< Eigen::Matrix<Scalar,4,4> > es(A);
186 es.eigenvalues().real().maxCoeff(&i);
187 const Eigen::Matrix<Scalar,4,1> qmat = es.eigenvectors().col(i).real();
188 const Eigen::Matrix<Scalar,4,1> smat = -(0.5f/float(npts))*C2*qmat;
190 const Eigen::Quaternion<Scalar> q( qmat(3), qmat(0), qmat(1), qmat(2) );
191 const Eigen::Quaternion<Scalar> s( smat(3), smat(0), smat(1), smat(2) );
193 const Eigen::Quaternion<Scalar> t = s*q.conjugate();
195 const Eigen::Matrix<Scalar,3,3> R( q.toRotationMatrix() );
197 for(
int i=0; i<3; ++i )
198 for(
int j=0; j<3; ++j)
199 transformation_matrix(i,j) = R(i,j);
201 transformation_matrix(0,3) = -t.x();
202 transformation_matrix(1,3) = -t.y();
203 transformation_matrix(2,3) = -t.z();