高翔《自动驾驶中的SLAM技术》代码详解 — 第6章 2D SLAM

目录

6.2 扫描匹配算法

6.2.1 点到点的扫描匹配

---

6.2 扫描匹配算法

6.2.1 点到点的扫描匹配

// src/ch6/test_2dlidar_io.cc
// Created by xiang on 2022/3/15.
//
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui.hpp>

#include "ch6/lidar_2d_utils.h"
#include "common/io_utils.h"

// DEFINE_string定义ROS数据包的路径
DEFINE_string(bag_path, "./dataset/sad/2dmapping/test_2d_lidar.bag", "数据包路径");

/// 测试从rosbag中读取2d scan并plot的结果
int main(int argc, char** argv) {
    // 来初始化Google日志记录库
    google::InitGoogleLogging(argv[0]);
    // 将日志级别设置为INFO
    FLAGS_stderrthreshold = google::INFO;
    // 将日志输出到标准错误流（stderr）并带有彩色显示
    FLAGS_colorlogtostderr = true;
    // 将日志输出到标准错误流（stderr）并带有彩色显示
    google::ParseCommandLineFlags(&argc, &argv, true);

    // 创建了一个sad::RosbagIO对象rosbag_io,用于从rosbag中读取数据
    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);
    rosbag_io.AddScan2DHandle("/pavo_scan_bottom",[](Scan2d::Ptr scan) {
                             cv::Mat image;     // 存储可视化的扫描图像
                             sad::Visualize2DScan(scan, SE2(), image, Vec3b(255, 0, 0)); //生成可视化的图像
                             cv::imshow("scan", image);
                             cv::waitKey(20);
                             return true;
                         })
        .Go();  // 用于从rosbag中读取数据

    return 0;
}

#include "ch6/lidar_2d_utils.h"
#include <opencv2/imgproc.hpp>

namespace sad {
// 扫描数据(scan)、位姿(pose)、图像(image)、
// 颜色(color)、图像大小(image_size)、分辨率(resolution)、子地图的位姿(pose_submap)
void Visualize2DScan(Scan2d::Ptr scan, const SE2& pose, cv::Mat& image, const Vec3b& color, int image_size,
                     float resolution, const SE2& pose_submap) {
    // 如果图像的数据为空,如果图像(image)的数据为空
    if (image.data == nullptr) {
        image = cv::Mat(image_size, image_size, CV_8UC3, cv::Vec3b(255, 255, 255));
    }

    // 扫描数据中的每一个激光点，判断激光点的测量距离是否在有效范围内
    for (size_t i = 0; i < scan->ranges.size(); ++i) {
        if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
            continue;
        }

        // 计算当前激光点的真实角度
        double real_angle = scan->angle_min + i * scan->angle_increment;
        // 通过角度计算出该点在二维空间中的坐标(x, y)
        double x = scan->ranges[i] * std::cos(real_angle);
        double y = scan->ranges[i] * std::sin(real_angle);

        // 如果当前激光点的真实角度是否在可视范围内
        if (real_angle < scan->angle_min + 30 * M_PI / 180.0 || real_angle > scan->angle_max - 30 * M_PI / 180.0) {
            continue;
        }
        // 计算出激光点在地图坐标系下的坐标
        Vec2d psubmap = pose_submap.inverse() * (pose * Vec2d(x, y));

        // 将激光点在地图坐标系下的坐标映射到图像坐标系中
        int image_x = int(psubmap[0] * resolution + image_size / 2);
        int image_y = int(psubmap[1] * resolution + image_size / 2);
        // 判断像素坐标是否在图像的有效范围内
        if (image_x >= 0 && image_x < image.cols && image_y >= 0 && image_y < image.rows) {
            image.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(color[0], color[1], color[2]);
        }
    }

    // 同时画出pose自身所在位置
    Vec2d pose_in_image =
        pose_submap.inverse() * (pose.translation()) * double(resolution) + Vec2d(image_size / 2, image_size / 2);
    cv::circle(image, cv::Point2f(pose_in_image[0], pose_in_image[1]), 5, cv::Scalar(color[0], color[1], color[2]), 2);
}

}  // namespace s