点云融合定位简析-优快云博客

本文链接：https://blog.youkuaiyun.com/xiner0114/article/details/140247282

文章目录

点云融合定位框架
- 框架图
框架初始化
- 主程序main
- 初始化
lidar和IMU同步
RTK处理
- RTK初始化网格搜索
IMU处理
点云处理

结合书籍《自动驾驶与机器人中的SLAM技术：从理论到实践》和配套源码，来理解基于ESKF的融合定位。

点云融合定位框架

框架图

在这里插入图片描述

框架初始化

主程序main

创建ESKF融合器，并初始化；
将gnss、IMU、点云消息发送给融合算法；

src/ch10/run_fusion_offline.cc

int main(int argc, char** argv) {
    ...
    sad::Fusion fusion(FLAGS_config_yaml);
    if (!fusion.Init()) {
        return -1;
    }
    ...
    /// 把各种消息交给fusion
    rosbag_io
        .AddAutoRTKHandle([&fusion](GNSSPtr gnss) {
            fusion.ProcessRTK(gnss);
            return true;
        })
        .AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {
            fusion.ProcessPointCloud(cloud);
            return true;
        })
        .AddImuHandle([&](IMUPtr imu) {
            fusion.ProcessIMU(imu);
            return true;
        })

    sleep(10);
    return 0;
}

初始化

融合器相关配置，例如IMU静态初始化、ndt分辨率设置；
加载地图相关设置，例如原点设置、地图源相关的配置、地图目录等；
lidar外参、IMU外参；
lidar与IMU同步；

src/ch10/fusion.cc

Fusion::Fusion(const std::string& config_yaml) {
    config_yaml_ = config_yaml;
    StaticIMUInit::Options imu_init_options;
    imu_init_options.use_speed_for_static_checking_ = false;  // 本节数据不需要轮速计
    imu_init_ = StaticIMUInit(imu_init_options);
    ndt_.setResolution(1.0);
}

bool Fusion::Init() {
    // 地图原点
    auto yaml = YAML::LoadFile(config_yaml_);
    auto origin_data = yaml["origin"].as<std::vector<double>>();
    map_origin_ = Vec3d(origin_data[0], origin_data[1], origin_data[2]);

    // 地图目录
    data_path_ = yaml["map_data"].as<std::string>();
    LoadMapIndex();// 读取地图的索引文件

    // lidar和IMU消息同步
    sync_ = std::make_shared<MessageSync>([this](const MeasureGroup& m) { 
    ProcessMeasurements(m); // 处理同步之后的IMU和雷达数据
    });
    sync_->Init(config_yaml_);

    // lidar和IMU外参
    std::vector<double> ext_t = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
    std::vector<double> ext_r = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
    Vec3d lidar_T_wrt_IMU = math::VecFromArray(ext_t);
    Mat3d lidar_R_wrt_IMU = math::MatFromArray(ext_r);
    TIL_ = SE3(lidar_R_wrt_IMU, lidar_T_wrt_IMU);
    
   ...
   
    return true;
}

lidar和IMU同步

处理同步之后的IMU和雷达数据

使用IMU初始化
对IMU状态进行预测
点云去畸变
执行一次配准和观测

void Fusion::ProcessMeasurements(const MeasureGroup& meas) {
    measures_ = meas;

    if (imu_need_init_) {
        TryInitIMU();// 使用IMU初始化
        return;
    }

    /// 以下三步与LIO一致，只是align完成地图匹配工作
    if (status_ == Status::WORKING) {
        Predict();// 对IMU状态进行预测
        Undistort();//点云去畸变
    } else {
        scan_undistort_ = measures_.lidar_;
    }
// 执行一次配准和观测
    Align();
}

Align()

void Fusion::Align() {
    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix());
    scan_undistort_ = scan_undistort_trans;
    current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);
    current_scan_ = VoxelCloud(current_scan_, 0.5);

    if (status_ == Status::WAITING_FOR_RTK) {
        // 若存在最近的RTK信号，则尝试初始化
        if (last_gnss_ != nullptr) {
            if (SearchRTK()) {
                status_ == Status::WORKING;
                ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
                ui_->UpdateNavState(eskf_.GetNominalState());
            }
        }
    } else {
        LidarLocalization();
        ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
        ui_->UpdateNavState(eskf_.GetNominalState());
    }
}

RTK处理

收到首个RTK有效信号时，对周边进行网格搜索。
这里主要用于搜索车辆的初始航向角。
RTK初始化搜索，使用的是多分辨率NDT搜索。

RTK初始化网格搜索

SearchRTK()
如果NDT配准结果中的最大值大于预设值，则RTK预设成功，正常进行融合步骤。

bool Fusion::SearchRTK() {
    if (init_has_failed_) {
        if ((last_gnss_->utm_pose_.translation() - last_searched_pos_.translation()).norm() < 20.0) {
            LOG(INFO) << "skip this position";
            return false;
        }
    }

    // 由于RTK不带姿态，我们必须先搜索一定的角度范围
    std::vector<GridSearchResult> search_poses;
    LoadMap(last_gnss_->utm_pose_);

    /// 由于RTK不带角度，这里按固定步长扫描RTK角度
    double grid_ang_range = 360.0, grid_ang_step = 10;  // 角度搜索范围与步长
    for (double ang = 0; ang < grid_ang_range; ang += grid_ang_step) {
        SE3 pose(SO3::rotZ(ang * math::kDEG2RAD), Vec3d(0, 0, 0) + last_gnss_->utm_pose_.translation());
        GridSearchResult gr;
        gr.pose_ = pose;
        search_poses.emplace_back(gr);
    }

    LOG(INFO) << "grid search poses: " << search_poses.size();
    std::for_each(std::execution::par_unseq, search_poses.begin(), search_poses.end(),
                  [this](GridSearchResult& gr) { AlignForGrid(gr); });

    // 选择最优的匹配结果
    auto max_ele = std::max_element(search_poses.begin(), search_poses.end(),
                                    [](const auto& g1, const auto& g2) { return g1.score_ < g2.score_; });
    LOG(INFO) << "max score: " << max_ele->score_ << ", pose: \n" << max_ele->result_pose_.matrix();
    if (max_ele->score_ > rtk_search_min_score_) {
        LOG(INFO) << "初始化成功, score: " << max_ele->score_ << ">" << rtk_search_min_score_;
        status_ = Status::WORKING;

        /// 重置滤波器状态
        auto state = eskf_.GetNominalState();
        state.R_ = max_ele->result_pose_.so3();
        state.p_ = max_ele->result_pose_.translation();
        state.v_.setZero();
        eskf_.SetX(state, eskf_.GetGravity());

        ESKFD::Mat18T cov;
        cov = ESKFD::Mat18T::Identity() * 1e-4;
        cov.block<12, 12>(6, 6) = Eigen::Matrix<double, 12, 12>::Identity() * 1e-6;
        eskf_.SetCov(cov);

        return true;
    }

    init_has_failed_ = true;
    last_searched_pos_ = last_gnss_->utm_pose_;
    return false;
}

多分辨率NDT搜索

AlignForGrid

void Fusion::AlignForGrid(sad::Fusion::GridSearchResult& gr) {
    /// 多分辨率
    pcl::NormalDistributionsTransform<PointType, PointType> ndt;
    ndt.setTransformationEpsilon(0.05);
    ndt.setStepSize(0.7);
    ndt.setMaximumIterations(40);

    ndt.setInputSource(current_scan_);
    auto map = ref_cloud_;

    CloudPtr output(new PointCloudType);
    std::vector<double> res{10.0, 5.0, 4.0, 3.0};
    Mat4f T = gr.pose_.matrix().cast<float>();
    for (auto& r : res) {
        auto rough_map = VoxelCloud(map, r * 0.1);
        ndt.setInputTarget(rough_map);
        ndt.setResolution(r);
        ndt.align(*output, T);
        T = ndt.getFinalTransformation();
    }

    gr.score_ = ndt.getTransformationProbability();
    gr.result_pose_ = Mat4ToSE3(ndt.getFinalTransformation());
}

地图区块加载卸载

LoadMap
根据给出的位姿，加载、卸载必要的地图区块。

void Fusion::LoadMap(const SE3& pose) {
    int gx = floor((pose.translation().x() - 50.0) / 100);
    int gy = floor((pose.translation().y() - 50.0) / 100);
    Vec2i key(gx, gy);

    // 一个区域的周边地图，我们认为9个就够了
    std::set<Vec2i, less_vec<2>> surrounding_index{
        key + Vec2i(0, 0), key + Vec2i(-1, 0), key + Vec2i(-1, -1), key + Vec2i(-1, 1), key + Vec2i(0, -1),
        key + Vec2i(0, 1), key + Vec2i(1, 0),  key + Vec2i(1, -1),  key + Vec2i(1, 1),
    };

    // 加载必要区域
    bool map_data_changed = false;
    int cnt_new_loaded = 0, cnt_unload = 0;
    for (auto& k : surrounding_index) {
        if (map_data_index_.find(k) == map_data_index_.end()) {
            // 该地图数据不存在
            continue;
        }

        if (map_data_.find(k) == map_data_.end()) {
            // 加载这个区块
            CloudPtr cloud(new PointCloudType);
            pcl::io::loadPCDFile(data_path_ + std::to_string(k[0]) + "_" + std::to_string(k[1]) + ".pcd", *cloud);
            map_data_.emplace(k, cloud);
            map_data_changed = true;
            cnt_new_loaded++;
        }
    }

    // 卸载不需要的区域，这个稍微加大一点，不需要频繁卸载
    for (auto iter = map_data_.begin(); iter != map_data_.end();) {
        if ((iter->first - key).cast<float>().norm() > 3.0) {
            // 卸载本区块
            iter = map_data_.erase(iter);
            cnt_unload++;
            map_data_changed = true;
        } else {
            iter++;
        }
    }

    LOG(INFO) << "new loaded: " << cnt_new_loaded << ", unload: " << cnt_unload;
    if (map_data_changed) {
        // rebuild ndt target map
        ref_cloud_.reset(new PointCloudType);
        for (auto& mp : map_data_) {
            *ref_cloud_ += *mp.second;
        }

        LOG(INFO) << "rebuild global cloud, grids: " << map_data_.size();
        ndt_.setInputTarget(ref_cloud_);
    }

    ui_->UpdatePointCloudGlobal(map_data_);
}

点云定位

加载预测位姿附近的点云，调用NDT配准。
LidarLocalization()

bool Fusion::LidarLocalization() {
    SE3 pred = eskf_.GetNominalSE3();
    LoadMap(pred);

    ndt_.setInputCloud(current_scan_);
    CloudPtr output(new PointCloudType);
    ndt_.align(*output, pred.matrix().cast<float>());

    SE3 pose = Mat4ToSE3(ndt_.getFinalTransformation());
    eskf_.ObserveSE3(pose, 1e-1, 1e-2);

    LOG(INFO) << "lidar loc score: " << ndt_.getTransformationProbability();

    return true;
}

IMU处理

处理IMU数据

void ProcessIMU(IMUPtr imu) {
    double timestamp = imu->timestamp_;
    if (timestamp < last_timestamp_imu_) {
        LOG(WARNING) << "imu loop back, clear buffer";
        imu_buffer_.clear();
    }

    last_timestamp_imu_ = timestamp;
    imu_buffer_.emplace_back(imu);
}

点云处理

处理点云地图
ProcessCloud()

/**
     * 处理sensor_msgs::PointCloud2点云
     * @param msg
     */
    void ProcessCloud(const sensor_msgs::PointCloud2::ConstPtr &msg) {
        if (msg->header.stamp.toSec() < last_timestamp_lidar_) {
            LOG(ERROR) << "lidar loop back, clear buffer";
            lidar_buffer_.clear();
        }

        FullCloudPtr cloud(new FullPointCloudType());
        conv_->Process(msg, cloud);
        lidar_buffer_.push_back(cloud);
        time_buffer_.push_back(msg->header.stamp.toSec());
        last_timestamp_lidar_ = msg->header.stamp.toSec();

        Sync();
    }