/*-----------------------------------------------------------------------------------------------------------------------------------
此函数的实现基于自撰《SVD与刚体变换》文档 dst_mat = R*src_mat +t
------------------------------------------------------------------------------------------------------------------------------------*/
bool Estimate_3D_Absolute_Orientation_By_SVD(Eigen::MatrixXd &src_mat,\ Eigen::MatrixXd &dst_mat,\ Eigen::Matrix3d &R, Eigen::Vector3d &t){
if(src_mat.rows() != dst_mat.rows() || src_mat.cols()!=dst_mat.cols()){
return false;
}
//将原点移至3D点集的中心点
Eigen::Vector3d center_xyz_tgt = dst_mat.rowwise().mean();
//按列求均值:x'=x/N, y'=y/N, z'=z/N, 3D点的中心点,
xxx.colwise().mean() Eigen::Vector3d center_xyz_src = src_mat.rowwise().mean();
for(uint32_t i=0; i svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
//ComputeThinU | ComputeThinV Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
Eigen::Matrix3d S = Eigen::Matrix3d::Identity();
Eigen::Matrix3d tmp_R = V*U.transpose();
// R = V*U^T if(tmp_R.determinant()<0){ S(2,2) = -1; } R = V*S*U.transpose(
);
t = center_xyz_tgt - R*center_xyz_src; return true;
}
/*---------------------------------------------------------------------------------------------------*/
bool Estimate_3D_Absolute_Orientation(const pcl::PointCloud::Ptr &src_cld,
\ const pcl::PointCloud::Ptr &dst_cld,
\ Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
if(src_cld->points.size() != dst_cld->points.size()){
std::cout<<"Err: points num mismatch! "<points.size()<<" != "<points.size()<points.size());
dst_mat.resize(3, dst_cld->points.size());
for(uint32_t i=0; ipoints.size(); i++){
src_mat.block<3,1>(0, i)<points[i].x,src_cld->points[i].y,src_cld->points[i].z;
dst_mat.block<3,1>(0, i)<points[i].x,dst_cld->points[i].y,dst_cld->points[i].z;
}
return Estimate_3D_Absolute_Orientation_By_SVD(src_mat, dst_mat, R, t);
}