38 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
39 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
43 namespace registration {
45 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
50 Matrix4& transformation_matrix)
const
52 const auto nr_points = cloud_src.
size();
53 if (cloud_tgt.
size() != nr_points) {
54 PCL_ERROR(
"[pcl::TransformationEstimation2D::estimateRigidTransformation] Number "
55 "or points in source (%zu) differs than target (%zu)!\n",
56 static_cast<std::size_t
>(nr_points),
57 static_cast<std::size_t
>(cloud_tgt.
size()));
63 estimateRigidTransformation(source_it, target_it, transformation_matrix);
66 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
70 const std::vector<int>& indices_src,
72 Matrix4& transformation_matrix)
const
74 if (indices_src.size() != cloud_tgt.
size()) {
75 PCL_ERROR(
"[pcl::Transformation2D::estimateRigidTransformation] Number or points "
76 "in source (%zu) differs than target (%zu)!\n",
78 static_cast<std::size_t
>(cloud_tgt.
size()));
84 estimateRigidTransformation(source_it, target_it, transformation_matrix);
87 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
91 const std::vector<int>& indices_src,
93 const std::vector<int>& indices_tgt,
94 Matrix4& transformation_matrix)
const
96 if (indices_src.size() != indices_tgt.size()) {
97 PCL_ERROR(
"[pcl::TransformationEstimation2D::estimateRigidTransformation] Number "
98 "or points in source (%lu) differs than target (%lu)!\n",
106 estimateRigidTransformation(source_it, target_it, transformation_matrix);
109 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
115 Matrix4& transformation_matrix)
const
119 estimateRigidTransformation(source_it, target_it, transformation_matrix);
122 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
127 Matrix4& transformation_matrix)
const
132 Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
140 centroid_src[2] = 0.0f;
141 centroid_tgt[2] = 0.0f;
143 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
148 getTransformationFromCorrelation(cloud_src_demean,
152 transformation_matrix);
155 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
159 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
160 const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
161 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
162 const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
163 Matrix4& transformation_matrix)
const
165 transformation_matrix.setIdentity();
168 Eigen::Matrix<Scalar, 3, 3> H =
169 (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);
171 float angle = std::atan2((H(0, 1) - H(1, 0)), (H(0, 0) + H(1, 1)));
173 Eigen::Matrix<Scalar, 3, 3> R(Eigen::Matrix<Scalar, 3, 3>::Identity());
174 R(0, 0) = R(1, 1) = std::cos(angle);
175 R(0, 1) = -std::sin(angle);
176 R(1, 0) = std::sin(angle);
179 transformation_matrix.topLeftCorner(3, 3).matrix() = R;
180 const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3).matrix());
181 transformation_matrix.block(0, 3, 3, 1).matrix() = centroid_tgt.head(3) - Rc;
187 #endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_