激光slam第4章前端配准

ChenXiaoFeng0420

已于 2022-09-27 21:04:44 修改

阅读量1.4k

点赞数 3

文章标签： c++

于 2022-09-21 19:19:03 首次发布

本文链接：https://blog.csdn.net/ChenXiaoFeng0420/article/details/126974839

版权

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 方法