目录
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