今天处理ORBslam的位姿进行点云拼接时碰到了问题
ORBslam获得的位姿旋转矩阵是一个在cv::Mat下定义的4×4的矩阵,需要转换到Eigen下,因为想要使用相应的点云转换函数
因此使用下面的函数进行转换
//把mat类型的旋转矩阵SE(3)转换为Eigen下的SE(3)
Eigen::Isometry3d Mat2Isometry3d(cv::Mat R)
{
Eigen::Isometry3d T;
{
cout<<"Convert Failed!"<<endl;
return Eigen::Isometry3d::Identity();
}
for(int i=0;i<4;i++)
for(int j=0;j<4;j++)
{
T(i,j)=R.at<float>(i,j);
}
return T;
}
需要注意的是在这一行T(i,j)=R.at<float>(i,j);
如果把float换成double,就会出现T的值非常的乱,与矩阵R没有任何关系,因此这儿的格式一定要注意