从零开始激光slam定位【1】- 激光里程计

2024-02-28 15:38
文章标签 slam 定位 从零开始 激光 里程计

本文主要是介绍从零开始激光slam定位【1】- 激光里程计，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

写在前面：本文是参考知乎任乾大佬的帖子完成，增加了自己的注释以及一些其他功能，如果有理解不到位的地方，请大家指正~

目录

  • 里程计流程
  • front_end_node.cpp的注释
  • 可视化
  • debug
  • 参考

里程计流程

1、如果当前帧是第一帧，那么将其看做关键帧，进行关键帧更新
2、关键帧更新都做了什么？更新local_map队列，更新local_map，更新ndt的target_cloud。
3、如果不是第一帧，那么将其与local_map进行ndt配准，配准的所需要的guess_pose是根据匀速运动模型进行计算的。
4、配准得到当前帧的pose，利用当前帧和上一阵的pose，结合匀速运动模型得到下一次配准的guess_pose；如果当前帧的pose相对于上一关键帧的pose距离较大，则更新关键帧。

front_end_node.cpp的注释

/** @Descripttion: 对任乾大佬的tag4.0增加map--->lidar的tf变换，并可视化最原始的变换后的点云* @vision: * @Author: suyunzzz* @Date: 2020-08-20 23:40:44* @LastEditors: Please set LastEditors* @LastEditTime: 2020-08-26 23:20:21*/
/** @Description: * @Author: Ren Qian* @Date: 2020-02-05 02:56:27*/#include <ros/ros.h>
#include <pcl/common/transforms.h>#include "glog/logging.h"#include "lidar_localization/global_defination/global_defination.h"
#include "lidar_localization/subscriber/cloud_subscriber.hpp"
#include "lidar_localization/subscriber/imu_subscriber.hpp"
#include "lidar_localization/subscriber/gnss_subscriber.hpp"
#include "lidar_localization/tf_listener/tf_listener.hpp"
#include "lidar_localization/publisher/cloud_publisher.hpp"
#include "lidar_localization/publisher/odometry_publisher.hpp"
#include "lidar_localization/front_end/front_end.hpp"           // 前端相关的头文件#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>using namespace lidar_localization;int main(int argc, char *argv[]) {google::InitGoogleLogging(argv[0]);FLAGS_log_dir = WORK_SPACE_PATH + "/Log";FLAGS_alsologtostderr = 1;// 初始化节点ros::init(argc, argv, "front_end_node");ros::NodeHandle nh;std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);std::shared_ptr<IMUSubscriber> imu_sub_ptr = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);// TF监听。lidar-->imustd::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");   // 发布者，发布到frame_id="/map"std::shared_ptr<CloudPublisher> cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");  std::shared_ptr<CloudPublisher> raw_cloud_pub_ptr = std::make_shared<CloudPublisher>(nh,"raw_current_scan",100,"/map");std::shared_ptr<CloudPublisher> local_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "local_map", 100, "/map");std::shared_ptr<CloudPublisher> global_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "global_map", 100, "/map");// 发布odom信息，topic_name,base_frame_id,child_frame_idstd::shared_ptr<OdometryPublisher> laser_odom_pub_ptr = std::make_shared<OdometryPublisher>(nh, "laser_odom", "map", "lidar", 100);std::shared_ptr<OdometryPublisher> gnss_pub_ptr = std::make_shared<OdometryPublisher>(nh, "gnss", "map", "lidar", 100);// 创建一个前端对象std::shared_ptr<FrontEnd> front_end_ptr = std::make_shared<FrontEnd>();// 数据队列std::deque<CloudData> cloud_data_buff;std::deque<IMUData> imu_data_buff;std::deque<GNSSData> gnss_data_buff;Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();bool transform_received = false;bool gnss_origin_position_inited = false;bool front_end_pose_inited = false;             // true 前端pose未初始化// 局部地图，全局地图，当前帧点云CloudData::CLOUD_PTR local_map_ptr(new CloudData::CLOUD());     CloudData::CLOUD_PTR global_map_ptr(new CloudData::CLOUD());CloudData::CLOUD_PTR current_scan_ptr(new CloudData::CLOUD());CloudData::CLOUD_PTR current_scan_ptr_raw(new CloudData::CLOUD());  // 当前帧变换后的结果，/****************************************************/// 以下为我新增的tf变换，map--->lidar// 发布tf变换 map--->lidar//static tf::TransformBroadcaster br;tf::TransformBroadcaster br_;          tf::Transform transform;tf::Quaternion q;struct pose{double x,y,z;double roll,pitch,yaw;};struct pose current_pose_;/***************************************************/double run_time = 0.0;double init_time = 0.0;bool time_inited = false;bool has_global_map_published = false; ros::Rate rate(100);while (ros::ok()) {ros::spinOnce();// 数据读入cloud_sub_ptr->ParseData(cloud_data_buff);imu_sub_ptr->ParseData(imu_data_buff);gnss_sub_ptr->ParseData(gnss_data_buff);// 如果没有得到变换if (!transform_received) {if (lidar_to_imu_ptr->LookupData(lidar_to_imu)) {transform_received = true;}} else {while (cloud_data_buff.size() > 0 && imu_data_buff.size() > 0 && gnss_data_buff.size() > 0) {CloudData cloud_data = cloud_data_buff.front();IMUData imu_data = imu_data_buff.front();GNSSData gnss_data = gnss_data_buff.front();if (!time_inited) {time_inited = true;init_time = cloud_data.time;} else {run_time = cloud_data.time - init_time;}double d_time = cloud_data.time - imu_data.time;if (d_time < -0.05) {       // cloud慢cloud_data_buff.pop_front();} else if (d_time > 0.05) {     // cloud快imu_data_buff.pop_front();gnss_data_buff.pop_front();} else {                                    // 开始cloud_data_buff.pop_front();            // 将使用的数据在队列中删除imu_data_buff.pop_front();gnss_data_buff.pop_front();Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity();if (!gnss_origin_position_inited) {gnss_data.InitOriginPosition();gnss_origin_position_inited = true;}gnss_data.UpdateXYZ();odometry_matrix(0,3) = gnss_data.local_E;odometry_matrix(1,3) = gnss_data.local_N;odometry_matrix(2,3) = gnss_data.local_U;odometry_matrix.block<3,3>(0,0) = imu_data.GetOrientationMatrix();odometry_matrix *= lidar_to_imu;gnss_pub_ptr->Publish(odometry_matrix);             // gnss发布里程计信息if (!front_end_pose_inited) {       // 如果未初始化前端，将gnss的pose作为初始posefront_end_pose_inited = true;front_end_ptr->SetInitPose(odometry_matrix);        }front_end_ptr->SetPredictPose(odometry_matrix);                 // gnss的里程计作为预测（没有用到）Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data);               // 更新当前帧，返回当前帧的poselaser_odom_pub_ptr->Publish(laser_matrix);                  // lidar发布里程计信息（当前帧的pose）/****************************************************/// 以下为我新增的tf变换，map--->lidar// 发布tf变换map--->lidar// current_pose_写入transformtransform.setOrigin(tf::Vector3(laser_matrix(0,3), laser_matrix(1,3), laser_matrix(2,3)));   // transform 设置平移tf::Matrix3x3 mat_b;mat_b.setValue(static_cast<double>(laser_matrix(0, 0)), static_cast<double>(laser_matrix(0, 1)),static_cast<double>(laser_matrix(0, 2)), static_cast<double>(laser_matrix(1, 0)),static_cast<double>(laser_matrix(1, 1)), static_cast<double>(laser_matrix(1, 2)),static_cast<double>(laser_matrix(2, 0)), static_cast<double>(laser_matrix(2, 1)),static_cast<double>(laser_matrix(2, 2)));                    mat_b.getRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw, 1);//mat2rpy  旋转q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw); //q from rpytransform.setRotation(q);//trans from qbr_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "lidar"));  // map--->scan的transform/****************************************************/// 当前帧的点云front_end_ptr->GetCurrentScan(current_scan_ptr);            // 变换到global下的当前帧点云cloud_pub_ptr->Publish(current_scan_ptr);                   // 发布当前帧点云front_end_ptr->GetCurrentScanRaw(current_scan_ptr_raw);        // 发布变换后的原始点云,未下采样raw_cloud_pub_ptr->Publish(current_scan_ptr_raw);if (front_end_ptr->GetNewLocalMap(local_map_ptr))           // 发布局部地图,只有遇到关键帧才能更新局部地图local_map_pub_ptr->Publish(local_map_ptr);}if (run_time > 460.0 && !has_global_map_published) {            // 全局地图，只能发布一次，并且有时间限制if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) {    //  每100个关键帧，才更新一次全局地图global_map_pub_ptr->Publish(global_map_ptr);has_global_map_published = true;}}}}rate.sleep();}return 0;
}

可视化

在这里插入图片描述黄色的代表gnss的里程计，红色的代表激光里程计，大的Axis的坐标系是lidar，小的坐标系是map。

debug

typename boost::detail::sp_dereference<T>::type boost::shared_ptr<T>::operator*() const [with T = pcl::PointCloud<pcl::PointXYZ>; typename boost::detail::sp_dereference<T>::type = pcl::PointCloud<pcl::PointXYZ>&]: Assertion `px != 0' failed

在这里插入图片描述
解决：
这个问题的原因是创建pcl点云指针的时候没有初始化，应该在构造函数中进行初始化。

参考

从零开始做自动驾驶定位(四): 前端里程计之初试

这篇关于从零开始激光slam定位【1】- 激光里程计的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/755914。

相关文章

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
生信代码入门：从零开始掌握生物信息学编程技能

生信代码入门：从零开始掌握生物信息学编程技能

少走弯路,高效分析;了解生信云,访问 【生信圆桌x生信专用云服务器】 : www.tebteb.cc 介绍 生物信息学是一个高度跨学科的领域，结合了生物学、计算机科学和统计学。随着高通量测序技术的发展，海量的生物数据需要通过编程来进行处理和分析。因此，掌握生信编程技能，成为每一个生物信息学研究者的必备能力。 生信代码入门，旨在帮助初学者从零开始学习生物信息学中的编程基础。通过学习常用
阅读更多...
js定位navigator.geolocation

js定位navigator.geolocation

一、简介 　　html5为window.navigator提供了geolocation属性，用于获取基于浏览器的当前用户地理位置。 　　window.navigator.geolocation提供了3个方法分别是： void getCurrentPosition(onSuccess,onError,options);//获取用户当前位置int watchCurrentPosition(
阅读更多...
BIRT--商业智能和报表工具，从零开始

BIRT--商业智能和报表工具，从零开始

1.简介 BIRT (Business Intelligence and Reporting Tools), 是为 Web 应用程序开发的基于 Eclipse 的开源报表系统，特别之处在于它是以 Java 和 JavaEE 为基础。BIRT 有两个主要组件：基于 Eclipse 的报表设计器，以及部署到应用服务器上的运行时组件。 2.下载 官网下载网址：http://download.ec
阅读更多...
flume系列之：记录一次flume agent进程被异常oom kill -9的原因定位

flume系列之：记录一次flume agent进程被异常oom kill -9的原因定位

flume系列之：记录一次flume agent进程被异常oom kill -9的原因定位 一、背景二、定位问题三、解决方法 一、背景 flume系列之：定位flume没有关闭某个时间点生成的tmp文件的原因，并制定解决方案在博主上面这篇文章的基础上，在机器内存、cpu资源、flume agent资源都足够的情况下，flume agent又出现了tmp文件无法关闭的情况 二、
阅读更多...
一次生产环境大量CLOSE_WAIT导致服务无法访问的定位过程

一次生产环境大量CLOSE_WAIT导致服务无法访问的定位过程

1.症状 生产环境的一个服务突然无法访问，服务的交互过程如下所示： 所有的请求都是通过网关进入，之后分发到后端服务。 现在的情况是用户服务无法访问商旅服务，网关有大量java.net.SocketTimeoutException: Read timed out报错日志，商旅服务也不断有日志打印，大多是回调和定时任务日志，所以故障点在网关和商旅服务，大概率是商旅服务无法访问导致网关超时。 后
阅读更多...
定位cpu占用过高的线程和对应的方法

定位cpu占用过高的线程和对应的方法

如何定位cpu占用过高的线程和对应的方法？ 主要是通过线程id找到对应的方法。 1 查询某个用户cpu占用最高的进程号 top -u 用户名 2 查询这个进程中占用cpu最高的线程号 top –p 进程号-H    3 查询到进程id后把进程相关的代码打印到jstack文件 jstack -l pid > jstack.txt 4 在jstack文件中通过16进制的线程id搜索到
阅读更多...
Autoware 定位之ndt定位（八）

Autoware 定位之ndt定位（八）

0. 简介 这一讲按照《Autoware 技术代码解读（三）》梳理的顺序，我们来说一说Autoware中的ndt定位，这个软件包有两个主要功能：1. 通过扫描匹配进行位置估计，2. 通过ROS服务使用蒙特卡洛方法估计初始位置 1. 代码阅读 1.1 debug.cpp 这段代码是一个用于生成用于调试的可视化标记数组的函数。它接受时间戳、坐标系、缩放比例、粒子信息和索引作为输入，并返回一个
阅读更多...
从零开始学习JVM（七）- StringTable字符串常量池

从零开始学习JVM（七）- StringTable字符串常量池

1 概述 String应该是Java使用最多的类吧，很少有Java程序没有使用到String的。在Java中创建对象是一件挺耗费性能的事，而且我们又经常使用相同的String对象，那么创建这些相同的对象不是白白浪费性能吗。所以就有了StringTable这一特殊的存在，StringTable叫做字符串常量池，用于存放字符串常量，这样当我们使用相同的字符串对象时，就可以直接从StringTable
阅读更多...
从零开始构建大语言模型并进行微调：全面指南

从零开始构建大语言模型并进行微调：全面指南

要从0开始搭建并训练一个大语言模型（LLM），涉及到多个步骤和资源，包括理论理解、工具使用、数据准备、模型训练与微调。以下是一个从基础到应用的指南，帮助你理解并逐步实现这一目标。 1. 理解基础概念 在开始搭建大语言模型之前，了解以下基本概念至关重要： 生成式AI：通过大语言模型生成自然语言文本，例如GPT、BERT等。机器学习：通过数据训练模型，使其具备从数据中学习规律的能力。深度学习：机
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2