深藍學院激光slam學習——第二章（里程計標定）習題

第二章 里程計運動模型及標定

作業要求

---

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;

    //end of TODO:


    last_pos = now_pos;

    return d_pos;
}

一開始以爲要求逆運算，但實際上這是個二維平面的運動，所以之間位姿相減就可以

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!!!!