多传感器融合定位 第四章 点云地图构建及基于点云地图定位

多传感器融合定位 第四章 点云地图构建及基于点云地图定位

代码下载

https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/%E7%AC%AC%E5%9B%9B%E7%AB%A0%20%E7%82%B9%E4%BA%91%E5%9C%B0%E5%9B%BE%E6%9E%84%E5%BB%BA%E5%8F%8A%E5%9F%BA%E4%BA%8E%E5%9C%B0%E5%9B%BE%E7%9A%84%E5%AE%9A%E4%BD%8D/lidar_localization

环境配置问题：

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 安装

protobuf3.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