多传感器融合定位 第四章 点云地图构建及基于点云地图定位
代码下载
环境配置问题:
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为例。
// Generated by the protocol buffer compiler. DO NOT EDIT!
// source: ring_keys.proto
#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
#include "ring_keys.pb.h" 替换为 #include "lidar_localization/models/scan_context_manager/ring_keys.pb.h"
#include <algorithm>
之后,用以上步骤生成的的.pb.h文件替换lidar_localization/include/lidar_localization/models/scan_context_manager
中的同名文件。
将.pb.cpp文件替换(注意:需要剪切,确保config文件中新生成的文件都转移到对应目录下,不能重复出现)lidar_localization/src/models/scan_context_manager中的同名文件。
构建地图
# set up session:
source install/setup.bash
# launch mapping:
roslaunch lidar_localization mapping.launch
# play ROS bag, lidar-only KITTI:
rosbag play kitti_lidar_only_2011_10_03_drive_0027_synced.bag
eg.构建地图过程可能会因为运行内存不足的问题,导致报错。因为本机配置为16G,启动构建地图程序时还需要把浏览器关掉,避免不必要的内存占用。
在运行过程中, 可以执行如下的命令, 保存地图与Loop Closure Data
# set up session:
source install/setup.bash
# force backend optimization:
rosservice call /optimize_map
# save optimized map:
rosservice call /save_map
# if you still use refence Scan Context Loop Closure implementation, execute this command.
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()) { // 第一帧点云数据
// naive implementation:
/*地图原点初始化 初始化设置 init_pose 为单位阵*/
Eigen::Matrix4f init_pose = Eigen::Matrix4f::Identity();
matching_ptr_->SetInitPose(init_pose);
matching_ptr_->SetInited();
/*利用ScanContext 进行位姿初始化*/
// matching_ptr_->SetScanContextPose(current_cloud_data_);
/*利用GNSS 进行位姿初始化*/
// matching_ptr_->SetGNSSPose(current_gnss_data_.pose);
}
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()) { // 第一帧点云数据
// naive implementation:
/*地图原点初始化 初始化设置 init_pose 为单位阵*/
//Eigen::Matrix4f init_pose = Eigen::Matrix4f::Identity();
//matching_ptr_->SetInitPose(init_pose);
//matching_ptr_->SetInited();
/*利用ScanContext 进行位姿初始化*/
matching_ptr_->SetScanContextPose(current_cloud_data_);
/*利用GNSS 进行位姿初始化*/
// matching_ptr_->SetGNSSPose(current_gnss_data_.pose);
}
return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}
FILE : lidar_localization/src/matching/matching.cpp
bool Matching::SetScanContextPose(const CloudData& init_scan) { // 利用闭环检测,找到定位的初始位姿
// get init pose proposal using scan context match:
Eigen::Matrix4f init_pose = Eigen::Matrix4f::Identity(); // 初始化位姿为单位阵
if (
!scan_context_manager_ptr_->DetectLoopClosure(init_scan, init_pose)
) {
return false;
}
// set init pose:
SetInitPose(init_pose);
has_inited_ = true;
return true;
}
效果:(分别播放数据集 100s 200s 300s 时的定位效果)
定位结果大致与GroundTruth 重合
roslaunch lidar_localization matching.launch
# play ROS bag, lidar-only KITTI:
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()) { // 第一帧点云数据
//
// TODO: implement global initialization here
//
// Hints: You can use SetGNSSPose & SetScanContextPose from matching.hpp
//
// naive implementation:
/*地图原点初始化 初始化设置 init_pose 为单位阵*/
// Eigen::Matrix4f init_pose = Eigen::Matrix4f::Identity();
// matching_ptr_->SetInitPose(init_pose);
// matching_ptr_->SetInited();
/*利用ScanContext 进行位姿初始化*/
//matching_ptr_->SetScanContextPose(current_cloud_data_);
/*利用GNSS 进行位姿初始化*/
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的坐标进行解算出初始位姿。
// TODO: understand this function
bool Matching::SetGNSSPose(const Eigen::Matrix4f& gnss_pose) { // 利用GNSS 数据找到定位的初始位姿,初始定位的GNSS值,需要在建图时保存
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 初始化
gnss_inited = true;
}
return gnss_inited;
}
效果:(分别播放数据集 100s 200s 300s 时的定位效果)
定位结果大致与GroundTruth 重合
roslaunch lidar_localization matching.launch
# play ROS bag, lidar-only KITTI:
rosbag play kitti_lidar_only_2011_10_03_drive_0027_synced.bag -s 100