从零开始学自动驾驶（2）——前端里程计

最新推荐文章于 2024-04-23 21:25:39 发布

英雄小摔哥

最新推荐文章于 2024-04-23 21:25:39 发布

阅读量1k

点赞数 2

分类专栏：从零开始学习自动驾驶

本文链接：https://blog.csdn.net/weixin_51326570/article/details/118057023

版权

激光里程计 NDT 点云滤波前端匹配地图拼接

关键词由CSDN通过智能技术生成

从零开始学习自动驾驶专栏收录该内容

11 篇文章 27 订阅

订阅专栏

任佬代码tag4.0

目前只涉及到了前端的激光里程计，没有融合imu以及GNSS数据和后端优化/闭环检测等内容，所以此时建立的地图还处于吃什么吐什么的阶段。

前端里程计模块

主要完成的任务就是点云的匹配---->点云位姿----->变换点云位姿态------>拼接地图以及发布

其中还涉及对点云的滤波

/*
 * @Description: 前端里程计算法
 * @Author: Ren Qian
 * @Date: 2020-02-04 18:53:06
 */
#include "lidar_localization/front_end/front_end.hpp"

#include <cmath>
#include <pcl/common/transforms.h>
#include "glog/logging.h"

namespace lidar_localization {
FrontEnd::FrontEnd()
    :ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()),
     local_map_ptr_(new CloudData::CLOUD()),
     global_map_ptr_(new CloudData::CLOUD()),
     result_cloud_ptr_(new CloudData::CLOUD()) {

    // 给个默认参数，以免类的使用者在匹配之前忘了设置参数
    // 采用ndt（正态分布变化）的方法进行点云的匹配
    // 设置滤波时创建的体素大小为1cm立方体
    cloud_filter_.setLeafSize(1.3, 1.3, 1.3);
    local_map_filter_.setLeafSize(0.6, 0.6, 0.6);
    display_filter_.setLeafSize(0.5, 0.5, 0.5);
    //设置NDT网格网格结构的分辨率
    ndt_ptr_->setResolution(1.0);
    //为More-Thuente线搜索设置最大步长
    ndt_ptr_->setStepSize(0.1);
    //为终止条件设置最大转换差异
    ndt_ptr_->setTransformationEpsilon(0.01);
    //设置匹配迭代的最大次数
    ndt_ptr_->setMaximumIterations(30);
}

Eigen::Matrix4f FrontEnd::Update(const CloudData& cloud_data) {
    current_frame_.cloud_data.time = cloud_data.time;
    std::vector<int> indices;
    //移除无效点  removeNaNFromPointCloud（输入点云，输出点云，对应保留的索引）
    pcl::removeNaNFromPointCloud(*cloud_data.cloud_ptr, *current_frame_.cloud_data.cloud_ptr, indices);
    //cloud_ptr(new CLOUD()) 等价于 cloud_ptr = new CLOUD 给cloud_ptr 分配内存 
    CloudData::CLOUD_PTR filtered_cloud_ptr(new CloudData::CLOUD());
    //给当前帧进行滤波操作
    cloud_filter_.setInputCloud(current_frame_.cloud_data.cloud_ptr);
    cloud_filter_.filter(*filtered_cloud_ptr);
    //所有pose初始化为单位矩阵
    static Eigen::Matrix4f step_pose = Eigen::Matrix4f::Identity();
    static Eigen::Matrix4f last_pose = init_pose_;
    static Eigen::Matrix4f predict_pose = init_pose_;
    static Eigen::Matrix4f last_key_frame_pose = init_pose_;

    // 局部地图容器中没有关键帧，代表是第一帧数据
    // 此时把当前帧数据作为第一个关键帧，并更新局部地图容器和全局地图容器
    if (local_map_frames_.size() == 0) {
        current_frame_.pose = init_pose_;
        UpdateNewFrame(current_frame_);
        return current_frame_.pose;
    }

    // 不是第一帧，就正常匹配 滤波后的点云进行匹配
    ndt_ptr_->setInputSource(filtered_cloud_ptr);
    // 提取匹配后两帧之间的相对位姿态
    ndt_ptr_->align(*result_cloud_ptr_, predict_pose);
    current_frame_.pose = ndt_ptr_->getFinalTransformation();

    // 更新相邻两帧的相对运动
    //用上一帧的变换位姿，根据当前帧对下一帧进行预测
    step_pose = last_pose.inverse() * current_frame_.pose;
    predict_pose = current_frame_.pose * step_pose;
    //当前帧赋值给上一帧
    last_pose = current_frame_.pose;

    // 匹配之后根据距离判断是否需要生成新的关键帧，如果需要，则做相应更新
    // 两帧相差两 2 米生成一个关键帧
    if (fabs(last_key_frame_pose(0,3) - current_frame_.pose(0,3)) + 
        fabs(last_key_frame_pose(1,3) - current_frame_.pose(1,3)) +
        fabs(last_key_frame_pose(2,3) - current_frame_.pose(2,3)) > 2.0) {
        UpdateNewFrame(current_frame_);
        last_key_frame_pose = current_frame_.pose;
    }

    return current_frame_.pose;
}

bool FrontEnd::SetInitPose(const Eigen::Matrix4f& init_pose) {
    init_pose_ = init_pose;
    return true;
}

bool FrontEnd::SetPredictPose(const Eigen::Matrix4f& predict_pose) {
    predict_pose_ = predict_pose;
    return true;
}

void FrontEnd::UpdateNewFrame(const Frame& new_key_frame) {
    Frame key_frame = new_key_frame;
    // 这一步的目的是为了把关键帧的点云保存下来
    // 由于用的是共享指针，所以直接复制只是复制了一个指针而已
    // 此时无论你放多少个关键帧在容器里，这些关键帧点云指针都是指向的同一个点云
    //*new_key_frame.cloud_data.cloud_ptr（新帧的指针）重置了key_frame.cloud_data.cloud_ptr（老帧）的指针
    key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));
    CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());
    
    // 更新局部地图
    local_map_frames_.push_back(key_frame);
    //局部地图保留 20 帧
    while (local_map_frames_.size() > 20) {
        local_map_frames_.pop_front();
    }
    //重置指针
    local_map_ptr_.reset(new CloudData::CLOUD());
    for (size_t i = 0; i < local_map_frames_.size(); ++i) {
        //pcl::transformPointCloud（输入点云，输出点云， 转化位姿）
        pcl::transformPointCloud(*local_map_frames_.at(i).cloud_data.cloud_ptr, 
                                 *transformed_cloud_ptr, 
                                 local_map_frames_.at(i).pose);
        //转化后的点云加入到局部地图中
        *local_map_ptr_ += *transformed_cloud_ptr;
    }
    has_new_local_map_ = true;

    // 更新ndt匹配的目标点云
    if (local_map_frames_.size() < 10) {
        //导入局部地图作为待匹配点云
        ndt_ptr_->setInputTarget(local_map_ptr_);
    } else {
        //如果数量过多，滤波后在进行导入局部地图作为待匹配点云
        CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
        local_map_filter_.setInputCloud(local_map_ptr_);
        local_map_filter_.filter(*filtered_local_map_ptr);
        ndt_ptr_->setInputTarget(filtered_local_map_ptr);
    }

    // 更新全局地图
    global_map_frames_.push_back(key_frame);
    if (global_map_frames_.size() % 100 != 0) {
        return;
    } else {
        global_map_ptr_.reset(new CloudData::CLOUD());
        for (size_t i = 0; i < global_map_frames_.size(); ++i) {
            //转化后的点云加入到全局地图中
            pcl::transformPointCloud(*global_map_frames_.at(i).cloud_data.cloud_ptr, 
                                    *transformed_cloud_ptr, 
                                    global_map_frames_.at(i).pose);
            *global_map_ptr_ += *transformed_cloud_ptr;
        }
        has_new_global_map_ = true;
    }
}

bool FrontEnd::GetNewLocalMap(CloudData::CLOUD_PTR& local_map_ptr) {
    if (has_new_local_map_) {
        //对局部地图进行滤波
        display_filter_.setInputCloud(local_map_ptr_);
        display_filter_.filter(*local_map_ptr);
        return true;
    }
    return false;
}

bool FrontEnd::GetNewGlobalMap(CloudData::CLOUD_PTR& global_map_ptr) {
    if (has_new_global_map_) {
        //对全局地图进行滤波
        display_filter_.setInputCloud(global_map_ptr_);
        display_filter_.filter(*global_map_ptr);
        return true;
    }
    return false;
}

bool FrontEnd::GetCurrentScan(CloudData::CLOUD_PTR& current_scan_ptr) {
    //对当前帧点云进行滤波
    display_filter_.setInputCloud(result_cloud_ptr_);
    display_filter_.filter(*current_scan_ptr);
    return true;
}
}