40 #include <pcl/cloud_iterator.h>
43 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
47 Matrix4 &transformation_matrix)
const
49 const auto nr_points = cloud_src.
points.size ();
50 if (cloud_tgt.
points.size () != nr_points)
52 PCL_ERROR (
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs from target (%lu)!\n", nr_points, cloud_tgt.
points.size ());
58 estimateRigidTransformation (source_it, target_it, transformation_matrix);
62 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
65 const std::vector<int> &indices_src,
67 Matrix4 &transformation_matrix)
const
69 const auto nr_points = indices_src.size ();
70 if (cloud_tgt.
points.size () != nr_points)
72 PCL_ERROR (
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
78 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 const auto nr_points = indices_src.size ();
92 if (indices_tgt.size () != nr_points)
94 PCL_ERROR (
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
100 estimateRigidTransformation (source_it, target_it, transformation_matrix);
104 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
109 Matrix4 &transformation_matrix)
const
113 estimateRigidTransformation (source_it, target_it, transformation_matrix);
117 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
120 Matrix4 &transformation_matrix)
const
123 const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
124 const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
125 const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
126 const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
127 const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
129 rotation_z * rotation_y * rotation_x;
130 transformation_matrix = transform.matrix ();
134 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
138 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
139 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
145 auto M = ATA.template selfadjointView<Eigen::Upper> ();
150 for (; source_it.
isValid () && target_it.
isValid (); ++source_it, ++target_it)
152 const Vector3 p (source_it->x, source_it->y, source_it->z);
153 const Vector3 q (target_it->x, target_it->y, target_it->z);
154 const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
155 const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
157 if (enforce_same_direction_normals_)
159 if (n1.dot (n2) >= 0.)
169 if (!p.array().isFinite().all() ||
170 !q.array().isFinite().all() ||
171 !n.array().isFinite().all())
177 v << (p + q).cross (n), n;
180 ATb += v * (q - p).dot (n);
184 const Vector6 x = M.ldlt ().solve (ATb);
187 constructTransformationMatrix (x, transformation_matrix);
191 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
195 enforce_same_direction_normals_ = enforce_same_direction_normals;
199 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline bool
203 return enforce_same_direction_normals_;