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 size_t nr_points = cloud_src.
points.size ();
52 if (cloud_tgt.
points.size () != nr_points)
54 PCL_ERROR (
"[pcl::TransformationEstimationDualQuaternion::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::TransformationEstimationDualQuaternion::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<double, 4, 4> C1 = Eigen::Matrix<double, 4, 4>::Zero ();
128 Eigen::Matrix<double, 4, 4> C2 = Eigen::Matrix<double, 4, 4>::Zero ();
129 double* c1 = C1.data ();
130 double* c2 = C2.data ();
132 for (
int i = 0; i < npts; ++i)
134 const PointSource& a = *source_it;
135 const PointTarget& b = *target_it;
136 const double axbx = a.x * b.x;
137 const double ayby = a.y * b.y;
138 const double azbz = a.z * b.z;
139 const double axby = a.x * b.y;
140 const double aybx = a.y * b.x;
141 const double axbz = a.x * b.z;
142 const double azbx = a.z * b.x;
143 const double aybz = a.y * b.z;
144 const double azby = a.z * b.y;
145 c1[0] += axbx - azbz - ayby;
146 c1[5] += ayby - azbz - axbx;
147 c1[10] += azbz - axbx - ayby;
148 c1[15] += axbx + ayby + azbz;
149 c1[1] += axby + aybx;
150 c1[2] += axbz + azbx;
151 c1[3] += aybz - azby;
152 c1[6] += azby + aybz;
153 c1[7] += azbx - axbz;
154 c1[11] += axby - aybx;
182 const Eigen::Matrix<double, 4, 4> A = (0.25 / double (npts)) * C2.transpose () * C2 - C1;
184 const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4> > es (A);
187 es.eigenvalues ().real ().maxCoeff (&i);
188 const Eigen::Matrix<double, 4, 1> qmat = es.eigenvectors ().col (i).real ();
189 const Eigen::Matrix<double, 4, 1> smat = - (0.5 / double (npts)) * C2 * qmat;
191 const Eigen::Quaternion<double> q (qmat (3), qmat (0), qmat (1), qmat (2));
192 const Eigen::Quaternion<double> s (smat (3), smat (0), smat (1), smat (2));
194 const Eigen::Quaternion<double> t = s * q.conjugate ();
196 const Eigen::Matrix<double, 3, 3> R (q.toRotationMatrix ());
198 for (
int i = 0; i < 3; ++i)
199 for (
int j = 0; j < 3; ++j)
200 transformation_matrix (i, j) = R (i, j);
202 transformation_matrix (0, 3) = - t.x ();
203 transformation_matrix (1, 3) = - t.y ();
204 transformation_matrix (2, 3) = - t.z ();
Iterator class for point clouds with or without given indices.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
size_t size() const
Size of the range the iterator is going through.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences