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.
points.size ();
56 if (cloud_tgt.
points.size () != nr_points)
58 PCL_ERROR (
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs from target (%lu)!\n", nr_points, cloud_tgt.
points.size ());
64 estimateRigidTransformation (source_it, target_it, transformation_matrix);
68 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
71 const std::vector<int> &indices_src,
73 Matrix4 &transformation_matrix)
const
75 const auto nr_points = indices_src.size ();
76 if (cloud_tgt.
points.size () != nr_points)
78 PCL_ERROR (
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
84 estimateRigidTransformation (source_it, target_it, transformation_matrix);
88 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
91 const std::vector<int> &indices_src,
93 const std::vector<int> &indices_tgt,
94 Matrix4 &transformation_matrix)
const
96 const auto nr_points = indices_src.size ();
97 if (indices_tgt.size () != nr_points)
99 PCL_ERROR (
"[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
109 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
114 Matrix4 &transformation_matrix)
const
118 estimateRigidTransformation (source_it, target_it, transformation_matrix);
122 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
125 Matrix4 &transformation_matrix)
const
128 const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
129 const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
130 const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
131 const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
132 const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
134 rotation_z * rotation_y * rotation_x;
135 transformation_matrix = transform.matrix ();
139 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
143 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
144 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
150 auto M = ATA.template selfadjointView<Eigen::Upper> ();
155 for (; source_it.
isValid () && target_it.
isValid (); ++source_it, ++target_it)
157 const Vector3 p (source_it->x, source_it->y, source_it->z);
158 const Vector3 q (target_it->x, target_it->y, target_it->z);
159 const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
160 const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
162 if (enforce_same_direction_normals_)
164 if (n1.dot (n2) >= 0.)
174 if (!p.array().isFinite().all() ||
175 !q.array().isFinite().all() ||
176 !n.array().isFinite().all())
182 v << (p + q).cross (n), n;
185 ATb += v * (q - p).dot (n);
189 const Vector6 x = M.ldlt ().solve (ATb);
192 constructTransformationMatrix (x, transformation_matrix);
196 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
200 enforce_same_direction_normals_ = enforce_same_direction_normals;
204 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline bool
208 return enforce_same_direction_normals_;
Iterator class for point clouds with or without given indices.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences