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)
62 "[pcl::TransformationEstimationDualQuaternion::estimateRigidTransformation] " 63 "Number or points in source (%zu) differs than target (%zu)!\n",
64 static_cast<std::size_t>(nr_points),
65 static_cast<std::size_t>(cloud_tgt.
size()));
71 estimateRigidTransformation (source_it, target_it, transformation_matrix);
75 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 78 const std::vector<int> &indices_src,
80 Matrix4 &transformation_matrix)
const 82 if (indices_src.size () != cloud_tgt.
size ())
84 PCL_ERROR(
"[pcl::TransformationDQ::estimateRigidTransformation] Number or points " 85 "in source (%zu) differs than target (%zu)!\n",
87 static_cast<std::size_t
>(cloud_tgt.
size()));
93 estimateRigidTransformation (source_it, target_it, transformation_matrix);
97 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void 100 const std::vector<int> &indices_src,
102 const std::vector<int> &indices_tgt,
103 Matrix4 &transformation_matrix)
const 105 if (indices_src.size () != indices_tgt.size ())
107 PCL_ERROR (
"[pcl::TransformationEstimationDualQuaternion::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
113 estimateRigidTransformation (source_it, target_it, transformation_matrix);
117 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 122 Matrix4 &transformation_matrix)
const 126 estimateRigidTransformation (source_it, target_it, transformation_matrix);
130 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void 134 Matrix4 &transformation_matrix)
const 136 const int npts =
static_cast<int> (source_it.
size ());
138 transformation_matrix.setIdentity ();
141 Eigen::Matrix<double, 4, 4> C1 = Eigen::Matrix<double, 4, 4>::Zero ();
142 Eigen::Matrix<double, 4, 4> C2 = Eigen::Matrix<double, 4, 4>::Zero ();
143 double* c1 = C1.data ();
144 double* c2 = C2.data ();
146 for (
int i = 0; i < npts; ++i)
148 const PointSource& a = *source_it;
149 const PointTarget& b = *target_it;
150 const double axbx = a.x * b.x;
151 const double ayby = a.y * b.y;
152 const double azbz = a.z * b.z;
153 const double axby = a.x * b.y;
154 const double aybx = a.y * b.x;
155 const double axbz = a.x * b.z;
156 const double azbx = a.z * b.x;
157 const double aybz = a.y * b.z;
158 const double azby = a.z * b.y;
159 c1[0] += axbx - azbz - ayby;
160 c1[5] += ayby - azbz - axbx;
161 c1[10] += azbz - axbx - ayby;
162 c1[15] += axbx + ayby + azbz;
163 c1[1] += axby + aybx;
164 c1[2] += axbz + azbx;
165 c1[3] += aybz - azby;
166 c1[6] += azby + aybz;
167 c1[7] += azbx - axbz;
168 c1[11] += axby - aybx;
196 const Eigen::Matrix<double, 4, 4> A = (0.25 / double (npts)) * C2.transpose () * C2 - C1;
198 const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4> > es (A);
201 es.eigenvalues ().real ().maxCoeff (&i);
202 const Eigen::Matrix<double, 4, 1> qmat = es.eigenvectors ().col (i).real ();
203 const Eigen::Matrix<double, 4, 1> smat = - (0.5 / double (npts)) * C2 * qmat;
205 const Eigen::Quaternion<double> q (qmat (3), qmat (0), qmat (1), qmat (2));
206 const Eigen::Quaternion<double> s (smat (3), smat (0), smat (1), smat (2));
208 const Eigen::Quaternion<double> t = s * q.conjugate ();
210 const Eigen::Matrix<double, 3, 3> R (q.toRotationMatrix ());
212 for (
int i = 0; i < 3; ++i)
213 for (
int j = 0; j < 3; ++j)
214 transformation_matrix (i, j) = R (i, j);
216 transformation_matrix (0, 3) = - t.x ();
217 transformation_matrix (1, 3) = - t.y ();
218 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.