建图定位与坐标变换2

坐标变换

1. A_to_B的问题（A2B）

  在建图定位初学者中，肯定有很多童鞋被坐标变换搞迷糊了，只要搞明白了坐标变换，建图定位上手或者在学习过程中会容易很多，首先解决谁to谁的问题。在上一篇博客中，参考slam十四讲，进行了扫盲。
假设我们定义两个坐标系1、坐标系2、向量a，那么向量 $a$ 在两个坐标系下的坐标为$a_1$, $a_2$,那么：

                  $a_2$ = $R_{21}{a}_1$ + $t_{21}$

这里的 $R_{21}$ 是指“把坐标系1的向量变换到坐标系2”中。也就是 1_to_2。此时可以求解得到向量a在坐标系2下的坐标$a2$。

2. 连续变换

变换矩阵 T ，连续变换不是一个线性关系，假设进行了两次变换：$R_1,t_1$ , $R_2,t_2$:
那么，从 a 到 c 的变换为：
利用变换矩阵T ：
依次左乘，前一个变换矩阵在右边，后续进行的变换矩阵 $T_2$ 左乘变换矩阵 $T_1$

3. 用于点的映射

  有两个坐标系， 坐标系W 和 坐标系A，那么 A 在 W 中的相对位姿可以用从 W 的原点到 A 的原点的平移量和A中坐标轴(xyz)在W中的旋转量来表示。
  如果将坐标位姿信息用于点的映射的话，那么$W_{A^T}$表示：将 $A$ 坐标系中的点转换到$W$坐标系中。其实，根据这个语法关系可以得出这样一个表达式：
                  $W_p=W_{A^T}\times A_p$
转换$W_{A^T}$的逆是转换$A_{W^T}$,点映射的逆是原转换的反方向。转换的逆可以通过btTransform的成员函数inverse()得到。

4. TF 举栗子

  在自家无人驾驶平台上平台上，车顶安装了三台Lidar，主激光安装支架正中央，支架的两边安装了两台侧激光，在完成多激光的位姿标定后，得到了两组TF，这两组TF在后续中如何使用呢。先上标定结果：

  上图为标定之后，将三台激光同时在rviz里显示，有的看到图片后会产生疑问，为什么没有点云，这里我解释一下，是视角问题，这一圈点云是打在了车顶上的点云，其它打在地上的点云，由于放大rviz截图，所以看不见了。定义各激光的frame_id。

• 主激光的frame_id : rslidar ;
• 左激光的frame_id: velodyne_left ;
• 右激光的frame_id: velodyne_right ;
  通过标定，得知rslidar与velodyne_left , rslidar与velodyne_right之间的TF。

0.|<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z qx qy qz qx link1 link2 100" />
1.|<node pkg="tf" type="static_transform_publisher" name="velodyne_right_to_rslidar" args="-0.00525205 -0.691748 -0.188128 0.00466339 0.00257724 0.505748  /rslidar /velodyne_right 10" />
2.|<node pkg="tf" type="static_transform_publisher" name="velodyne_left_to_rslidar" args="-0.00770524 0.787548 -0.0191263 0.00413675 -0.00719475 -0.54002  /rslidar /velodyne_left 10" />

• x y z ：代表 link2 相对与 link1 的位置变换。即：velodyne_right 相对于rslidar的位置变换
• yaw pitch roll 表示link2相对与link1的位姿变换，即：velodyne_right 相对于rslidar的旋转变换，其中 yaw 为绕 z 轴的旋转， pitch 为绕 y 轴的旋转， roll 为绕 x 轴的旋转，逆时针旋转为正，顺时针旋转为负。
• 参数 10 表示每隔10毫秒发布一次。

通过4*4变换矩阵T，若已知velodyne坐标系下的点的坐标，可以得到该点在rslidar坐标系下的坐标：
                 $R_p$ = $R_{V^T}\ast V_p$
$R_{V^T}$代表变换矩阵T，$V_p$ 代表velodyne坐标系下的向量，$R_p$ 代表rslidar坐标系下的向量。

5. 代码验证

/*节点说明：
  车体坐标系：base_link（base）
  主激光坐标系：rslidar （rs）
  侧激光坐标系：velodyne（vel）
*/

#include <ros/ros.h>
#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "trans_vv6_node");
    ros::NodeHandle nh;

    Eigen::Matrix4d Tf_rs_to_vel;                        //定义4*4的变换矩阵
    Eigen::Matrix4d Tf_base_to_rs;                   //定义4*4位姿变换矩阵
        
    Eigen::Translation3d tl_rs_to_vel(0.0,1.0,0.0);      //rstovel平移 
    Eigen::Vector3d  eulerAngle_rs_to_vel(0.0,0.0,M_PI/2);    //rstovel旋转角      
    
    Eigen::Translation3d tl_base_to_rs(0.0,1.0,0.0);     //basetors平移
    Eigen::Vector3d  eulerAngle_base_to_rs(0.0,0.0,0);   //basetors旋转

    Eigen::Vector4d a_vel;                //velodyne坐标系下的向量a_ve
    Eigen::Vector4d a_rs;                 //定义rslidar坐标系下的向量a_rs
    Eigen::Vector4d a_base;               //车体坐标系下的向量

    Eigen::AngleAxisd rollAngle_1(Eigen::AngleAxisd(eulerAngle_base_to_rs(2),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle_1(Eigen::AngleAxisd(eulerAngle_base_to_rs(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle_1(Eigen::AngleAxisd(eulerAngle_base_to_rs(0),Eigen::Vector3d::UnitZ()));
    Tf_base_to_rs = (tl_base_to_rs*yawAngle_1*pitchAngle_1*rollAngle_1).matrix();

    Eigen::AngleAxisd rollAngle_2(Eigen::AngleAxisd(eulerAngle_rs_to_vel(2),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle_2(Eigen::AngleAxisd(eulerAngle_rs_to_vel(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle_2(Eigen::AngleAxisd(eulerAngle_rs_to_vel(0),Eigen::Vector3d::UnitZ()));
    Tf_rs_to_vel = (tl_rs_to_vel*yawAngle_2*pitchAngle_2*rollAngle_2).matrix();

    
    //a_rs = Tf_rs_to_vel*a_vel;
    //a_base = Tf_base_to_rs*a_rs;
    //a_base=Tf_base_to_rs*Tf_rs_to_vel*a_vel;

    //a_vel = Tf_rs_to_vel.inverse()*a_rs;
    //a_base=Tf_base_to_rs*a_rs;

    //a_vel = (Tf_base_to_rs*Tf_rs_to_vel).inverse()*a_base;
    //Eigen::Vector4d a_vel_ = Tf_rs_to_vel.inverse()*Tf_base_to_rs.inverse()*a_base;

    

//***********通过vel下的向量计算出其在rs下的向量值

    a_vel << 1,1,1,1;     
    a_rs = Tf_rs_to_vel*a_vel;
    
    std::cout<<"a_vel: "<<a_vel.transpose()<<std::endl;
    std::cout<<"a_rs: "<<a_rs.transpose()<<std::endl; //a_rs 输出为（1,0,1,1）

    a_rs << 1,-1,1,1;
    a_vel = Tf_rs_to_vel.inverse()*a_rs;
    std::cout<<"a_vel: "<<a_vel.transpose()<<std::endl; //a_vel 输出为（1,1,2,1）
    std::cout<<"a_rs: "<<a_rs.transpose()<<std::endl;
    std::cout<<"***********************"<<std::endl;
//***********通过vel下的向量计算出其在a_base下的向量值

    a_vel << 1,1,1,1;
    a_rs = Tf_rs_to_vel*a_vel;
    a_base = Tf_base_to_rs*a_rs;
    Eigen::Vector4d a_base_ = Tf_base_to_rs*Tf_rs_to_vel*a_vel;

    std::cout<<"a_vel: "<<a_vel.transpose()<<std::endl; //1,1,1,1
    std::cout<<"a_rs: "<<a_rs.transpose()<<std::endl;   //1,0,1,1
    std::cout<<"a_base: "<<a_base.transpose()<<std::endl; //1,1,1,1
    std::cout<<"a_base_: "<<a_base_.transpose()<<std::endl;//1,1,1,1
    std::cout<<"***********************"<<std::endl;
    
    a_base << 1,0,1,1;
    a_rs = Tf_base_to_rs.inverse()*a_base;
    a_vel = (Tf_base_to_rs*Tf_rs_to_vel).inverse()*a_base;
    Eigen::Vector4d a_vel_ = Tf_rs_to_vel.inverse()*Tf_base_to_rs.inverse()*a_base;

    std::cout<<"a_base: "<<a_base.transpose()<<std::endl; //1,0,1,1
    std::cout<<"a_rs: "<<a_rs.transpose()<<std::endl;     //1,-1,1,1
    std::cout<<"a_vel: "<<a_vel.transpose()<<std::endl;   //1,1,2,1
    std::cout<<"a_vel_: "<<a_vel_.transpose()<<std::endl;//1,1,2,1
    
    ros::spin();

    return 0;
}