【OpenREALM学习笔记：11】关于位姿矩阵从Sophus库的SE3类型转换为OpenCV库的Mat类型

隐匿活动的学习者2024-05-24 16:00

cpp 复制代码

#include <Eigen/Core>
#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

cv::Mat Tcw;
Sophus::SE3f  Tcw_SE3f = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();
cv::eigen2cv(Tcw_Matrix, Tcw);

第一行：声明一个OpenCV的Mat类型对象，用以存储相机在世界坐标系下的变换矩阵。Tcw表示从世界坐标系到相机坐标系的变换矩阵；

第二行：从SLAM系统中追踪单目相机的运动并获取位姿，而后将其存储在一个Sophus库的SE3f对象当中Tcw_SE3f当中。SE3f表示的是一个欧式群的单精度浮点类型，用以表示三维的旋转和平移；

第三行：将第二行的SE3f对象转换为4x4的浮点数矩阵，并将其赋值给Eigen::Matrix4f类型的矩阵对象；

第四行：这行代码主要用于将Eigen库的矩阵变换为OpenCV库的矩阵。

这里需要注意一点：在头文件的声明的时候，Eigen头文件的声明要放在opencv的eigen.hpp之前；否则，eigen.hpp头文件在程序运行的时候，会出现Eigen::XXXXXXX未定义的情况。