当前位置：首页 > 编程日记 > 正文

得到的旋转向量和平移向量转换成旋转矩阵 (SE(3))

编程日记 2024-10-14 16:10:00

理论过程
头文件说明
1.使用罗德里格斯公式需要包含头文件为#include<opencv2/calib3d.hpp>
2.使用函数cv2eigen需要包含头文件<opencv2/core/eigen.hpp>，但是在则之前要包含一个对eigen定义的头文件，否则会报eigen.hpp文件的错，所以使用cv2eigen函数应该这样写头文件

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

把旋转向量转换成旋转矩阵SO(3)，使用罗德里格斯公式进行转换

需要注意到的是solvepnp函数解出来的是旋转向量r,将r转换成r=theta*u;其中u是旋转向量进行单位化后的单位向量，theta是旋转向量的模，这个theta在计算正弦值是需要把这个theta当作弧度去计算而不是角度
实践
有旋转向量rvec如下

rvec=[-0.02387454801078471;
0.02952769731891602;
0.02088407602780516]

通过这一段代码，先把旋转向量rvec通过罗德里格斯公式计算得到旋转矩阵R，在把在cv下定义的旋转矩阵R转换到Eigen下定义的矩阵r

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

然后对应的有平移向量tvec如下

tvec=[0.005806541820330855;
0.001584643814482065;
0.003887003832579859]

通过下一段代码就能的到对应的旋转矩阵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);//先把旋转向量rvec通过罗德里格斯公式计算得到旋转矩阵R
Eigen::Matrix3d r;
cv::cv2eigen(R,r);//在把在cv下定义的旋转矩阵R转换到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);//先把旋转向量rvec通过罗德里格斯公式计算得到旋转矩阵REigen::Matrix3d r;cv::cv2eigen(R,r);//在把在cv下定义的旋转矩阵R转换到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

https://www.dkcj.cn/info/23026.html

得到的旋转向量和平移向量转换成旋转矩阵 (SE(3))

相关文章：

[译稿]同步复制提议 2010-09

STL vector

JavaScript 复习之数据类型

Nginx反代配置

STL queue priority_queue

镜头评价指标及测试方法(二)—畸变与分辨率

优秀Java程序员应该知道的20个实用开源库

sersync 同步

python-opencv 形态学

【 HihoCoder】1082 The Marshtomp has seen it all before （暴力或脑力）

java 多维数组转化为字符串

python-opencv 轮廓检测

JavaDate类

Jupyter Notebook使用教程

一次性理清JavaScript变量等高难度面试问题

yolov3（一：模型训练）

Ios生产证书申请（含推送证书）

【Python】SyntaxError: unexpected EOF while parsing

华为云家庭视频监控帮你一起守护家

SLAM之特征匹配（三）————RANSAC------LO-RANSAC Algorithm

【Codeforces】1136C Nastya Is Transposing Matrices （矩阵转置）

linux基础篇-02，linux时间管理date hwclock cal 简述

Spark shuffle调优

多传感器融合之滤波（一）——卡尔曼滤波（KF）推导

【HDU】1284 钱币兑换问题（想一想）

抓取网页的脚本【修复】

实例规格 ECS (共享计算型)和 (通用型-原独享)性能上有什么区别?...

yolov3（二：车牌识别）

[转]CSS hack大全详解

【Python】打印魔方阵