融合定位学习--构建点云地图

点云地图构建及基于点云地图定位

环境问题

出现下图问题是g20库问题：

git clone https://github.com/jiajunhua/gaoxiang12-slambook.git

cd gaoxiang12-slambook/3rdparty/g2o/
mkdir build
cd build
cmake ..
make
sudo make install

编译问题

通信时传递消息使用的是protobuf，需要在本地重新生成.pb文件
打开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文件放到对应位置
头文件放到lidar_localization/models/scan_context_manager/
源文件放到src/models/scan_context_manager/

地图构建

source devel/setup.bash
roslaunch lidar_localization mapping.launch
rosbag play kitti_2011_10_03_drive_0027_synced.bag

在运行过程中, 可以执行如下的命令, 保存地图与Loop Closure Data

# set up session:
source devel/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 

启动了这三个service后会将地图保存在：
src/lidar_localization/slam_data/map 和 src/lidar_localization/slam_data/scan_context目录下

基于地图的定位

框架中已经实现了初始位姿为单位阵的定位，但是不准确，跑一会就会发散。

bool MatchingFlow::UpdateMatching() {
    if (!matching_ptr_->HasInited()) {
        //
        // TODO: implement global initialization here
        //
        // Hints: You can use SetGNSSPose & SetScanContextPose from matching.hpp
        //

        // naive implementation:
        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位姿初始化

此部分代码框架已经实现，调用即可

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  init pose*/
        matching_ptr_->SetScanContextPose(current_cloud_data_);
        /*GNSS init pose*/
        //matching_ptr_->SetGNSSPose(current_gnss_data_.pose);
    }

    return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}

从100s播放bag

rosbag play kitti_2011_10_03_drive_0027_synced.bag -s 100

GNSS位姿初始化

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  init pose*/
        //matching_ptr_->SetScanContextPose(current_cloud_data_);
        /*GNSS init pose*/
        matching_ptr_->SetGNSSPose(current_gnss_data_.pose);
    }

    return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}

将建图时的原点转换到导航坐标系

void GNSSData::InitOriginPosition() {
    geo_converter.Reset(48.982545,  8.390366, 116.382141);  
   //geo_converter.Reset(latitude,  longitude, altitude);  

    origin_longitude = longitude;
    origin_latitude = latitude;
    origin_altitude = altitude;

    origin_position_inited = true;
}

100s播放bag

200s播放bag