Tao's Blog
0%

lidarSLAM - 第四次作业

Posted on Edited on In Comment Counts:
5.8k 5 mins.

本文是我在学习深蓝学院的激光slam课程第四次作业的笔记。本次作业主要是熟悉 imls-icp 的过程以及对 csm 接口调用，除此之外还有对课上讲的四种 icp 算法进行总结以及想一下 icp 的改进思路。

实现两帧之间的 IMLS-ICP 激光匹配

补充代码,实现两帧间的 IMLS-ICP 激光匹配;

前提准备

编程环境： ubuntu 18.04 + ROS medodic
IDE: CLion, 安装官方教程配置：

  • 以项目方式打开想要编译的 package 文件夹中的 CMakeLists.txt
  • CMake设置中： Generation path 设置成 WORKSPACE_PATH/build
  • Cmake Option: -DCATKIN_DEVEL_PREFIX:PATH=WORKSPACE_PATH/devel

本次作业只给了模块代码，所以可以放在上次给的工作空间/自己的工作空间里编译运行。首先，需要安装好一些需要的模块：

  • champion_nav_msgs：这个模块在上一次作业也有用到，如果上次安装了这次就不用了，否则可以根据内部的 readme.md 自行编译安装，注意 ros 版本的差异更改对应文件的对应位置。
  • libnabo: 参考这篇教程
  • csm：第二次作业有用到，可以根据这篇教程选择自己喜欢的方式安装，安装完成后在 CMakeLists.txt 修改csm的对应路径位置。

另外注意一下 melodic 下安装的 pcl 版本貌似是1.8的，需要在对应 hpp， CMakeLists.txt 下修改需要的 pcl 版本。

代码结构

老规矩，还是先阅读给定的框架代码，了解代码结构再考虑补全。

main.cpp：和之前的作业类似，在一个定义好的类 imlsDebug 里面完成所有项目需要的基本函数（数据读取、初始化、话题订阅/发布），用另一个类 imls_icp 完成作业需要的功能（针对这次作业而言，就是激光数据配准）。

imlsDebug：六个函数和一些成员变量，主要功能如下：

  • 构造函数：初始化两条路径的发布话题信息（ imls-icp 生成的和 odom 生成的）、利用rosbag:View 来读取 rosbag 中的话题信息（激光和里程计数据），并逐条处理，手动进入相应的 callback函数。（rosbag 读取的 api 可以参考官方文档）

  • void odomCallback(...) 和 pubPath(const nav_msgs::OdometryConstPtr& msg, ...) : 简单的接收里程计数据并发布

  • void ConvertChampionLaserScanToEigenPointCloud(...)：工具函数，将激光扫描数据（距离，角度）换算成2d坐标（x, y）

  • void championLaserScanCallback(...)：先将激光数据转换成点云，调用 imls_icp 中的方法对上一阵点云数据进行匹配，匹配成功的话输出当前帧位姿（通过上一阵位姿乘相对位姿变换）并发布

  • void pubPath(Eigen::Vector3d& pose, nav_msgs::Path &path, ros::Publisher &mcu_path_pub_)：发布由 imls-icp 生成的位姿

imls_icp类中功能函数比较多，大部分函数名字和注释都能说明功能，这里不赘述，只介绍一下几个核心的功能函数：

  • projSourcePtToSurface(...) ：将当前帧点云投影到上一阵点云构造的曲面上（对每个店调用下面的函数），并去除匹配不合格的点，计算输出匹配后的点云和法向量

  • bool ImplicitMLSFunction(Eigen::Vector2d x, double& height)：将当前激光点投影到上一帧曲面上，计算高度；大致流程是利用 kd 数搜索距离当前点最近的三个点（需要具有代表性）构造平面，进行投影。

  • SolveMotionEstimationProblem()：已知匹配后的对应点和法向量，计算相对位姿，这部分已经写好了

我们需要完成的有三个函数：computeNormal()（给定某个点周围的 n 个点，计算法向量），ImplictMLSFunction() 的高度计算部分（参考课上学习的公式），projSourcePtToSurface()将匹配后的点转换成点云和法向量输出。

代码补充

  • ComputeNormal：补充如下，基本参考课上内容，利用 pca 求解法向量
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
/**
 * @brief IMLSICPMatcher::ComputeNormal
 * 计算法向量
 * @param nearPoints    某个点周围的所有激光点
 * @return
 */
Eigen::Vector2d IMLSICPMatcher::ComputeNormal(std::vector<Eigen::Vector2d> &nearPoints)
{
    Eigen::Vector2d normal;

    //根据周围的激光点计算法向量，参考ppt中NICP计算法向量的方法
    // 计算周围点的几何中心
    Eigen::Vector2d meanPoints = Eigen::Vector2d::Zero();
    for (auto p: nearPoints) {
        meanPoints += p;
    }
    meanPoints /= nearPoints.size();

    // 计算方差
    Eigen::Matrix2d sigma = Eigen::Matrix2d::Zero();
    for (auto p: nearPoints) {
        Eigen::Vector2d diff = p - meanPoints;
        sigma += diff * diff.transpose();
    }
    sigma /= nearPoints.size();

    // 特征分解
    Eigen::EigenSolver<Eigen::Matrix2d> solver(sigma);
    // 取出特征值和特征向量
    Eigen::MatrixXd eigenvalue = solver.eigenvalues().real();
    Eigen::MatrixXd eigenvector = solver.eigenvectors().real();
    Eigen::Index evalsMin;
    // 找到最小特征值的位置
    eigenvalue.rowwise().sum().minCoeff(&evalsMin);
    // 取出该列
    normal << eigenvector(0, evalsMin), eigenvector(1, evalsMin);

    return normal;
}
  • ImplicitMLSFunction：这个函数主要是补充高度计算部分，按照论文里的公式计算即可：
1
2
3
4
5
6
7
8
9
10
11
12
13
//TODO
//根据函数进行投影．计算height，即ppt中的I(x)
double sum1 = 0.0, sum2 = 0.0;
for (int i = 0; i < nearPoints.size(); i++) {
    auto p_i = nearPoints[i];
    auto normal_i = nearNormals[i];
    auto diff = x - p_i;
    double wi = exp(- (diff.squaredNorm()) / (m_h * m_h));
    sum1 += (wi * (diff.dot(normal_i)));
    sum2 += wi;
}
height = sum1 / sum2;
//end of TODO
  • projSourcePtToSurface 这个主要是补充计算匹配后的点云 yi，同样按照论文公式计算即可
1
2
3
4
//TODO
//计算yi．
yi = xi - height * nearNormal;
//end of TODO

运行结果

补充完成后运行结果如下所示：红色为根据 imls 算出的位姿轨迹，蓝色为 odom 的位姿轨迹。虽然由于没有进行后端优化导致后面轨迹飘了，但是整体来看大致方向是对的。

用 csm 进行激光匹配比较结果

将第一题 IMLS-ICP 匹配的接口换成第二次作业中 CSM 库的 ICP 匹配接口,并生成激光匹配的轨迹

代码修改

这一题主要是熟悉 csm 的接口并调用，主要是以下几个部分：

添加以下成员变量：

1
2
3
4
5
// csm 进行 pl-icp 需要的变量
bool run_pl_icp = true;
LDP m_prevLDP;
sm_params m_PIICPParams;
sm_result m_OutputResult;

添加 csm 相关的函数，注意部分变量的修改以及 LaserScan 和 ChampionNavLaserScan 之间的差异

1
2
3
4
5
6
7
8
9
10
11
12
13
// 进行pl-icp的相关函数
    void SetPIICPParams() {...}

    void LaserScanToLDP(const champion_nav_msgs::ChampionNavLaserScanConstPtr& pScan,
                        LDP& ldp) {
        int nPts = pScan->ranges.size(); // intensity 在 ChampionNavLaserScan 没用
        ...
        ldp->theta[i] = pScan->angles[i]; // ChampionNavLaserScan 不规定统一角度间隔，所以可以直接取
        ...
    }

    Eigen::Vector3d  PIICPBetweenTwoFrames(LDP& currentLDPScan,
                                           Eigen::Vector3d tmprPose) {...}

然后在构造函数中进行 csm 的相关设置：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
imlsDebug()
    {
        ...

        // 进行 pl-icp 相关设置
        m_prevLDP = NULL;
        SetPIICPParams();

        // 读取 rosbag 信息
        rosbag::Bag bag;
        bag.open(bagfile, rosbag::bagmode::Read);

        ...
    }

最后在激光数据的 callback 函数中增加使用 pl-icp 匹配的分支：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
if(m_isFirstFrame == true)
        {
            std::cout <<"First Frame"<<std::endl;
            m_isFirstFrame = false;
            m_prevLaserPose = Eigen::Vector3d(0, 0, 0);
            pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);
            // 针对不同匹配算法做不一样的设置
            if (!run_pl_icp) {
                ConvertChampionLaserScanToEigenPointCloud(msg, m_prevPointCloud);
            } else {
                LaserScanToLDP(msg, m_prevLDP);
            }
            return ;
        }

        if (!run_pl_icp) { // 使用 imls-icp 匹配

            ...

        } else { // 用 csm 进行 pl-icp

            //把当前的激光数据转换为 pl-icp能识别的数据 & 进行矫正
            //d_point_scan就是用激光计算得到的两帧数据之间的旋转 & 平移
            LDP currentLDP;
            LaserScanToLDP(msg, currentLDP);
            Eigen::Vector3d d_point_scan = PIICPBetweenTwoFrames(currentLDP, Eigen::Vector3d::Zero());
            Eigen::Matrix3d lastPose, rPose;

            lastPose << cos(m_prevLaserPose(2)), -sin(m_prevLaserPose(2)), m_prevLaserPose(0),
                        sin(m_prevLaserPose(2)),  cos(m_prevLaserPose(2)), m_prevLaserPose(1),
                        0,  0,  1;

            rPose << cos(d_point_scan(2)), -sin(d_point_scan(2)), d_point_scan(0),
                    sin(d_point_scan(2)),  cos(d_point_scan(2)), d_point_scan(1),
                    0,  0,  1;

            Eigen::Matrix3d nowPose = lastPose * rPose;
            m_prevLaserPose << nowPose(0, 2) , nowPose(1, 2), atan2(nowPose(1, 0), nowPose(0, 0));
            pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);

        }
    }

pl-icp 运行结果

运行结果如下所示，使用 pl-icp 的速度明显变快，但由于没有优化结果同样也发生了漂移。

比较各类 ICP 算法的特点以及异同

阅读 ICP 相关论文,总结课上所学的几种 ICP 及其相关变型并简述其异同(ICP,PL-ICL,NICP,
IMLS-ICP)

课上讲解的几种 ICP 算法的基本思路都是类似的：找到前后两帧激光扫描数据的匹配关系，并根据匹配关系迭代找到相对位姿的最优值，主要的差异就是寻找对应点（匹配关系）以及计算误差两方面，几种 ICP 算法的详细说明可以参考我的上篇博客。

ICP 算法改进思路

简答题,开放性答案:现在你已经了解了多种 ICP 算法,你是否也能提出一种改进的 ICP 算法,或能提
升 ICP 总体匹配精度或速度的技巧?请简述你的改进策略。

两个方面：

  • 精度上可以采用合理的方法提供初值（目前是人工设置）：其他传感器（odom等），用 Ransac 去除 outlier
  • 效率上可以对点云采样（提高匹配速度）