40 #include <pcl/cloud_iterator.h>
46 namespace registration
49 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
53 Matrix4 &transformation_matrix)
const
55 const auto nr_points = cloud_src.
size ();
56 if (cloud_tgt.
size () != nr_points)
58 PCL_ERROR(
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
59 "estimateRigidTransformation] Number or points in source (%zu) differs "
60 "from target (%zu)!\n",
61 static_cast<std::size_t
>(nr_points),
62 static_cast<std::size_t
>(cloud_tgt.
size()));
68 estimateRigidTransformation (source_it, target_it, transformation_matrix);
72 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
75 const std::vector<int> &indices_src,
77 Matrix4 &transformation_matrix)
const
79 const auto nr_points = indices_src.size ();
80 if (cloud_tgt.
size () != nr_points)
82 PCL_ERROR(
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
83 "estimateRigidTransformation] Number or points in source (%zu) differs "
84 "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 const auto nr_points = indices_src.size ();
105 if (indices_tgt.size () != nr_points)
107 PCL_ERROR(
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
108 "estimateRigidTransformation] Number or points in source (%zu) differs "
109 "than target (%zu)!\n",
117 estimateRigidTransformation (source_it, target_it, transformation_matrix);
121 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
126 Matrix4 &transformation_matrix)
const
130 estimateRigidTransformation (source_it, target_it, transformation_matrix);
134 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
137 Matrix4 &transformation_matrix)
const
140 const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
141 const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
142 const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
143 const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
144 const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
146 rotation_z * rotation_y * rotation_x;
147 transformation_matrix = transform.matrix ();
151 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
155 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
156 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
162 auto M = ATA.template selfadjointView<Eigen::Upper> ();
167 for (; source_it.
isValid () && target_it.
isValid (); ++source_it, ++target_it)
169 const Vector3 p (source_it->x, source_it->y, source_it->z);
170 const Vector3 q (target_it->x, target_it->y, target_it->z);
171 const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
172 const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
174 if (enforce_same_direction_normals_)
176 if (n1.dot (n2) >= 0.)
186 if (!p.array().isFinite().all() ||
187 !q.array().isFinite().all() ||
188 !n.array().isFinite().all())
194 v << (p + q).cross (n), n;
197 ATb += v * (q - p).dot (n);
201 const Vector6 x = M.ldlt ().solve (ATb);
204 constructTransformationMatrix (x, transformation_matrix);
208 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
212 enforce_same_direction_normals_ = enforce_same_direction_normals;
216 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline bool
220 return enforce_same_direction_normals_;