aruco二维码降落中的坐标系变换

二维码降落中的坐标系

变换矩阵的说明
以下面这个变换矩阵为例，一个变换矩阵可以有两种理解方式，可以理解为机体坐标系在世界坐标系下的位姿，最右边的写法可以体现这个含义，也可以说是从机体系到世界系的变换矩阵，最左边的写法可以体现这个含义，因为变换矩阵右边乘的是变换前的坐标系坐标，左边得到的是变换后的坐标系坐标，所以写为Twb正好顺序一致，念的时候也是先念b再念w。
 

二维码降落中的坐标系变换
 

 

基于无人机对的世界系位姿和aruco二维码检测结果即aruco二维码在相机系中的位姿，可以最终解算出aruco二维码在世界系中的位姿。

Tbc的计算过程

Tbc的含义,可以理解为相机坐标系在机体系下的位姿即旋转和平移。是从相机系到机体系的变换矩阵，注意是相机系到机体系不是机体系到相机系。
确定Rbc和tbc
对于坐标系间的旋转角度是90度的倍数时，可以直接观察坐标轴的对应关系或者取坐标轴的特定坐标值的方式快速确定坐标系间的旋转矩阵。默认基于左乘，位置为列向量。
 

tbc 这里默认(0 , 0 , 0) ,顺序依次对应x y z

代码实现关键Eigen

变换矩阵用Eigen::Isometry3d类型
旋转矩阵用Eigen::Matrix3d类型
平移向量用Eigen::Vector3d类型
四元数用Eigen::Quaterniond类型

https://blog.csdn.net/u011092188/article/details/77430988

以下代码示例取自aruco_land_demo.cpp

Eigen::Isometry3d Twa,Tca,Tbc,Twb,Twc;  //变换矩阵

Eigen::Matrix3d rotation_matrix_bc,rotation_matrix_wb,rotation_matrix_ca;;

Eigen::Vector3d t_bc,t_wb,t_ca;

Eigen::Quaterniond q_wb,q_ca;

Vector3d类型和Matrix3d类型的直接赋值

    t_bc <<  0, 0, 0;
    rotation_matrix_bc << 0, -1, 0,
                         -1 , 0, 0,
                          0, 0, -1;

四元数转旋转矩阵

rotation_matrix_ca = q_ca.toRotationMatrix();

基于旋转矩阵和平移向量给变换矩阵赋值

            Tca.linear() = rotation_matrix_ca;
            Tca.translation() = t_ca;

取出变换矩阵里的平移向量

Vector3d t_wa = Twa.translation();

访问平移向量的某一项

Vector3d t_wa;
t_wa[0];
t_wa.x();

变换矩阵相乘

Twa = Twb * Tbc * Tca;