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

最新推荐文章于 2025-06-14 22:23:49 发布

天际的鸟

最新推荐文章于 2025-06-14 22:23:49 发布

阅读量4.9k

点赞数

CC 4.0 BY-SA版权

分类专栏：一起动手做rgb-D

本文链接：https://blog.youkuaiyun.com/qq_34122731/article/details/91045704

一起动手做rgb-D 专栏收录该内容

9 篇文章

订阅专栏

本文详细介绍了如何使用OpenCV中的罗德里格斯公式将旋转向量转换为旋转矩阵，并利用cv2eigen函数将旋转矩阵从OpenCV格式转换为Eigen格式，最后结合平移向量构建SE(3)变换矩阵。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

理论过程
头文件说明
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);