RGB-D SLAM——点云拼接篇(二)

1.OpenCv旋转向量平移向量转换为Eigen变换矩阵

在上一节中我们已经知道如何求出俩张图片的旋转向量和平移向量，那么接下来我们需要将旋转向量和平移向量转换成平移矩阵，可以使用Eigen工具来实现：

Eigen::Isometry3d cvMat2Eigen(cv::Mat &rvec,cv::Mat &tvec)
{
    cv::Mat R;
    cv::Rodrigues(rvec,R);//罗德里格旋转公式，将3x1的旋转向量转换成3x3的旋转矩阵
    Eigen::Matrix3d r; //the matrix are 3x3，类型为double
    cv::cv2eigen(R,r)；//将cv格式的矩阵(Mat类型的)转换成Eigen类型的Matrix
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//旋转和平移的集合,4*4的矩阵
    Eigen::AngleAxisd angle(r); 
    Eigen::Translation<double,3> trans(tvec.at<double>(0,0),tvec.at<double>(0,1),tvec.at<double>(0,2));
    T=angle;  //完成旋转矩阵的存储
    T(0,3)=tvec.at<double>(