从零开始激光slam定位【1】- 激光里程计

里程计流程

1、如果当前帧是第一帧，那么将其看做关键帧，进行关键帧更新
2、关键帧更新都做了什么？更新local_map队列，更新local_map，更新ndt的target_cloud。
3、如果不是第一帧，那么将其与local_map进行ndt配准，配准的所需要的guess_pose是根据匀速运动模型进行计算的。
4、配准得到当前帧的pose，利用当前帧和上一阵的pose，结合匀速运动模型得到下一次配准的guess_pose；如果当前帧的pose相对于上一关键帧的pose距离较大，则更新关键帧。

front_end_node.cpp的注释

/*
 * @Descripttion: 对任乾大佬的tag4.0增加map--->lidar的tf变换，并可视化最原始的变换后的点云
 * @vision: 
 * @Author: suyunzzz
 * @Date: 2020-08-20 23:40:44
 * @LastEditors: Please set LastEditors
 * @LastEditTime: 2020-08-26 23:20:21
 */
/*
 * @Description: 
 * @Author: Ren Qian
 * @Date: 2020-02-05 02:56:27
 */

#include <ros/ros.h>
#include <pcl/common/transforms.h>

#include "glog/logging.h"

#include "lidar_localization/global_defination/global_defination.h"
#include "lidar_localization/subscriber/cloud_subscriber.hpp"
#include "lidar_localization/subscriber/imu_subscriber.hpp"
#include "lidar_localization/subscriber/gnss_subscriber.hpp"
#include "lidar_localization/tf_listener/tf_listener.hpp"
#include "lidar_localization/publisher/cloud_publisher.hpp"
#include "lidar_localization/publisher/odometry_publisher.hpp"
#include "lidar_localization/front_end/front_end.hpp"           // 前端相关的头文件

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

using namespace lidar_localization;

int main(int argc, char *argv[]) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
    FLAGS_alsologtostderr = 1;

    // 初始化节点
    ros::init(argc, argv, "front_end_node");
    ros::NodeHandle nh;

    std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
    std::shared_ptr<IMUSubscriber> imu_sub_ptr = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
    std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);

    // TF监听。lidar-->imu
    std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");   

    // 发布者，发布到frame_id="/map"
    std::shared_ptr<CloudPublisher> cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");  
    std::shared_ptr<CloudPublisher> raw_cloud_pub_ptr = std::make_shared<CloudPublisher>(nh,"raw_current_scan",100,"/map");
    std::shared_ptr<CloudPublisher> local_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "local_map", 100, "/map");
    std::shared_ptr<CloudPublisher> global_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "global_map", 100, "/map");

    // 发布odom信息，topic_name,base_frame_id,child_frame_id
    std::shared_ptr<OdometryPublisher> laser_odom_pub_ptr = std::make_shared<OdometryPublisher>(nh, "laser_odom", "map", "lidar", 100);
    std::shared_ptr<OdometryPublisher> gnss_pub_ptr = std::make_shared<OdometryPublisher>(nh, "gnss", "map", "lidar", 100);

    // 创建一个前端对象
    std::shared_ptr<FrontEnd> front_end_ptr = std::make_shared<FrontEnd>();

    // 数据队列
    std::deque<CloudData> cloud_data_buff;
    std::deque<IMUData> imu_data_buff;
    std::deque<GNSSData> gnss_data_buff;
    Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();
    bool transform_received = false;
    bool gnss_origin_position_inited = false;
    bool front_end_pose_inited = false;             // true 前端pose未初始化

    // 局部地图，全局地图，当前帧点云
    CloudData::CLOUD_PTR local_map_ptr(new CloudData::CLOUD());     
    CloudData::CLOUD_PTR global_map_ptr(new CloudData::CLOUD());
    CloudData::CLOUD_PTR current_scan_ptr(new CloudData::CLOUD());
    CloudData::CLOUD_PTR current_scan_ptr_raw(new CloudData::CLOUD());  // 当前帧变换后的结果，

    /****************************************************/
    // 以下为我新增的tf变换，map--->lidar
    // 发布tf变换 map--->lidar
    //static tf::TransformBroadcaster br;
    tf::TransformBroadcaster br_;          
    tf::Transform transform;
    tf::Quaternion q;
    struct pose{double x,y,z;
                double roll,pitch,yaw;};
    struct pose current_pose_;
    /***************************************************/
    
    double run_time = 0.0;
    double init_time = 0.0;
    bool time_inited = false;
    bool has_global_map_published = false; 

    ros::Rate rate(100);
    while (ros::ok()) {
        ros::spinOnce();

        // 数据读入
        cloud_sub_ptr->ParseData(cloud_data_buff);
        imu_sub_ptr->ParseData(imu_data_buff);
        gnss_sub_ptr->ParseData(gnss_data_buff);

        // 如果没有得到变换
        if (!transform_received) {
            if (lidar_to_imu_ptr->LookupData(lidar_to_imu)) {
                transform_received = true;
            }
        } else {
            while (cloud_data_buff.size() > 0 && imu_data_buff.size() > 0 && gnss_data_buff.size() > 0) {
                CloudData cloud_data = cloud_data_buff.front();
                IMUData imu_data = imu_data_buff.front();
                GNSSData gnss_data = gnss_data_buff.front();

                if (!time_inited) {
                    time_inited = true;
                    init_time = cloud_data.time;
                } else {
                    run_time = cloud_data.time - init_time;
                }

                double d_time = cloud_data.time - imu_data.time;
                if (d_time < -0.05) {       // cloud慢
                    cloud_data_buff.pop_front();
                } else if (d_time > 0.05) {     // cloud快
                    imu_data_buff.pop_front();
                    gnss_data_buff.pop_front();
                } else {                                    // 开始
                    cloud_data_buff.pop_front();            // 将使用的数据在队列中删除
                    imu_data_buff.pop_front();
                    gnss_data_buff.pop_front();

                    Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity();

                    if (!gnss_origin_position_inited) {
                        gnss_data.InitOriginPosition();
                        gnss_origin_position_inited = true;
                    }
                    gnss_data.UpdateXYZ();
                    odometry_matrix(0,3) = gnss_data.local_E;
                    odometry_matrix(1,3) = gnss_data.local_N;
                    odometry_matrix(2,3) = gnss_data.local_U;
                    odometry_matrix.block<3,3>(0,0) = imu_data.GetOrientationMatrix();
                    odometry_matrix *= lidar_to_imu;
                    gnss_pub_ptr->Publish(odometry_matrix);             // gnss发布里程计信息

                    if (!front_end_pose_inited) {       // 如果未初始化前端，将gnss的pose作为初始pose
                        front_end_pose_inited = true;
                        front_end_ptr->SetInitPose(odometry_matrix);        
                    }
                    front_end_ptr->SetPredictPose(odometry_matrix);                 // gnss的里程计作为预测（没有用到）
                    Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data);               // 更新当前帧，返回当前帧的pose
                    laser_odom_pub_ptr->Publish(laser_matrix);                  // lidar发布里程计信息（当前帧的pose）

                    /****************************************************/
                    // 以下为我新增的tf变换，map--->lidar
                    // 发布tf变换map--->lidar
                    // current_pose_写入transform
                    transform.setOrigin(tf::Vector3(laser_matrix(0,3), laser_matrix(1,3), laser_matrix(2,3)));   // transform 设置平移
                    tf::Matrix3x3 mat_b;
                    mat_b.setValue(static_cast<double>(laser_matrix(0, 0)), static_cast<double>(laser_matrix(0, 1)),
                                    static_cast<double>(laser_matrix(0, 2)), static_cast<double>(laser_matrix(1, 0)),
                                    static_cast<double>(laser_matrix(1, 1)), static_cast<double>(laser_matrix(1, 2)),
                                    static_cast<double>(laser_matrix(2, 0)), static_cast<double>(laser_matrix(2, 1)),
                                    static_cast<double>(laser_matrix(2, 2)));                    
                    mat_b.getRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw, 1);//mat2rpy  旋转
                    q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw); //q from rpy
                    transform.setRotation(q);//trans from q
                    br_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "lidar"));  // map--->scan的transform
                    /****************************************************/
                    
                    // 当前帧的点云
                    front_end_ptr->GetCurrentScan(current_scan_ptr);            // 变换到global下的当前帧点云
                    cloud_pub_ptr->Publish(current_scan_ptr);                   // 发布当前帧点云
                    front_end_ptr->GetCurrentScanRaw(current_scan_ptr_raw);        // 发布变换后的原始点云,未下采样
                    raw_cloud_pub_ptr->Publish(current_scan_ptr_raw);


                    if (front_end_ptr->GetNewLocalMap(local_map_ptr))           // 发布局部地图,只有遇到关键帧才能更新局部地图
                        local_map_pub_ptr->Publish(local_map_ptr);
                }
                if (run_time > 460.0 && !has_global_map_published) {            // 全局地图，只能发布一次，并且有时间限制
                    if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) {    //  每100个关键帧，才更新一次全局地图
                        global_map_pub_ptr->Publish(global_map_ptr);
                        has_global_map_published = true;
                    }
                }
            }
        }

        rate.sleep();
    }

    return 0;
}

可视化

黄色的代表gnss的里程计，红色的代表激光里程计，大的Axis的坐标系是lidar，小的坐标系是map。

debug

typename boost::detail::sp_dereference<T>::type boost::shared_ptr<T>::operator*() const [with T = pcl::PointCloud<pcl::PointXYZ>; typename boost::detail::sp_dereference<T>::type = pcl::PointCloud<pcl::PointXYZ>&]: Assertion `px != 0' failed

解决：
这个问题的原因是创建pcl点云指针的时候没有初始化，应该在构造函数中进行初始化。

参考

https://zhuanlan.zhihu.com/p/104791974