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 const auto nr_points = cloud_src.
size ();
59 if (cloud_tgt.
size () != nr_points)
61 PCL_ERROR(
"[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number " 62 "or points in source (%zu) differs than target (%zu)!\n",
63 static_cast<std::size_t>(nr_points),
64 static_cast<std::size_t>(cloud_tgt.
size()));
70 estimateRigidTransformation (source_it, target_it, transformation_matrix);
74 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 77 const std::vector<int> &indices_src,
79 Matrix4 &transformation_matrix)
const 81 if (indices_src.size () != cloud_tgt.
size ())
83 PCL_ERROR(
"[pcl::TransformationDQ::estimateRigidTransformation] Number or points " 84 "in source (%zu) differs than target (%zu)!\n",
86 static_cast<std::size_t
>(cloud_tgt.
size()));
92 estimateRigidTransformation (source_it, target_it, transformation_matrix);
96 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void 99 const std::vector<int> &indices_src,
101 const std::vector<int> &indices_tgt,
102 Matrix4 &transformation_matrix)
const 104 if (indices_src.size () != indices_tgt.size ())
106 PCL_ERROR(
"[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number " 107 "or points in source (%zu) differs than target (%zu)!\n",
115 estimateRigidTransformation (source_it, target_it, transformation_matrix);
119 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 124 Matrix4 &transformation_matrix)
const 128 estimateRigidTransformation (source_it, target_it, transformation_matrix);
132 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void 136 Matrix4 &transformation_matrix)
const 138 const int npts = static_cast <
int> (source_it.
size ());
140 transformation_matrix.setIdentity ();
143 Eigen::Matrix<Scalar,4,4> C1 = Eigen::Matrix<Scalar,4,4>::Zero();
144 Eigen::Matrix<Scalar,4,4> C2 = Eigen::Matrix<Scalar,4,4>::Zero();
145 Scalar *c1 = C1.data();
146 Scalar *c2 = C2.data();
148 for(
int i=0; i<npts; i++ ) {
149 const PointSource &a = *source_it;
150 const PointTarget &b = *target_it;
151 const Scalar axbx = a.x*b.x;
152 const Scalar ayby = a.y*b.y;
153 const Scalar azbz = a.z*b.z;
154 const Scalar axby = a.x*b.y;
155 const Scalar aybx = a.y*b.x;
156 const Scalar axbz = a.x*b.z;
157 const Scalar azbx = a.z*b.x;
158 const Scalar aybz = a.y*b.z;
159 const Scalar azby = a.z*b.y;
160 c1[0] += axbx - azbz - ayby;
161 c1[5] += ayby - azbz - axbx;
162 c1[10]+= azbz - axbx - ayby;
163 c1[15]+= axbx + ayby + azbz;
164 c1[1] += axby + aybx;
165 c1[2] += axbz + azbx;
166 c1[3] += aybz - azby;
167 c1[6] += azby + aybz;
168 c1[7] += azbx - axbz;
169 c1[11]+= axby - aybx;
197 const Eigen::Matrix<Scalar,4,4> A = (0.25f/float(npts))*C2.transpose()*C2 - C1;
199 const Eigen::EigenSolver< Eigen::Matrix<Scalar,4,4> > es(A);
202 es.eigenvalues().real().maxCoeff(&i);
203 const Eigen::Matrix<Scalar,4,1> qmat = es.eigenvectors().col(i).real();
204 const Eigen::Matrix<Scalar,4,1> smat = -(0.5f/float(npts))*C2*qmat;
206 const Eigen::Quaternion<Scalar> q( qmat(3), qmat(0), qmat(1), qmat(2) );
207 const Eigen::Quaternion<Scalar> s( smat(3), smat(0), smat(1), smat(2) );
209 const Eigen::Quaternion<Scalar> t = s*q.conjugate();
211 const Eigen::Matrix<Scalar,3,3> R( q.toRotationMatrix() );
213 for(
int i=0; i<3; ++i )
214 for(
int j=0; j<3; ++j)
215 transformation_matrix(i,j) = R(i,j);
217 transformation_matrix(0,3) = -t.x();
218 transformation_matrix(1,3) = -t.y();
219 transformation_matrix(2,3) = -t.z();
Iterator class for point clouds with or without given indices.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
std::size_t size() const
Size of the range the iterator is going through.