理論過程
頭文件說明
1.使用羅德里格斯公式需要包含頭文件為#include<opencv2/calib3d.hpp>
2.使用函數(shù)cv2eigen需要包含頭文件<opencv2/core/eigen.hpp>，但是在則之前要包含一個(gè)對(duì)eigen定義的頭文件，否則會(huì)報(bào)eigen.hpp文件的錯(cuò)，所以使用cv2eigen函數(shù)應(yīng)該這樣寫頭文件

#include<eigen3/Eigen/Dense>//for cv2eigen()
#include <opencv2/core/eigen.hpp>//for cv2eigen()

把旋轉(zhuǎn)向量轉(zhuǎn)換成旋轉(zhuǎn)矩陣SO(3)，使用羅德里格斯公式進(jìn)行轉(zhuǎn)換

需要注意到的是solvepnp函數(shù)解出來的是旋轉(zhuǎn)向量r,將r轉(zhuǎn)換成r=theta*u;其中u是旋轉(zhuǎn)向量進(jìn)行單位化后的單位向量，theta是旋轉(zhuǎn)向量的模，這個(gè)theta在計(jì)算正弦值是需要把這個(gè)theta當(dāng)作弧度去計(jì)算而不是角度
實(shí)踐
有旋轉(zhuǎn)向量rvec如下

rvec=[-0.02387454801078471;
?0.02952769731891602;
?0.02088407602780516]

通過這一段代碼，先把旋轉(zhuǎn)向量rvec通過羅德里格斯公式計(jì)算得到旋轉(zhuǎn)矩陣R，在把在cv下定義的旋轉(zhuǎn)矩陣R轉(zhuǎn)換到Eigen下定義的矩陣r

cv::Mat R;
cv::Rodrigues(rvec,R);
Eigen::Matrix3d r;
cv::cv2eigen(R,r);

然后對(duì)應(yīng)的有平移向量tvec如下

tvec=[0.005806541820330855;
?0.001584643814482065;
?0.003887003832579859]

通過下一段代碼就能的到對(duì)應(yīng)的旋轉(zhuǎn)矩陣SE(3)了

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd angle(r);
cout<<"translation"<<endl;
T=angle;
T(0,3) = result.tvec.at<double>(0,0);?
T(1,3) = result.tvec.at<double>(0,1);?
T(2,3) = result.tvec.at<double>(0,2);

?

Mat rvec, tvec, inliers;
cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
num_inliers_ = inliers.rows;
cout<<"pnp inliers: "<<num_inliers_<<endl;

cv::Mat R;
cv::Rodrigues(rvec,R);//先把旋轉(zhuǎn)向量rvec通過羅德里格斯公式計(jì)算得到旋轉(zhuǎn)矩陣R
Eigen::Matrix3d r;
cv::cv2eigen(R,r);//在把在cv下定義的旋轉(zhuǎn)矩陣R轉(zhuǎn)換到Eigen下定義的矩陣r
Eigen::Quaterniond q =Eigen::Quaterniond(r);
cout<<"quaterniond =\n"<< q.coeffs()<<endl;// x y z w

//    cout << endl << "Saving camera pose trajectory to " << filename << " ..." << endl;
//    ofstream f;
//    f.open(filename.c_str());
//    f << fixed;cv::Mat R;cv::Rodrigues(rvec,R);//先把旋轉(zhuǎn)向量rvec通過羅德里格斯公式計(jì)算得到旋轉(zhuǎn)矩陣REigen::Matrix3d r;cv::cv2eigen(R,r);//在把在cv下定義的旋轉(zhuǎn)矩陣R轉(zhuǎn)換到Eigen下定義的矩陣rEigen::Matrix<double,3,3> M=r;Eigen::Quaterniond q(M);std::vector<double> v(4);v[0] = q.x();v[1] = q.y();v[2] = q.z();v[3] = q.w();//vector<float> q = Converter::toQuaternion(r);//cout<<"r=\n"<<r<<endl;//cout<<"R=\n"<<R<<endl;//cout<<"T(0,3)=\n"<<tvec.at<double>(0,0)<<endl;
//<< setprecision(6) << *lT << " " <<  setprecision(9)ofstream outfile("data.txt",ios::trunc);outfile.open("data.txt");//  f << tvec.at<double>[0] << " " << tvec.at<double>[1] << " " << tvec.at<double>[2] << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;outfile<< tvec.at<double>(0,0) << " " << rvec.at<double>(1,0) << " " << rvec.at<double>(2,0)<< " " << v[0] << " " << v[1] << " " << v[2] << " "<< v[3] << endl;outfile.close();//    cout<<" "<<Eigen::Matrix<double, 0, 1><<endl;
//    f.close();
//    cout << endl << "trajectory saved!" << endl

?

?

總結(jié)

以上是生活随笔為你收集整理的得到的旋转向量和平移向量转换成旋转矩阵 (SE(3))的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。