tf变换及工具说明（3）之Eigen库使用

二维平面不同坐标系下增量转换

说明：机器人在A坐标系下上一时刻坐标a_pos_last，同时刻在B坐标系下坐标为b_pos_last。当前时刻时，机器人在A坐标系下运行至坐标为a_pos，则采用Eigen库可获取机器人当前时刻在B坐标系。在ros下，根据odometer的变化更新，机器人在世界坐标系下坐标，demo如下：

    //上时刻B坐标系下坐标，即世界坐标系下坐标
    Eigen::Vector3f last_scan_pose = b_pos_last;     
    //上时刻A坐标系下坐标，即里程计坐标系坐标                                                                          
    Eigen::Vector3f last_odom_pose = a_pos_last;      
    //当前时刻A坐标系下坐标，即里程计坐标系坐标                                                                       
    Eigen::Vector3f odom_pose = a_pos；      
    // 计算旋转向量，即沿着z轴旋转角度差                                                                                    
    Eigen::AngleAxisf rotation(last_scan_pose[2] - last_odom_pose[2], Eigen::Vector3f(0, 0, 1));      
    // 获取在B坐标系下坐标，即在世界坐标系下机器人当前坐标
    Eigen::Vector3f b_pos = last_scan_pose + rotation * (odom_pose - last_odom_pose);         

同理：在同一世界坐标系已知0时刻和1时刻的位姿pos0和pos1，则pos1与pos0之间的转换矩阵应为以其中0时刻为原点坐标（0,0,0），获取得到的位置向量为转换矩阵；故将其分成两部分实现，结果与上面一致，demo如下：

    //上时刻B坐标系下坐标，即世界坐标系下坐标
    Eigen::Vector3f last_scan_pose = b_pos_last;     
    //上时刻A坐标系下坐标，即里程计坐标系坐标                                                                          
    Eigen::Vector3f last_odom_pose = a_pos_last;      
    //当前时刻A坐标系下坐标，即里程计坐标系坐标                                                                       
    Eigen::Vector3f odom_pose = a_pos；     
    // 计算旋转向量，即沿着z轴旋转角度差
    Eigen::AngleAxisf rotation0( - last_odom_pose[2], Eigen::Vector3f(0, 0, 1));
    // 计算前后两个时刻坐标转换关系
    Eigen::Vector3f delta_odom_pos =  rotation0 * (odom_pose - last_odom_pose);

    // 计算旋转向量，即沿着z轴旋转角度差
    Eigen::AngleAxisf rotation1( last_scan_pose[2], Eigen::Vector3f(0, 0, 1));
    // 根据两次坐标关系和上时刻B坐标系下坐标，获取在当前时刻B坐标系下坐标，
    Eigen::Vector3f b_pos2 =  last_scan_pose + rotation1 * (delta_odom_pos);

二维下坐标转换

在平面坐标下，如已知A点在W下在坐标为$W_p（x，y，\theta ）$即所谓的$W_{A^T}$（即transform）。假设A_p点在A坐标下坐标为（x_a,y_b）;则$A_p$点在W下的坐标为$W_p$
　　采用矩阵转换，从ros的tf工具下转换Eigen库的demo如下：

 Eigen::Affine3d W_to_A_eig;                                  // 仿射变换矩阵
 tf::Transform W_to_A;
 tf::transformTFToEigen(W_to_A, W_to_A_eig);    // 转换eigen矩阵格式
 

2维平面坐标系下demo如下：

 // 根据激光当前位置（x,y, yaw）构建仿射变换矩阵
 Eigen::Affine2f transform(Eigen::Translation2f(x,y) * Eigen::Rotation2Df(yaw));
 // point 为在机器人坐标系下坐标
 Vector2f point;
 // 转换激光点在以机器人坐标系下的坐标
 Eigen::Vector2f transformed_point = transform * point;