第二章 里程计运动模型及标定

作业要求

---

1. 本次的作业为用直接线性方法来对机器人的里程计进行校正。
2. 给出的文件中包含有本次作业使用的bag数据，路径为odom_ws/bag/odom.bag。
3. 本次的作业中，需要实现三个函数，分别为：
4. Main.cpp，第340行中的cal_delta_distance()函数，该函数的功能为给定两个里程计位姿，计算这两个位姿之间的位姿差。
5. Odom_Calib.cpp，第23行Add_Data()函数，该函数的功能为构建超定方程组Ax=b，具体参考PPT。
6. Odom_Calib.cpp，第44行 Solve()函数，该函数的功能为对2中构建的超定方程组进行求解。

本次程序的运行过程为：
7. 实现上述的三个函数，并且进行编译。
8. 在odom_ws下，进行source：source devel/setup.bash
9. 运行launch文件:roslaunch calib_odom odomCalib.launch。执行本条指令的时候，必须保证没有任何ros节点在运行，roscore也要关闭。
10. 在3正常的情况下，运行rviz，fix_frame选择为odom_frame。在Add选项卡中增加三条Path消息。一条订阅的topic为：odom_path_pub_;一条订阅的topic为:scan_path_pub_；最后一条为:calib_path_pub_。分别选择不同的颜色。
11. 进入到odom_ws/bag目录下，运行指令：rosbag play –clock odom.bag。
12. 如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。当打印的数据到达一个的数量之后，则可以开始矫正。
13. 矫正的命令为，在calib_flag的topic下发布一个数据：rostopic pub /calib_flag std_msgs/Empty 。
14. 程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib_path。可以观察里程计路径odom_path和矫正路径_calib_path区别来判断此次矫正的效果。
15.

---

注意事项

（1）
建议自己新建一个工作空间，然后初始化，比如这里我就新建了一个工作空间lidar_space。然后在~/.bashrc中加入

source /home/mjy/dev/lidar_space/devel/setup.bash

建后将功能包calib_odom直接拷贝到此工作空间src之下。

（2）
由于里程代码是跑在indigo上面的，但是我的是ros-kinetic，因此需要改一个地方：
/home/mjy/dev/lidar_space/src/calib_odom/CMakeLists.txt
文件中搜索到indigo的地方，均换为kinetic。

# 第126行
 /opt/ros/kinetic/include/csm
 # 第150行
/opt/ros/kinetic/lib/libcsm.so

如果不修改，则编译的时候会报错：No rule to make target '/opt/ros/indigo/lib/libcsm.so'

（3）
pcl包是依赖Eigen的，而我是新装的eigen，路径在usr/local/include之下，因此pcl是找不到eigen的。
当然如果你是默认eigen，则无须理会。

所以，我这种特殊情况要修改pcl的源代码：

cd /opt/ros/kinetic/share/pcl_conversions/cmake
sudo gedit pcl_conversionsConfig.cmake 
cd /opt/ros/kinetic/share/pcl_ros/cmake
sudo gedit pcl_rosConfig.cmake 

将上述两个文件中的eigen搜寻路径改为自己的。

---

答案

答案分享给大家：

cal_delta_distance()函数

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 = last_pos.inverse() * now_pos;
    // d_pos = now_pos - last_pos;是不对的
    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;
}

一开始以为要求逆运算，但实际上这是个二维平面的运动，所以之间位姿相减就可以

2020.7.24更正

十分感谢评论区chyou510的更正！
直接相减求位姿差是在世界坐标系中的结果，而激光雷达的scan-matching的位姿差需要以上一时刻的位姿作为参考系，因此需要将直接相减的结果做一次旋转。这个是我一开始没有考虑周全。

Add_Data()函数

bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{

    if(now_len<INT_MAX)
    // if(now_len<data_len)
    {
        //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;
    }
}

Eigen块赋值操作 学习：https://www.cnblogs.com/wangxiaoyong/p/8906213.html
或我的blog：https://blog.csdn.net/weixin_44684139/article/details/105514563

Solve()函数

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;
}

因为是超定方程，并且直接套公式的话容易放大误差，因此用到了QR分解求线性方程组。colPivHouseholderQr()

然后编译—roslaunch–rviz–rostopic pub这些都按照作业要求走即可。

rviz画面，中间蓝线为标定矫正后的结果。

标定结果：

correct_matrix:
   0.946384    -1.64479   0.0124617
 -0.0709081    -1.04267  0.00173608
-0.00674715   0.0415879   0.0619791
calibration over!!!!