目录
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]);// 将日志级别设置为INFOFLAGS_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
