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);
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;
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.