激光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中构建的超定方程组进行求解。

本次程序的运行过程为：

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

可能出现的问题

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

• source /home/nn/dev/lidar_space/devel/setup.bash

2）由于里程代码是跑在indigo上面的，但是我的是ros-kinetic，因此需要改一个地方：
/home/nn/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）编译出现的问题
①odom_calib/src/odometry_calibration/src/main.cpp:5:40: fatal error: nav_core/recovery_behavior.h: 没有那个文件或目录

解决办法：打开odom_calib/src/odometry_calibration/src/main.cpp，注释#nav_core/recovery_behavior.h。

② odom_calib/src/odometry_calibration/src/main.cpp:28:25: fatal error: csm/csm_all.h: 没有那个文件或目录

解决办法：安装ros-kinetic-csm。
sudo apt install ros-kinetic-csm

③make后出现缺少库的错误：No rule to make target ‘/usr/lib/x86_64-linux-gnu/libproj.so’

错误具体如下：

make[2]: *** No rule to make target '/usr/lib/x86_64-linux-gnu/libproj.so', needed by 'joinMap'.  Stop.
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/joinMap.dir/all' failed
make[1]: *** [CMakeFiles/joinMap.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

出现缺少库的情况，通常有两种原因：

• 此库并未被安装。
• 此库已安装，但并未链接到此工程。（如工程中需要库的名字是：libproj4.so。但我们实际拥有：libvtkproj4.so.5.10.1，这时需要做一个链接。）

解决办法：
在 /usr/lib下寻找是否有 libproj.so库，如果没有则下载；如果显示已安装，则需要将这个库链接到工程。

为了得到详细的错误原因，我们运行下面代码：

make --debug

得到如下运行结果：

GNU Make 4.1
Built for x86_64-pc-linux-gnu
Copyright (C) 1988-2014 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Reading makefiles...
Updating goal targets....
 File 'default_target' does not exist.
   File 'all' does not exist.
     File 'cmake_check_build_system' does not exist.
    Must remake target 'cmake_check_build_system'.
    Successfully remade target file 'cmake_check_build_system'.
  Must remake target 'all'.
GNU Make 4.1
Built for x86_64-pc-linux-gnu
Copyright (C) 1988-2014 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Reading makefiles...
Updating goal targets....
 File 'all' does not exist.
   File 'CMakeFiles/joinMap.dir/all' does not exist.
  Must remake target 'CMakeFiles/joinMap.dir/all'.
GNU Make 4.1
Built for x86_64-pc-linux-gnu
Copyright (C) 1988-2014 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Reading makefiles...
Updating goal targets....
 File 'CMakeFiles/joinMap.dir/depend' does not exist.
Must remake target 'CMakeFiles/joinMap.dir/depend'.
Scanning dependencies of target joinMap
Successfully remade target file 'CMakeFiles/joinMap.dir/depend'.
GNU Make 4.1
Built for x86_64-pc-linux-gnu
Copyright (C) 1988-2014 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Reading makefiles...
Updating goal targets....
 File 'CMakeFiles/joinMap.dir/build' does not exist.
   File 'joinMap' does not exist.
     File 'CMakeFiles/joinMap.dir/joinMap.cpp.o' does not exist.
    Must remake target 'CMakeFiles/joinMap.dir/joinMap.cpp.o'.
[ 50%] Building CXX object CMakeFiles/joinMap.dir/joinMap.cpp.o
    Successfully remade target file 'CMakeFiles/joinMap.dir/joinMap.cpp.o'.
     File '/usr/lib/x86_64-linux-gnu/libproj.so' does not exist.
    Must remake target '/usr/lib/x86_64-linux-gnu/libproj.so'.
make[2]: *** No rule to make target '/usr/lib/x86_64-linux-gnu/libproj.so', needed by 'joinMap'.  Stop.
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/joinMap.dir/all' failed
make[1]: *** [CMakeFiles/joinMap.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

重点在于其中的这两句：

File '/usr/lib/x86_64-linux-gnu/libproj.so' does not exist.
    Must remake target '/usr/lib/x86_64-linux-gnu/libproj.so'.

现在我们已确定 libproj.so库不存在，接下来去下载它。查找我们所需库的标准形式，执行指令如下：

apt-cache search libproj*

得到：

libproc-waitstat-perl - interpret and act on wait() status values
libprocess-cpp-dev - C++11 library for handling processes - dev headers and libraries
libprocess-cpp-doc - Documentation files for libprocess-cpp-dev
libprocess-cpp3 - C++11 library for handling processes - runtime library
libprocps4 - library for accessing process information from /proc
libprocps4-dev - library for accessing process information from /proc
libproperties-cpp-dev - C++11 library providing properties/signals. - dev headers
libproperties-cpp-doc - Documentation files for libproperties-cpp-dev
libprotobuf-dev - protocol buffers C++ library (development files)
libprotobuf-lite9v5 - protocol buffers C++ library (lite version)
libprotobuf9v5 - protocol buffers C++ library
libprotoc-dev - protocol buffers compiler library (development files)
libprotoc9v5 - protocol buffers compiler library
libproxy-cil-dev - automatic proxy configuration management library (CLI devel)
libproxy-dev - automatic proxy configuration management library (devel)
libproxy0.4-cil - automatic proxy configuration management library (CLI)
libproxy1-plugin-gsettings - automatic proxy configuration management library (GSettings plugin)
libproxy1-plugin-networkmanager - automatic proxy configuration management library (Network Manager plugin)
libproxy1v5 - automatic proxy configuration management library (shared)
libprobe-perl-perl - module to obtain information about the currently running Perl interpreter
libproc-background-perl - generic interface for Unix and Win32 background process management
libproc-daemon-perl - module for running scripts as daemons
libproc-fork-perl - Perl interface to the fork() system call
libproc-invokeeditor-perl - Perl extension for starting a text editor
libproc-pid-file-perl - Perl module for managing process id files
libproc-processtable-perl - Perl library for accessing process table information
libproc-queue-perl - Perl module to limit the number of child processes
libproc-reliable-perl - Perl module to run external processes reliably
libproc-simple-perl - Perl interface to launch and control background processes
libproc-syncexec-perl - spawn processes but report exec() errors properly
libproc-terminator-perl - module to conveniently terminate processes
libproc-wait3-perl - Perl interface to the wait3() system call
libprocesscore7 - library for monitoring your system - shared library
libprocessing-core-java - Java animation and interaction library
libprocessui7 - library for monitoring your system - shared library
libproj-dev - Cartographic projection library (development files)
libproj-java - Cartographic projection library (JNI bindings)
libproj9 - Cartographic projection library
libprojectm-dev - Advanced Milkdrop-compatible music visualization library - dev
libprojectm-qt-dev - projectM Qt4 (development files)
libprojectm-qt1v5 - projectM Qt4 bindings
libprojectm2v5 - Advanced Milkdrop-compatible music visualization library
libpromises3 - libpromises library for cfengine3 binaries
libproperties-maven-plugin-java - Maven Plugin to read and write property files from mojo.codehaus.org
libproperties-maven-plugin-java-doc - Documentation for Properties Maven Plugin
libprophet-perl - distributed, peer-to-peer replicated database system
libprotobuf-c-dev - Protocol Buffers C static library and headers (protobuf-c)
libprotobuf-c1 - Protocol Buffers C shared library (protobuf-c)
libprotobuf-c1-dbg - Protocol Buffers C shared library debug symbols (protobuf-c)
libprotobuf-java - Java bindings for protocol buffers
libprotocol-osc-perl - module to implement (de)coding and processing of OSC packets
libprotozero-dev - Minimalistic protocol buffer decoder and encoder in C++
libprotozero-doc - Minimalistic protocol buffer decoder and encoder in C++ - docs
libproxool-java - Java JDBC connection pool
libproxy-tools - automatic proxy configuration management library (tools)
libproxy1-plugin-kconfig - automatic proxy configuration management library (KConfig plugin)
libproxy1-plugin-webkit - automatic proxy configuration management library (Webkit plugin)
libproxychains-dev - proxy chains -- shared library (development)
libproxychains3 - proxy chains -- shared library (runtime)
projectm-data - Advanced Milkdrop-compatible music visualization library - data
protobuf-c-compiler - Protocol Buffers C compiler (protobuf-c)
python-libproxy - automatic proxy configuration management library (python)

得知所需库包含在 libproj-dev中，运行如下指令，进行安装：

sudo apt-get install libproj-dev

我们发现，文件make成功了，但还是报了第二个错误，如下：

Scanning dependencies of target joinMap
[ 50%] Building CXX object CMakeFiles/joinMap.dir/joinMap.cpp.o
[100%] Linking CXX executable joinMap
/usr/bin/ld: cannot find -lvtkproj4
collect2: error: ld returned 1 exit status
CMakeFiles/joinMap.dir/build.make:377: recipe for target 'joinMap' failed
make[2]: *** [joinMap] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/joinMap.dir/all' failed
make[1]: *** [CMakeFiles/joinMap.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

现在开始着手解决第二个错误。通过 make --debug得知，又是库的问题。所以我们首先在 /usr/lib里面使用通配符搜索 libvtkproj

nn@ubuntu:~$ cd /usr/lib
nn@ubuntu:/usr/lib$ ls libvtkproj*

得到如下结果：

libvtkproj4.so.5.10  libvtkproj4.so.5.10.1

从而得知系统中存在 libvtkproj4.so 文件，只是我们所运行的程序不认识此版本号（libvtkproj4.so.5.10.1）。所以只需要做一个库链接，就可以解决问题。

*系统中已存在一个链接库，可以看一下它效果：
libvtkproj4.so.5.10 是 libvtkproj4.so.5.10.1 的链接文件。

链接库的制作方法也很简单，只需要输入如下命令：

sudo ln -s libvtkproj4.so.5.10.1 libvtkproj4.so

usr/bin 多放置可执行文件；usr/lib 多放置库文件。

当缺少库时，我们首先去lib中检查。库文件（lib）的两种格式：.so; .a

三个函数

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

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

直接相减求位姿差是在世界坐标系中的结果，而激光雷达的scan-matching的位姿差需要以上一时刻的位姿作为参考系，因此需要将直接相减的结果做一次旋转。

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

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()

运行步骤

1. cd ~/lidar_space
2. mkdir src
3. cd src
4. catkin_init_workspace
5. 将calib_odom文件夹复制到/home/nn/lidar_space/src/文件目录下
6. cd …
7. 编译：catkin_make
8. source /devel/setup.bash
9. roslaunch calib_odom odomCalib.launch（注意：保证没有任何ROS节点在运行，roscore也要关闭。）
10. ①打开rviz
    rosrun rviz rviz
    ②配置rviz
    fix_frame选择为odom。在Add选项卡中增加三条Path消息，订阅的topic分别为：
    odom_path_pub_
    scan_path_pub_
    calib_path_pub_
    分别选择不同的颜色，用来区分路径。
    可以将此rviz的配置保存为odom_calib.rviz文件，然后在odomCalib.launch文件中添加：
    这样，在启动launch文件时，就自动打开了配置好的rviz。

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find calib_odom)/rviz_cfg/odom_calib.rviz" />

11. 播放bag
    rosbag play –clock odom.bag
    如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。
12. 矫正里程计
    当打印的数据到达一定的数量后，则可以开始矫正。这里，需要给calib_flag话题发布一个数据。

rostopic pub /calib_flag std_msgs/Empty "{}"

结果

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

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

输出bag文件的信息：

其中，/odom话题为里程计的pose，/sick_scan为2D激光雷达的数据，同时还发布了TF。激光雷达的pose是通过PI-ICP算法计算的，而里程计是直接从bag发布的。

获取里程计pose：

bool Scan2::getOdomPose(Eigen::Vector3d& pose, const ros::Time& t)
{
    // Get the robot's pose
    tf::Stamped<tf::Pose> ident(tf::Transform(tf::createQuaternionFromRPY(0,0,0),
                                               tf::Vector3(0,0,0)), t, base_frame_);
    tf::Stamped<tf::Transform> odom_pose;

    try
    {
        tf_.transformPose(odom_frame_, ident, odom_pose);
    }
    catch(tf::TransformException e)
    {
        ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
        return false;
    }

    double yaw = tf::getYaw(odom_pose.getRotation());

    pose << odom_pose.getOrigin().x(),
            odom_pose.getOrigin().y(),
            yaw;
    //pub_msg(pose, path_odom, odom_path_pub_);
    return true;
}

小结

理论上，相邻的两个时刻，激光雷达和里程计的位姿变换是相同的，而实际的估计值和真实值呈线性关系，因此此可以构建基于直接线性方法的最小二乘问题。

目前此项目，只是演示如何使用激光雷达纠正里程计的误差，属于后处理，并不适用于实际应用。