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>(