137 Matrix4& transformation_matrix)
const
139 transformation_matrix.setIdentity();
143 Eigen::Matrix<Scalar, 4, 1> source_mean, target_mean;
150 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> source_demean, target_demean;
157 Eigen::Matrix<Scalar, 3, 1> s1 =
158 source_demean.col(1).template head<3>() - source_demean.col(0).template head<3>();
161 Eigen::Matrix<Scalar, 3, 1> s2 =
162 source_demean.col(2).template head<3>() - source_demean.col(0).template head<3>();
163 s2 -= s2.dot(s1) * s1;
166 Eigen::Matrix<Scalar, 3, 3> source_rot;
167 source_rot.col(0) = s1;
168 source_rot.col(1) = s2;
169 source_rot.col(2) = s1.cross(s2);
171 Eigen::Matrix<Scalar, 3, 1> t1 =
172 target_demean.col(1).template head<3>() - target_demean.col(0).template head<3>();
175 Eigen::Matrix<Scalar, 3, 1> t2 =
176 target_demean.col(2).template head<3>() - target_demean.col(0).template head<3>();
177 t2 -= t2.dot(t1) * t1;
180 Eigen::Matrix<Scalar, 3, 3> target_rot;
181 target_rot.col(0) = t1;
182 target_rot.col(1) = t2;
183 target_rot.col(2) = t1.cross(t2);
186 Eigen::Matrix<Scalar, 3, 3> R = target_rot * source_rot.transpose();
187 transformation_matrix.template topLeftCorner<3, 3>() = R;
190 transformation_matrix.template block<3, 1>(0, 3) =
191 target_mean.template head<3>() - R * source_mean.template head<3>();
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.