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()).
template 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.template topLeftCorner<3, 3>().matrix() = R;
180 const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.template head<3>().matrix());
181 transformation_matrix.template block<3, 1>(0, 3).matrix() =
182 centroid_tgt.template 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.