自动驾驶与机器人中的SLAM技术--第九章--自动驾驶车辆的离线地图构建

weixin_47046769

于 2024-08-29 15:18:55 发布

阅读量512

点赞数 15

文章标签：自动驾驶机器人人工智能

本文链接：https://blog.csdn.net/weixin_47046769/article/details/141607546

版权

9.1 点云建图的流程

地图构建系统可以工作在离线模式下，离线系统的好处就是有很强的确定性，会比实时SLAM系统更加容易，可以实现更强大的自动化能力。

9.2 前端实现

在前端中，首先将ROS数据包中的RTK数据提取出来，放入RTK消息队列。
在RemoveMapOrigin()中将第一个有效的RTK数据作为地图原点，并把其他的RTK数据减去原点，得到RTK位置观测。
随后用IMU和激光数据运行LIO。判断关键帧时，从LIO中获取位姿，计算上一关键帧的位姿和当前位姿的相对运动，按照距离和角度阈值抽取关键帧，保存关键帧结果（点云数据）。LIO结束后，保存所有关键帧的位姿数据（雷达位姿、RTK位姿、第一轮优化位姿、第二轮优化位姿），但此时还没有开始优化。

void Frontend::Run() {
    sad::RosbagIO rosbag_io(bag_path_, DatasetType::NCLT);

    // 先提取RTK pose，注意NCLT只有平移部分
    rosbag_io
        .AddAutoRTKHandle([this](GNSSPtr gnss) {
            gnss_.emplace(gnss->unix_time_, gnss);
            return true;
        })
        .Go();
    rosbag_io.CleanProcessFunc();  // 不再需要处理RTK

    RemoveMapOrigin();

    // 再运行LIO
    rosbag_io
        .AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {
            lio_->PCLCallBack(cloud);
            ExtractKeyFrame(lio_->GetCurrentState());
            return true;
        })
        .AddImuHandle([&](IMUPtr imu) {
            lio_->IMUCallBack(imu);
            return true;
        })
        .Go();
    lio_->Finish();

    // 保存运行结果
    SaveKeyframes();

    LOG(INFO) << "done.";
}

接下来用前端保存的位姿数据和点云来做回环检测和位姿优化，后续建图流程从前端结果出发，不必再次解析ROS包。首先将关键帧点云合并成一个完成的地图文件：
加载关键帧文本数据，初始化全局点云和体素滤波器。遍历关键帧，获取点云位姿，根据id取出对应的点云，经过点云变换和体素滤波后添加到全局点云中，最后保存全局点云为pcd文件。

//
// Created by xiang on 22-12-7.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

DEFINE_double(voxel_size, 0.1, "导出地图分辨率");
DEFINE_string(pose_source, "lidar", "使用的pose来源:lidar/rtk/opti1/opti2");
DEFINE_string(dump_to, "./data/ch9/", "导出的目标路径");

#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>

#include "keyframe.h"
#include "common/point_cloud_utils.h"

/**
 * 将keyframes.txt中的地图和点云合并为一个pcd
 */

int main(int argc, char** argv) {
    google::ParseCommandLineFlags(&argc, &argv, true);
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;

    using namespace sad;
    std::map<IdType, KFPtr> keyframes;
    if (!LoadKeyFrames("./data/ch9/keyframes.txt", keyframes)) {
        LOG(ERROR) << "failed to load keyframes.txt";
        return -1;
    }

    if (keyframes.empty()) {
        LOG(INFO) << "keyframes are empty";
        return 0;
    }

    // dump kf cloud and merge
    LOG(INFO) << "merging";
    CloudPtr global_cloud(new PointCloudType);

    pcl::VoxelGrid<PointType> voxel_grid_filter;
    float resolution = FLAGS_voxel_size;
    voxel_grid_filter.setLeafSize(resolution, resolution, resolution);

    int cnt = 0;
    for (auto& kfp : keyframes) {
        auto kf = kfp.second;
        SE3 pose;
        if (FLAGS_pose_source == "rtk") {
            pose = kf->rtk_pose_;
        } else if (FLAGS_pose_source == "lidar") {
            pose = kf->lidar_pose_;
        } else if (FLAGS_pose_source == "opti1") {
            pose = kf->opti_pose_1_;
        } else if (FLAGS_pose_source == "opti2") {
            pose = kf->opti_pose_2_;
        }

        kf->LoadScan("./data/ch9/");

        CloudPtr cloud_trans(new PointCloudType);
        pcl::transformPointCloud(*kf->cloud_, *cloud_trans, pose.matrix());

        // voxel size
        CloudPtr kf_cloud_voxeled(new PointCloudType);
        voxel_grid_filter.setInputCloud(cloud_trans);
        voxel_grid_filter.filter(*kf_cloud_voxeled);

        *global_cloud += *kf_cloud_voxeled;
        kf->cloud_ = nullptr;

        LOG(INFO) << "merging " << cnt << " in " << keyframes.size() << ", pts: " << kf_cloud_voxeled->size()
                  << " global pts: " << global_cloud->size();
        cnt++;
    }

    if (!global_cloud->empty()) {
        sad::SaveCloudToFile(FLAGS_dump_to + "/map.pcd", *global_cloud);
    }

    LOG(INFO) << "done.";
    return 0;
}

9.3 后端位姿图优化与异常值检验

后端图优化需要包含以下因子。

RTK因子：3维单元边，连接到每个关键帧顶点。误差为GNSS估计位姿（由雷达位姿转换而来）与RTK位姿的平移差。

雷达里程计因子：6维二元边（称为相对运动约束边），对每个关键帧，连接当前关键帧和下三个关键帧。误差为两帧之间的相对运动差，即T21*T12。

回环因子：和雷达里程计因子是同一种边，对所有的回环候选关键帧对添加相对运动约束边。

/**
 * 只有平移的GNSS
 * 此时需要提供RTK外参 TBG，才能正确施加约束
 */
class EdgeGNSSTransOnly : public g2o::BaseUnaryEdge<3, Vec3d, VertexPose> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    /**
     * 指定位姿顶点、RTK观测 t_WG、外参TGB
     * @param v
     * @param obs
     */
    EdgeGNSSTransOnly(VertexPose* v, const Vec3d& obs, const SE3& TBG) : TBG_(TBG) {
        setVertex(0, v);
        setMeasurement(obs);
    }

    void computeError() override {
        VertexPose* v = (VertexPose*)_vertices[0];
        _error = (v->estimate() * TBG_).translation() - _measurement; // Twb*Tbg=Twg
    };

    // void linearizeOplus() override {
    //     VertexPose* v = (VertexPose*)_vertices[0];
    //     // jacobian 6x6
    //     _jacobianOplusXi.setZero();
    //     _jacobianOplusXi.block<3, 3>(0, 0) = (_measurement.so3().inverse() * v->estimate().so3()).jr_inv();  // dR/dR
    //     _jacobianOplusXi.block<3, 3>(3, 3) = Mat3d::Identity();                                              // dp/dp
    // }

    virtual bool read(std::istream& in) { return true; }
    virtual bool write(std::ostream& out) const { return true; }

   private:
    SE3 TBG_;
};

/**
 * 6 自由度相对运动
 * 误差的平移在前，角度在后
 * 观测：T12
 */
class EdgeRelativeMotion : public g2o::BaseBinaryEdge<6, SE3, VertexPose, VertexPose> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeRelativeMotion() = default;
    EdgeRelativeMotion(VertexPose* v1, VertexPose* v2, const SE3& obs) {
        setVertex(0, v1);
        setVertex(1, v2);
        setMeasurement(obs);
    }

    void computeError() override {
        VertexPose* v1 = (VertexPose*)_vertices[0];
        VertexPose* v2 = (VertexPose*)_vertices[1];
        SE3 T12 = v1->estimate().inverse() * v2->estimate();
        _error = (_measurement.inverse() * v1->estimate().inverse() * v2->estimate()).log();
    };

   private:
};

优化的主要流程如代码所示，优化分为第一轮和第二轮，流程大致一样。
如果RTK没有姿态，且处于第一阶段，就把雷达位姿和RTK位姿对齐。
1. 建立优化问题，包括优化器的创建、顶点创建、顶点和边的连接。
2. 带鲁棒核优化一次。
3. 遍历回环边和RTK边，异常值的边等级设置为1，非异常值去掉鲁棒核。（第一轮还没有回环边）
4. 再优化一次。
5. 保存结果。保存位姿顶点优化的结果到opti_pose_1，即第一轮优化位姿。

void Optimization::Run() {
    LOG(INFO) << "running optimization on stage " << stage_;
    if (!rtk_has_rot_ && stage_ == 1) {
        InitialAlign();
    }

    BuildProblem();  // 建立问题

    SaveG2O("./data/ch9/before.g2o");
    LOG(INFO) << "RTK 误差：" << print_info(gnss_edge_, rtk_outlier_th_);
    LOG(INFO) << "RTK 平移误差：" << print_info(gnss_trans_edge_, rtk_outlier_th_);
    LOG(INFO) << "lidar 误差：" << print_info(lidar_edge_, 0);

    Solve();           // 带着RK求解一遍
    RemoveOutliers();  // 移除异常值
    Solve();           // 再求解一遍

    SaveG2O("./data/ch9/after.g2o");

    SaveResults();  // 保存结果
    LOG(INFO) << "done";
}

9.4 回环检测

系统是离线运行的，可以一次性把所有该检查的区域检查完，不必像实时系统那样需要等待前端的结果。执行匹配时，可以充分利用并行机制，加速回环检测的过程。回环检测结果以回环候选对的形式描述：

struct LoopCandidate {
    LoopCandidate() {}
    LoopCandidate(IdType id1, IdType id2, SE3 Tij) : idx1_(id1), idx2_(id2), Tij_(Tij) {}

    IdType idx1_ = 0;
    IdType idx2_ = 0;
    SE3 Tij_;
    double ndt_score_ = 0.0;
};

回环检测步骤如下：

1. 遍历第一轮优化轨迹中的关键帧。

         对每个关键帧kf_first，遍历其之后的关键帧kf_second，检查是否满足回环条件。
        使用check_first和check_second跟踪之前已添加为候选对的关键帧对，如果正在检查的关键帧kf_first离check_first的距离太近，则跳过此关键帧进行下一个kf_first。
        当kf_first满足距离条件，进而判断kf_second是否同时满足与check_second以及kf_first的距离条件（都是指关键帧id距离），如果都满足则使用第一轮优化位姿来计算并判定kf_first和kf_second之间的平移距离是否满足最小距离阈值，满足则添加到回环候选对当中。

void LoopClosure::DetectLoopCandidates() {
    KFPtr check_first = nullptr;
    KFPtr check_second = nullptr;

    LOG(INFO) << "detecting loop candidates from pose in stage 1";

    // 本质上是两重循环
    for (auto iter_first = keyframes_.begin(); iter_first != keyframes_.end(); ++iter_first) {
        auto kf_first = iter_first->second;

        if (check_first != nullptr && abs(int(kf_first->id_) - int(check_first->id_)) <= skip_id_) {
            // 两个关键帧之前ID太近
            continue;
        }

        for (auto iter_second = iter_first; iter_second != keyframes_.end(); ++iter_second) {
            auto kf_second = iter_second->second;

            if (check_second != nullptr && abs(int(kf_second->id_) - int(check_second->id_)) <= skip_id_) {
                // 两个关键帧之前ID太近
                continue;
            }

            if (abs(int(kf_first->id_) - int(kf_second->id_)) < min_id_interval_) {
                /// 在同一条轨迹中，如果间隔太近，就不考虑回环
                continue;
            }

            Vec3d dt = kf_first->opti_pose_1_.translation() - kf_second->opti_pose_1_.translation();
            double t2d = dt.head<2>().norm();  // x-y distance
            double range_th = min_distance_;

            if (t2d < range_th) {
                LoopCandidate c(kf_first->id_, kf_second->id_,
                                kf_first->opti_pose_1_.inverse() * kf_second->opti_pose_1_);
                loop_candiates_.emplace_back(c);
                check_first = kf_first;
                check_second = kf_second;
            }
        }
    }
    LOG(INFO) << "detected candidates: " << loop_candiates_.size();
}

2. 计算回环候选对是否成立。

并行构建候选对中的点云，对候选对中第一个关键帧kf1建立子地图（前后40个关键帧的点云拼接），对第二个关键帧kf2只取其一帧点云。子图的建立在世界坐标系下。
使用Scan to Map的配准方式，将点云与子地图进行多分辨率NDT配准，得到kf2的世界坐标系。在回环检测配准过程中，初始估计位姿一般偏差较大，因此给NDT方法增加一个由粗至精的配准过程，希望算法不太依赖于给定的位姿初值。
判断配准后的回环候选对中的ndt_score_是否满足阈值，满足则视为成功的候选对。

void LoopClosure::ComputeLoopCandidates() {
    // 执行计算
    std::for_each(std::execution::par_unseq, loop_candiates_.begin(), loop_candiates_.end(),
                  [this](LoopCandidate& c) { ComputeForCandidate(c); });
    // 保存成功的候选
    std::vector<LoopCandidate> succ_candidates;
    for (const auto& lc : loop_candiates_) {
        if (lc.ndt_score_ > ndt_score_th_) {
            succ_candidates.emplace_back(lc);
        }
    }
    LOG(INFO) << "success: " << succ_candidates.size() << "/" << loop_candiates_.size();

    loop_candiates_.swap(succ_candidates);
}

void LoopClosure::ComputeForCandidate(sad::LoopCandidate& c) {
    LOG(INFO) << "aligning " << c.idx1_ << " with " << c.idx2_;
    const int submap_idx_range = 40;
    KFPtr kf1 = keyframes_.at(c.idx1_), kf2 = keyframes_.at(c.idx2_);

    auto build_submap = [this](int given_id, bool build_in_world) -> CloudPtr {
        CloudPtr submap(new PointCloudType);
        for (int idx = -submap_idx_range; idx < submap_idx_range; idx += 4) {
            int id = idx + given_id;
            if (id < 0) {
                continue;
            }
            auto iter = keyframes_.find(id);
            if (iter == keyframes_.end()) {
                continue;
            }

            auto kf = iter->second;
            CloudPtr cloud(new PointCloudType);
            pcl::io::loadPCDFile("./data/ch9/" + std::to_string(id) + ".pcd", *cloud);
            sad::RemoveGround(cloud, 0.1);

            if (cloud->empty()) {
                continue;
            }

            // 转到世界系下
            SE3 Twb = kf->opti_pose_1_;

            if (!build_in_world) {
                Twb = keyframes_.at(given_id)->opti_pose_1_.inverse() * Twb;
            }

            CloudPtr cloud_trans(new PointCloudType);
            pcl::transformPointCloud(*cloud, *cloud_trans, Twb.matrix());

            *submap += *cloud_trans;
        }
        return submap;
    };

    auto submap_kf1 = build_submap(kf1->id_, true);

    CloudPtr submap_kf2(new PointCloudType);
    pcl::io::loadPCDFile("./data/ch9/" + std::to_string(kf2->id_) + ".pcd", *submap_kf2);

    if (submap_kf1->empty() || submap_kf2->empty()) {
        c.ndt_score_ = 0;
        return;
    }

    pcl::NormalDistributionsTransform<PointType, PointType> ndt;

    ndt.setTransformationEpsilon(0.05);
    ndt.setStepSize(0.7);
    ndt.setMaximumIterations(40);

    Mat4f Tw2 = kf2->opti_pose_1_.matrix().cast<float>();

    /// 不同分辨率下的匹配
    CloudPtr output(new PointCloudType);
    std::vector<double> res{10.0, 5.0, 4.0, 3.0};
    for (auto& r : res) {
        ndt.setResolution(r);
        auto rough_map1 = VoxelCloud(submap_kf1, r * 0.1);
        auto rough_map2 = VoxelCloud(submap_kf2, r * 0.1);
        ndt.setInputTarget(rough_map1);
        ndt.setInputSource(rough_map2);

        ndt.align(*output, Tw2);
        Tw2 = ndt.getFinalTransformation();
    }

    Mat4d T = Tw2.cast<double>();
    Quatd q(T.block<3, 3>(0, 0));
    q.normalize();
    Vec3d t = T.block<3, 1>(0, 3);
    c.Tij_ = kf1->opti_pose_1_.inverse() * SE3(q, t);
    c.ndt_score_ = ndt.getTransformationProbability();
}

3. 保存结果

void LoopClosure::SaveResults() {
    auto save_SE3 = [](std::ostream& f, SE3 pose) {
        auto q = pose.so3().unit_quaternion();
        Vec3d t = pose.translation();
        f << t[0] << " " << t[1] << " " << t[2] << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";
    };

    std::ofstream fout("./data/ch9/loops.txt");
    for (const auto& lc : loop_candiates_) {
        fout << lc.idx1_ << " " << lc.idx2_ << " " << lc.ndt_score_ << " ";
        save_SE3(fout, lc.Tij_);
        fout << std::endl;
    }
}

回环检测过后，就进行第二轮优化，流程和第一轮差不多，优化问题多了回环边。

9.5 地图的导出

最后，应该对整个点云地图进行导出。在多数应用中，我们希望控制实时点云的载入规模，例如只加载自身周围200米范围内的点云，其他范围的点云则视情况卸载，这样可以控制实时系统的计算量。以下是分割并导出地图的代码。

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    using namespace sad;

    std::map<IdType, KFPtr> keyframes;
    if (!LoadKeyFrames("./data/ch9/keyframes.txt", keyframes)) {
        LOG(ERROR) << "failed to load keyframes";
        return 0;
    }

    ...
}

加载关键帧数据之后，初始化map_data变量，一个以二维网格id为键，点云为值的映射。初始化体素滤波器。

std::map<Vec2i, CloudPtr, less_vec<2>> map_data;  // 以网格ID为索引的地图数据
    pcl::VoxelGrid<PointType> voxel_grid_filter;
    float resolution = FLAGS_voxel_size;
    voxel_grid_filter.setLeafSize(resolution, resolution, resolution);

遍历所有关键帧，获取其点云并进行体素滤波，对滤波后的点云计算每个点所在的网格id(gx,gy)，如果网格id不存在，就创建新的网格并把id和点云添加到新网格中，如果该id早已存在，则在id对应的网格中添加点云。

// 逻辑和dump map差不多，但每个点个查找它的网格ID，没有的话会创建
    for (auto& kfp : keyframes) {
        auto kf = kfp.second;
        kf->LoadScan("./data/ch9/");

        CloudPtr cloud_trans(new PointCloudType);
        pcl::transformPointCloud(*kf->cloud_, *cloud_trans, kf->opti_pose_2_.matrix());

        // voxel size
        CloudPtr kf_cloud_voxeled(new PointCloudType);
        voxel_grid_filter.setInputCloud(cloud_trans);
        voxel_grid_filter.filter(*kf_cloud_voxeled);

        LOG(INFO) << "building kf " << kf->id_ << " in " << keyframes.size();

        // add to grid
        for (const auto& pt : kf_cloud_voxeled->points) {
            int gx = floor((pt.x - 50.0) / 100);
            int gy = floor((pt.y - 50.0) / 100);
            Vec2i key(gx, gy);
            auto iter = map_data.find(key);
            if (iter == map_data.end()) {
                // create point cloud
                CloudPtr cloud(new PointCloudType);
                cloud->points.emplace_back(pt);
                cloud->is_dense = false;
                cloud->height = 1;
                map_data.emplace(key, cloud);
            } else {
                iter->second->points.emplace_back(pt);
            }
        }
    }

存储点云和索引文件。切分完地图后，每块地图会单独导出一个pcd文件

// 存储点云和索引文件
    LOG(INFO) << "saving maps, grids: " << map_data.size();
    std::system("mkdir -p ./data/ch9/map_data/");
    std::system("rm -rf ./data/ch9/map_data/*");  // 清理一下文件夹
    std::ofstream fout("./data/ch9/map_data/map_index.txt");
    for (auto& dp : map_data) {
        fout << dp.first[0] << " " << dp.first[1] << std::endl;
        dp.second->width = dp.second->size();
        sad::VoxelGrid(dp.second, 0.1);

        sad::SaveCloudToFile(
            "./data/ch9/map_data/" + std::to_string(dp.first[0]) + "_" + std::to_string(dp.first[1]) + ".pcd",
            *dp.second);
    }
    fout.close();

    LOG(INFO) << "done.";
    return 0;

9.6 本章小结

本章主要向读者演示了一个完整的点云地图构建、优化、切分、导出的程序。把本章的各步骤程序按固定顺序调用，就可以完成一个完成的建图程序。这些地图可以帮助我们进行激光高精定位，让车辆和机器人在没有RTK的环境下获取高精度位姿。

//
// Created by xiang on 22-12-20.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "frontend.h"
#include "loopclosure.h"
#include "optimization.h"

DEFINE_string(config_yaml, "./config/mapping.yaml", "配置文件");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    LOG(INFO) << "testing frontend";
    sad::Frontend frontend(FLAGS_config_yaml);
    if (!frontend.Init()) {
        LOG(ERROR) << "failed to init frontend.";
        return -1;
    }

    frontend.Run();

    sad::Optimization opti(FLAGS_config_yaml);
    if (!opti.Init(1)) {
        LOG(ERROR) << "failed to init opti1.";
        return -1;
    }
    opti.Run();

    sad::LoopClosure lc(FLAGS_config_yaml);
    if (!lc.Init()) {
        LOG(ERROR) << "failed to init loop closure.";
        return -1;
    }
    lc.Run();

    sad::Optimization opti2(FLAGS_config_yaml);
    if (!opti2.Init(2)) {
        LOG(ERROR) << "failed to init opti2.";
        return -1;
    }
    opti2.Run();

    LOG(INFO) << "done.";
    return 0;
}

weixin_47046769

关注

15
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
自动驾驶与机器人中的SLAM技术--第九章--自动驾驶车辆的离线地图构建

判断关键帧时，从LIO中获取位姿，计算上一关键帧的位姿和当前位姿的相对运动，按照距离和角度阈值抽取关键帧，保存关键帧结果（点云数据）。遍历所有关键帧，获取其点云并进行体素滤波，对滤波后的点云计算每个点所在的网格id(gx,gy)，如果网格id不存在，就创建新的网格并把id和点云添加到新网格中，如果该id早已存在，则在id对应的网格中添加点云。在多数应用中，我们希望控制实时点云的载入规模，例如只加载自身周围200米范围内的点云，其他范围的点云则视情况卸载，这样可以控制实时系统的计算量。存储点云和索引文件。
复制链接

扫一扫