激光SLAM学习——机器人的里程计标定（直接线性方法）

1 实践操作

1.1 本次的作业中,需要实现三个函数,分别为:

1. Main.cpp ,第 358 行中的 cal_delta_distance()函数,该函数的功能为给定两个里程计位姿,计
   算这两个位姿之间的位姿差。（路径：/odom_ws/src/calib_odom/src/main.cpp）

//求解得到两帧数据之间的位姿差
//即求解当前位姿在上一时刻坐标系中的坐标
Eigen::Vector3d  cal_delta_distance(Eigen::Vector3d odom_pose)
{
    static Eigen::Vector3d now_pos,last_pos;
    Eigen::Vector3d d_pos;  //return value
    now_pos = odom_pose;

    //TODO:
    d_pos = now_pos - last_pos;
    Eigen::AngleAxisd temp(last_pos(2),Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d trans=temp.matrix().inverse();
    d_pos=trans*d_pos;

    //end of TODO:
    
    last_pos = now_pos;
    return d_pos;
}

2. Odom_Calib.cpp ,第 28 行 Add_Data()函数,该函数的功能为构建超定方程组
   Ax=b ,具体参考 PPT。（路径：/odom_ws/src/calib_odom/src/Odom_Calib.cpp）

构建最小二乘需要的超定方程组
Ax = b
*/
bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{
    if(now_len<INT_MAX)
    {
        //TODO: 构建超定方程组
        Eigen::Matrix<double,3,9> temp_A;
        Eigen::Vector3d temp_b;
        temp_A << Odom(0), Odom(1),Odom(2),0,0,0,0,0,0,
           		 0,0,0,Odom(0), Odom(1),Odom(2),0,0,0,
            	 0,0,0,0,0,0,Odom(0), Odom(1),Odom(2);
        temp_b << scan(0),scan(1),scan(2);

        // 给到A和B
        A.block<3,9>(3*now_len,0) = temp_A;
        b.block<3,1>(3*now_len,0) = temp_b;

        //end of TODO
        now_len++;
        return true;
    }
    else
    {
        return false;
    }
}

3. Odom_Calib.cpp ,第 48 行 Solve() 函数,该函数的功能为对 2 中构建的超定方程组进行求解。(路径：/odom_ws/src/calib_odom/src/Odom_Calib.cpp)

 * 求解线性最小二乘Ax=b
 * 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;

    //TODO: 求解线性最小二乘

    Eigen::Matrix<double,9,1> X_ = A.colPivHouseholderQr().solve(b);
    correct_matrix << X_(0),X_(1),X_(2),X_(3),X_(4),X_(5),X_(6),X_(7),X_(8);
    //end of TODO

    return correct_matrix;
}

1.2 本次程序的运行过程为:

1. 实现上述的三个函数,在 odom_ws 下用 catkin_make 进行编译。
   

（如果编译过程中提示缺少 csm 库,可以用命令 sudo apt-get install ros-melodic-csm 进行安装。
如果你用的 ROS 版本为其它版本,则将 CMakeLists.txt 中所有 melodic 内容改为你的 ROS 版本。其它库安装方式也类似,比如缺少 nav_core,则用 sudo apt-get install ros-melodic-nav-core 安装。）
2. 在 odom_ws 下,进行 source,具体命令为:source devel/setup.bash。

（每次不用再source的操作：1.ctrl+h后打开 .bashrc ；2.在 # Set ROS melodic 下 加入一行 source ~/jg_slam（你自己在主目录下放的文件名）/devel/setup.bash ）

3. 运行 launch 文件:roslaunch calib_odom odomCalib.launch。执行本条指令的时候,必须保
   证没有任何 ros 节点在运行,roscore 也要关闭。
   

4. 在 3 正常的情况下,ctrl+alt+T 打开新终端，运行 rviz，fix_frame 选择为 odom 。在 Add 选项卡中增加三条 Path 消息。一条订阅的 topic 为:odom_path_pub_ ;一条订阅的 topic 为:scan_path_pub_;最后一条为:calib_path_pub_。分别选择不同的颜色。
   
   
   

5. 进入到 odom_ws/bag 目录下,运行指令: rosbag play --clock odom.bag。如果一切正常, 则能看到 运行矫正程序的终端会打印数据,并且 rviz 中可以看到 两条路径。当打印的数据到达一个的数量之后,则可以开始矫正。
   

6. 进行7前，还需检查在此路径中 /jg_slam/src/basicTransformStudy/basic_transform_study.cpp 中机器人的坐标转换代码是否完成；

#include 
#include 
#include 

using namespace std;

int main(int argc, char** argv)
{
    // 机器人B在坐标系O中的坐标：
    Eigen::Vector3d B(3, 4, M_PI);

    // 坐标系B到坐标O的转换矩阵：
    Eigen::Matrix3d TOB;
    TOB << cos(B(2)), -sin(B(2)), B(0),
           sin(B(2)),  cos(B(2)), B(1),
              0,          0,        1;

    // 坐标系O到坐标B的转换矩阵:
    Eigen::Matrix3d TBO = TOB.inverse();

    // 机器人A在坐标系O中的坐标：
    Eigen::Vector3d A(1, 3, -M_PI / 2);

    // 求机器人A在机器人B中的坐标：
    Eigen::Matrix3d TOA;
    // TODO 参照第一课PPT
    TOA << cos(A(2)), -sin(A(2)), A(0),
           sin(A(2)),  cos(A(2)), A(1),
              0,          0,        1;

    // start your code here (5~10 lines)
    Eigen::Matrix3d TBA;
    TBA=TBO*TOA;
    Eigen::Vector3d BA(TBA(0,2),TBA(1,2),atan2(TBA(1,0),TBA(0,0)));
    // end your code here
 
    cout << "The right answer is BA: 2 1 1.5708" << endl;
    cout << "Your answer is BA: " << TBA.transpose() << endl;

    return 0;
}

7. 矫正的命令为,在 calib_flag 的 topic 下发布一个数据:rostopic pub /calib_flag std_msgs/
   Empty “{}” 。（此处命令不要复制，自己手打）
   

8. 程序矫正完毕会输出对应的矫正矩阵,并且 会在 rviz 中显示出第三条路径,即 calib_path。可
   以观察里程计路径 odom_path 和矫正路径 calib_path 的区别来判断此次矫正的效果。
   

2 参考结果

可以看到，校正后的轨迹跟激光的轨迹接近了很多。