159 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>&
cloud_src_demean,
161 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>&
cloud_tgt_demean,
168 Eigen::Matrix<Scalar, 3, 3> H =
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);
180 const Eigen::Matrix<Scalar, 3, 1>
Rc(
R *
centroid_src.head(3).matrix());
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.