激光slam第4章前端配准

1、安装

（出现的问题与解决方法）：

1.1 使用系统为 Ubuntu 20.04+ ROS noetic

将工程文件中的 knetic 全部替换为 noetic

1.2 安装 libnabo 库及 libpointmatcher 库

报错1

sudo apt install ros-noetic-libnabo

sudo apt install ros-noetic-libpointmatcher

1.3 报错2 pcl问题本系统使用是的 pcl 1.10

在 Project 中的 CatkinList.txt 文件中，将 PCL 版本改为 1.10

同时将 imsl-icp 的 src 中，将头文件.h 中的 PCL 更为 1.10

1又报错

error: no match for call to ‘(pcl::getFieldIndex(const pcl::PCLPointCloud2&, const string&)::<lambda(int)>) (const pcl::PCLPointField&)’

类似一堆，调式了2个多小时，一定要看日记，看到c++11反应过来

set(CMAKE_CXX_FLAGS “${CMAKE_CXX_FLAGS} -std=c++14 -O3”)

2接着报错

许多未引用

/usr/bin/ld: CMakeFiles/imlsMatcher_node.dir/src/imls_icp.cpp.o: in function `IMLSICPMatcher::setTargetPointCloud(std::vector<Eigen::Matrix<double, 2, 1, 0, 2, 1>, std::allocator<Eigen::Matrix<double, 2, 1, 0, 2, 1> > >&)’:

/home/cxf/laser_demo/one_Project/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:118: undefined reference to `Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::createKDTreeLinearHeap(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, unsigned int, Nabo::Parameters const&)’

/usr/bin/ld: CMakeFiles/imlsMatcher_node.dir/src/imls_icp.cpp.o: in function `IMLSICPMatcher::ImplicitMLSFunction(Eigen::Matrix<double, 2, 1, 0, 2, 1>, double&)’:

/home/cxf/laser_demo/one_Project/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:144: undefined reference to `Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::createKDTreeLinearHeap(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, unsigned int, Nabo::Parameters const&)’

/usr/bin/ld: /home/cxf/laser_demo/one_Project/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:156: undefined reference to `Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::knn(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<int, -1, 1, 0, -1, 1>&, Eigen::Matrix<double, -1, 1, 0, -1, 1>&, int, double, unsigned int, double) const’

/usr/bin/ld: CMakeFiles/imlsMatcher_node.dir/src/imls_icp.cpp.o: in function `IMLSICPMatcher::projSourcePtToSurface(std::vector<Eigen::Matrix<double, 2, 1, 0, 2, 1>, std::allocator<Eigen::Matrix<double, 2, 1, 0, 2, 1> > >&, std::vector<Eigen::Matrix<double, 2, 1, 0, 2, 1>, std::allocator<Eigen::Matrix<double, 2, 1, 0, 2, 1> > >&, std::vector<Eigen::Matrix<double, 2, 1, 0, 2, 1>, std::allocator<Eigen::Matrix<double, 2, 1, 0, 2, 1> > >&)’:

/home/cxf/laser_demo/one_Project/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:244: undefined reference to `Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::knn(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<int, -1, 1, 0, -1, 1>&, Eigen::Matrix<double, -1, 1, 0, -1, 1>&, int, double, unsigned int, double) const’

/usr/bin/ld: CMakeFiles/imlsMatcher_node.dir/src/imls_icp.cpp.o: in function `IMLSICPMatcher::Match(Eigen::Matrix<double, 3, 3, 0, 3, 3>&, Eigen::Matrix<double, 3, 3, 0, 3, 3>&)’:

/home/cxf/laser_demo/one_Project/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:470: undefined reference to `Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::knn(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<int, -1, 1, 0, -1, 1>&, Eigen::Matrix<double, -1, 1, 0, -1, 1>&, int, double, unsigned int, double) const’

collect2: error: ld returned 1 exit status

修改

解决办法 1：在 CatkinList.txt 中添加编译引用${libnabo_LIBRARIES} libnabo::nabo

解决办法 2：在使用 libnabo 库函数的程序中添加引用即#include<libnabo>

1. 补充代码，实现两帧间的 IMLS-ICP 激光匹配；（6 分）的理解并通过代码进行实现

每一个点的平方差后全部累加, 为2*1 *1*2 最后得2*2

elfAdjointEigenSolver 计算特征值和特征向量需要特征分解，确保矩阵是自伴矩阵

ImplicitMLSFunction：函数

//根据函数进行投影．计算height，即ppt中的I(x)

projSourcePtToSurface()函数

编译后启动 roscore ，然后在另一个终端进行 source ，并运行命令 rosrun imlsMatcher

imlsMatcher_node 启动节点。

之后启动 rviz 可以查看激光和里程计的轨迹

ros配置jason文件快捷键

{

// 有关 tasks.json 格式的文档，请参见

// https://go.microsoft.com/fwlink/?LinkId=733558

“version”: “2.0.0”,

“tasks”: [

{

“label”: “catkin_make:debug”, //代表提示的描述性信息

“type”: “shell”, //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行

“command”: “catkin_make”,//这个是我们需要运行的命令

“args”: [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”

“group”: {“kind”:”build”,”isDefault”:true},

“presentation”: {

“reveal”: “always”//可选always或者silence，代表是否输出信息

},

“problemMatcher”: “$msCompile”

}

]

}

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

我们这里说将 IMSL-ICP 的方法改为 CSM 库中 ICP 的方法，根据现有程序有两种思路

（一）在 main.cpp 中写入 if 判断，用来选择使用 IMSL-ICP / PL-ICP 帧间匹配算法，即将 PLICP 方法写为子函数，遵循现有的程序框架。

（二）在 main.cpp 中将 IMSL-ICP 进行注释，将 PL-ICP 写入 main.cpp 中。

这里采用第二种基本思路，具体工作为，使用 if、else 注释 IMSL-ICP（可实现思路一），初始化

PL-ICP，将雷达数据转换为 PL-ICP 所需数据格式，写入 PL-ICP 帧间匹配算法，修改回调函数。

这里实际上使用的是作业 2 中 PL-ICP 的移植，作业 2 中的代码 PL-ICP 在代码中依旧为 PIICP，这里未作更改

第一步：注释 IMSL-ICP 方法