多传感器融合定位 第四章 点云地图构建及基于点云地图定位
代码下载
环境配置问题：
g2o 安装
出现如下问题,为g2o图优化库没安装好
// 建议下载高博slam14讲的2017g2o 库
https://github.com/jiajunhua/gaoxiang12-slambook/tree/master/3rdparty
// 安装依赖
sudo apt-get install cmake libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev
// 编译
cd g2o
mkdir build
cd build
cmake ..
make -j4
//安装
$ sudo make install
sudo ldconfig
protobuf 3.14.x 安装
cd protobuf-3.14.x
./autogen.sh
./configure
make
sudo make install
编译过程中出现如下报错
按照GeYao README中的方法，重新生成基于自己基环境protobuf的proto：
打开lidar_localization/config/scan_context文件夹，输入如下命令，生成pb文件
protoc --cpp_out=./ key_frames.proto
protoc --cpp_out=./ ring_keys.proto
protoc --cpp_out=./ scan_contexts.proto
mv key_frames.pb.cc key_frames.pb.cpp
mv ring_keys.pb.cc ring_keys.pb.cpp
mv scan_contexts.pb.cc scan_contexts.pb.cpp
分别修改生成的三个.pb.cpp文件。如下，以ring_keys.pb.cpp为例。
#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
#include "ring_keys.pb.h" 替换为 #include "lidar_localization/models/scan_context_manager/ring_keys.pb.h"
#include
之后，用以上步骤生成的的.pb.h文件替换lidar_localization/include/lidar_localization/models/scan_context_manager
中的同名文件。
将.pb.cpp文件替换（注意：需要剪切，确保config文件中新生成的文件都转移到对应目录下，不能重复出现）lidar_localization/src/models/scan_context_manager中的同名文件。
构建地图
source install/setup.bash
roslaunch lidar_localization mapping.launch
rosbag play kitti_lidar_only_2011_10_03_drive_0027_synced.bag
eg.构建地图过程可能会因为运行内存不足的问题，导致报错。因为本机配置为16G，启动构建地图程序时还需要把浏览器关掉，避免不必要的内存占用。
在运行过程中, 可以执行如下的命令, 保存地图与Loop Closure Data
source install/setup.bash
rosservice call /optimize_map
rosservice call /save_map
rosservice call /save_scan_context
上述三个ROS Service会生成所需的 Map与 Scan Context Data. 分别位于:
- Map: src/lidar_localization/slam_data/map
- Scan Context Data: src/lidar_localization/slam_data/scan_context
基于地图定位
参考博客：深蓝学院-多传感器融合定位-第4章作业
FILE : lidar_localization/src/matching/matching_flow.cpp
基于原点地图的初始化
源代码，已实现地图原点初始化，但是初始化的位姿为单位阵，并不准确，跑一会儿就会发散
bool MatchingFlow::UpdateMatching() {
if (!matching_ptr_->HasInited()) {
Eigen::Matrix4f init_pose = Eigen::Matrix4f::Identity();
matching_ptr_->SetInitPose(init_pose);
matching_ptr_->SetInited();
}
return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}
全局初始化
ScanContext位姿初始化
FILE : lidar_localization/src/matching/matching_flow.cpp
通过构建地图时保存的sc回环数据，进行初始定位搜寻位姿
bool MatchingFlow::UpdateMatching() {
if (!matching_ptr_->HasInited()) {
matching_ptr_->SetScanContextPose(current_cloud_data_);
}
return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}
FILE : lidar_localization/src/matching/matching.cpp
bool Matching::SetScanContextPose(const CloudData& init_scan) {
Eigen::Matrix4f init_pose = Eigen::Matrix4f::Identity();
if (
!scan_context_manager_ptr_->DetectLoopClosure(init_scan, init_pose)
) {
return false;
}
SetInitPose(init_pose);
has_inited_ = true;
return true;
}
效果：(分别播放数据集 100s 200s 300s 时的定位效果)
定位结果大致与GroundTruth 重合
roslaunch lidar_localization matching.launch
rosbag play kitti_lidar_only_2011_10_03_drive_0027_synced.bag -s 100
Gnss 位姿初始化
FILE : lidar_localization/src/matching/matching_flow.cpp
bool MatchingFlow::UpdateMatching() {
if (!matching_ptr_->HasInited()) {
matching_ptr_->SetGNSSPose(current_gnss_data_.pose);
}
return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}
FILE : lidar_localization/src/matching/matching.cpp
SetGNSSPose() 获取当前gnss 坐标，并通过与建图原点GNSS的坐标进行解算出初始位姿。
bool Matching::SetGNSSPose(const Eigen::Matrix4f& gnss_pose) {
static int gnss_cnt = 0;
current_gnss_pose_ = gnss_pose;
if ( gnss_cnt == 0 ) {
SetInitPose(gnss_pose);
} else if (gnss_cnt > 3) {
has_inited_ = true;
}
gnss_cnt++;
return true;
}
通过记录获得建图时gnss 原点坐标为：
latitude = 48.9825452359;
longitude = 8.39036610005;
altitude = 116.382141113;
将建图原点的”经纬高” 转换为到导航系(ENU系)下的原点
FILE: lidar_localization/src/sensor_data/gnss_data.cpp
void GNSSData::InitOriginPosition() {
geo_converter.Reset(48.982658, 8.390455, 116.396412);
origin_longitude = longitude;
origin_latitude = latitude;
origin_altitude = altitude;
origin_position_inited = true;
}
调用 InitOriginPosition
FILE: lidar_localization/src/apps/data_pretreat_node.cpp
bool DataPretreatFlow::InitGNSS() {
static bool gnss_inited = false;
if (!gnss_inited) {
GNSSData gnss_data = gnss_data_buff_.front();
gnss_data.InitOriginPosition();
gnss_inited = true;
}
return gnss_inited;
}
效果：(分别播放数据集 100s 200s 300s 时的定位效果)
定位结果大致与GroundTruth 重合
roslaunch lidar_localization matching.launch
rosbag play kitti_lidar_only_2011_10_03_drive_0027_synced.bag -s 100
Original: https://blog.csdn.net/weixin_41281151/article/details/120116838
Author: KaHoWong
Title: 多传感器融合定位 第四章 点云地图构建及基于点云地图定位
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/597776/
转载文章受原作者版权保护。转载请注明原作者出处！