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::TransformationEstimationDualQuaternion::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::TransformationEstimationDualQuaternion::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<double, 4, 4> C1 = Eigen::Matrix<double, 4, 4>::Zero ();
135 Eigen::Matrix<double, 4, 4> C2 = Eigen::Matrix<double, 4, 4>::Zero ();
136 double* c1 = C1.data ();
137 double* c2 = C2.data ();
139 for (
int i = 0; i < npts; ++i)
141 const PointSource& a = *source_it;
142 const PointTarget& b = *target_it;
143 const double axbx = a.x * b.x;
144 const double ayby = a.y * b.y;
145 const double azbz = a.z * b.z;
146 const double axby = a.x * b.y;
147 const double aybx = a.y * b.x;
148 const double axbz = a.x * b.z;
149 const double azbx = a.z * b.x;
150 const double aybz = a.y * b.z;
151 const double azby = a.z * b.y;
152 c1[0] += axbx - azbz - ayby;
153 c1[5] += ayby - azbz - axbx;
154 c1[10] += azbz - axbx - ayby;
155 c1[15] += axbx + ayby + azbz;
156 c1[1] += axby + aybx;
157 c1[2] += axbz + azbx;
158 c1[3] += aybz - azby;
159 c1[6] += azby + aybz;
160 c1[7] += azbx - axbz;
161 c1[11] += axby - aybx;
189 const Eigen::Matrix<double, 4, 4> A = (0.25 / double (npts)) * C2.transpose () * C2 - C1;
191 const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4> > es (A);
194 es.eigenvalues ().real ().maxCoeff (&i);
195 const Eigen::Matrix<double, 4, 1> qmat = es.eigenvectors ().col (i).real ();
196 const Eigen::Matrix<double, 4, 1> smat = - (0.5 / double (npts)) * C2 * qmat;
198 const Eigen::Quaternion<double> q (qmat (3), qmat (0), qmat (1), qmat (2));
199 const Eigen::Quaternion<double> s (smat (3), smat (0), smat (1), smat (2));
201 const Eigen::Quaternion<double> t = s * q.conjugate ();
203 const Eigen::Matrix<double, 3, 3> R (q.toRotationMatrix ());
205 for (
int i = 0; i < 3; ++i)
206 for (
int j = 0; j < 3; ++j)
207 transformation_matrix (i, j) = R (i, j);
209 transformation_matrix (0, 3) = - t.x ();
210 transformation_matrix (1, 3) = - t.y ();
211 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.
std::size_t size() const
Size of the range the iterator is going through.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences