高翔《自动驾驶中的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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/61353.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

Redis—全局命令

Redis—全局命令 &#x1f50e;get / set&#x1f50e;常用全局命令keysexistsdelexpirettlkey 的过期策略type Redis 官网 Redis 中的命令不区分大小写 进入 Redis 客户端 redis-cli -h Redis 服务器的 IP 地址 -p Redis 服务器的端口号省略 -h 表示 Redis 服务器的 IP 地址…

OKR管理策略:为开发团队注入动力

引言 在这个快速变化的世界中&#xff0c;公司需要迅速应对市场变化&#xff0c;并保持其目标和战略的清晰性和一致性。而OKR&#xff08;Objectives and Key Results&#xff09;正是这个挑战的解决方案之一。OKR的实施可以帮助开发团队明确目标&#xff0c;关注关键结果&…

powershell几句话设置环境变量

设置环境变量比较繁琐&#xff0c;现在用这段话&#xff0c;在powershell中就可以轻松完成。 $existingPath [Environment]::GetEnvironmentVariable("Path", "Machine") $newPath "C:\Your\Path\Here"if ($existingPath -split ";"…

DSP学习笔记

一个汇编语句包含4个固定顺序的区域&#xff1a;标号&#xff08;必须从第一列开始 不能空格&#xff0c;后面可以有&#xff1a;也可以没有&#xff09; 助记符 操作数 注释&#xff08;用分号或者星号来注释&#xff09;伪指令用.开头&#xff0c;注释可以用第一列开始写&…

软件设计师(五)软件工程基础知识

一、软件工程概述 软件开发和维护过程中所遇到的各种问题称为“软件危机”。 软件工程是指应用计算机科学、数学及管理科学等原理&#xff0c;以工程化的原则和方法来解决软件问题的工程&#xff0c;其目的是提高软件生产率、提高软件质量、降低软件成本。 #mermaid-svg-h3j6K…

大数据指标体系-笔记

指标体系 1 总体流程图 1.1 2 模型‘ 2.1 OSM OSM(Object,Strategy,Measure) 「业务度量」涉及到以下两个概念:一个是KPI ,用来直 接衡量策略的有效性;一个是Target,是预先给出的值,用来判断是否达到预期 2.2 UJM User, Journey, Map 2.3 AARRR-海盗 AARRR(Acquisitio…

【C语言】通讯录3.0 (文件存储版)

前言 通讯录是一种记录联系人信息的工具&#xff0c;包括姓名、电话号码、电子邮件地址、住址等。 文章的一二三章均于上一篇相同&#xff0c;可以直接看第四章改造内容。 此通讯录是基于通讯录2.0&#xff08;动态增长版&#xff09;的基础上进行增加文件操作功能&#xff0c…

【多线程学习6】synchronized关键字

【多线程学习6】synchronized关键字 一、synchronized关键字是什么&#xff1f;有什么作用&#xff1f; synchronized关键字是Java线程同步的关键字&#xff0c;其可以修饰方法或代码块&#xff0c;并可以保证其修饰的方法或代码块在任意时刻只能有一个线程执行。 synchroni…

K8s集群安全机制

1.访问K8s集群的时候&#xff0c;需要经过三个步骤完成具体操作 &#xff08;1&#xff09;认证&#xff08;2&#xff09;鉴权&#xff08;授权&#xff09;&#xff08;3&#xff09;准入控制 进行访问的时候&#xff0c;过程中都要经过apiserver&#xff0c;apiserver做统…

java,python,c++有什么区别,python java c c++区别

大家好&#xff0c;给大家分享一下java,python,c有什么区别&#xff0c;很多人还不知道这一点。下面详细解释一下。现在让我们来看看&#xff01; 从这四种语言的难度、受欢迎度还有作用以及优点缺点给楼主做一个全面的分析&#xff0c;我们可以从中了解其区别&#xff0c;以及…

AP2400 LED汽车摩灯照明电源驱动 过EMC DC-DC降压恒流IC

产品特点 宽输入电压范围&#xff1a;5V&#xff5e;100V 可设定电流范围&#xff1a;10mA&#xff5e;6000mA 固定工作频率&#xff1a;150KHZ 内置抖频电路&#xff0c;降低对其他设备的 EMI干扰 平均电流模式采样&#xff0c;恒流精度更高 0-100%占空比控制&#xff0…

【状态估计】基于UKF法、AUKF法的电力系统三相状态估计研究(Matlab代码实现)

&#x1f4a5;&#x1f4a5;&#x1f49e;&#x1f49e;欢迎来到本博客❤️❤️&#x1f4a5;&#x1f4a5; &#x1f3c6;博主优势&#xff1a;&#x1f31e;&#x1f31e;&#x1f31e;博客内容尽量做到思维缜密&#xff0c;逻辑清晰&#xff0c;为了方便读者。 ⛳️座右铭&a…

并查集练习—省份数量

上一篇中讲了并查集及其原理&#xff0c;在这篇文章中简单应用一下。如果对并查集不是很了解强烈建议先看上一篇。 题目&#xff1a; 有 n 个城市&#xff0c;其中一些彼此相连&#xff0c;另一些没有相连。如果城市 a 与城市 b 直接相连&#xff0c;且城市 b 与城市 c 直接相…

大数据Flink(五十七):Yarn集群环境(生产推荐)

文章目录 Yarn集群环境(生产推荐) 一、准备工作

目标检测中的IOU

IOU 什么是IOU?IOU应用场景写代码调试什么是IOU? 简单来说IOU就是用来度量目标检测中预测框与真实框的重叠程度。在图像分类中,有一个明确的指标准确率来衡量模型分类模型的好坏。其公式为: 这个公式显然不适合在在目标检测中使用。我们知道目标检测中都是用一个矩形框住…

【福建事业单位-推理判断】02图形推理(数量-空间重构)

【福建事业单位-推理判断】02图形推理&#xff08;数量-空间重构&#xff09; 一、数量规律1.1点&#xff08;交点、切点&#xff09;点的细化考法总结 1.2线条&#xff08;线条的数量&#xff09;线的细化考点一笔画&#xff08;重点&#xff09;一笔画的判定 总结 1.3 面面的…

flutter开发实战-video_player视频播放功能及视频缓存

flutter开发实战-video_player视频播放功能及视频缓存 最近开发过程中video_player播放视频&#xff0c; 一、引入video_player 在pubspec.yaml引入video_player video_player: ^2.7.0在iOS上&#xff0c;video_player使用的是AVPlayer进行播放。 在Android上&#xff0c;…

医疗实施-集成平台下门诊就诊流程详解

目录 集成平台下门诊就诊流程详解1.患者建档2. 门诊挂号3. 医生就诊4.处方开立5.费用收取、6、科室执行医嘱集成平台下门诊就诊流程详解 这篇文章是考虑医院使用了集成平台之后,门诊就诊流程详解。与我的文章《医疗实施-门诊就诊流程详解》的大致一样,供学有余力的人阅读。 …

AMASS database

AMASS是一个由不同的光学标记运动捕捉数据集统一表示在一个公共框架和参数化下的大型人体运动数据库。它包含了超过40小时的运动数据&#xff0c;涵盖了300多个主体和11000多个运动。它使用了SMPL人体模型&#xff0c;它是一种基于混合形状和姿态空间的生成式人体模型&#xff…

spring boot中web容器配置

web容器配置 spring boot 默认的web容器是 tomcat&#xff0c;如果需要换成其他的 web 容器&#xff0c;可以如下配置。 <dependency><groupId>org.springframework.boot</groupId><artifactId>spring-boot-starter-web</artifactId><!-- 默…