修改PointLIO项目

添加key_frame_info.msg消息

新建.msg文件，内容填写为：

# Cloud Info
Header header # cloud messages
sensor_msgs/PointCloud2 key_frame_cloud_ori
sensor_msgs/PointCloud2 key_frame_cloud_transed
sensor_msgs/PointCloud2 key_frame_poses

其中key_frame_cloud_ori为原始点云（需要包含行号ring），key_frame_cloud_transed 为利用SLAM转换到统一坐标系后的点云．key_frame_poses为SLAM输出的位姿．

把构建好的key_frame_info.msg文件放在msg文件夹下，随后，在CMakeLists.txt文件中添加

add_message_files(FILESkey_frame_info.msg
)

并修改generate_messages，在generate_messages中添加sensor_msgs

generate_messages(DEPENDENCIESgeometry_msgssensor_msgs
)

编译，会生成key_frame_info.h文件．

之后在common_lib.h文件中添加key_frame_info.h文件的引用，即可添加使用自定义的key_frame_info消息类型

#include <point_lio/key_frame_info.h>

添加自定义数据结构PointXYZIRPYT，并重命名为PointTypePose（用于存储X,Y,Z,Roll,Pitch,Yaw）

找到preprocess.h头文件，在velodyne_ros，hesai_ros，ouster_ros等一系列数据结构后面添加下述代码：

/** A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)*/
struct PointXYZIRPYT
{PCL_ADD_POINT4DPCL_ADD_INTENSITY;                  // preferred way of adding a XYZ+paddingfloat roll;float pitch;float yaw;double time;EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignmentPOINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,(float, x, x) (float, y, y)(float, z, z) (float, intensity, intensity)(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)(double, time, time))typedef PointXYZIRPYT  PointTypePose;

添加额外的变量

注意：本文以ouster lidar为例，所以在变量命名过程中大多贴近Ouster

  1. ouster_buffer (存储ouster原始点云类型指针)，ouster_undistort (存储ouster单帧原始点云数据，从ouster_buffer提取数据)，key_frame_poses_data (存储PointLIO计算的关键帧位姿，点类型为前述定义的PointTypePose)

在li_initialization.h头文件中添加声明：

// set the ouster data buffer
extern  std::deque<pcl::PointCloud<ouster_ros::Point>::Ptr> ouster_buffer;
// set the ouster point cloud and the key frame pose
extern pcl::PointCloud<ouster_ros::Point>::Ptr ouster_undistort;
// set the key frame pose
extern pcl::PointCloud<PointTypePose>::Ptr key_frame_poses_data;

在li_initialization.cpp头文件中添加定义：

// set the ouster data buffer
std::deque<pcl::PointCloud<ouster_ros::Point>::Ptr>  ouster_buffer;
// set the ouster point cloud and the key frame pose
pcl::PointCloud<ouster_ros::Point>::Ptr ouster_undistort(new pcl::PointCloud<ouster_ros::Point>());
// set the key frame pose
pcl::PointCloud<PointTypePose>::Ptr key_frame_poses_data(new pcl::PointCloud<PointTypePose>());

上述两部分均放在了lidar_buffer, time_buffer, imu_deque等变量之后．

２. pubKeyFrameInfo变量，以发布关键帧相关信息
在laserMapping.cpp文件中添加，定义如下：

 ros::Publisher pubKeyFrameInfo  = nh.advertise<point_lio::key_frame_info> ("/point_lio/key_frame_info", 100000);

即发布"/point_lio/key_frame_info"消息，存储关键帧位置的原始点云，转换到world坐标系的点云，关键帧位姿等信息．

添加自定义函数

  1. 在laserMapping.cpp文件中添加发布关键帧信息函数publish_key_frame_info：
/******* Publish Key Frame Info *******/
void publish_key_frame_info(const ros::Publisher pubKeyFrameInfo)
{// set the key frame infopoint_lio::key_frame_info keyframeInfo;keyframeInfo.header.stamp = ros::Time().fromSec(lidar_end_time);keyframeInfo.header.frame_id = "camera_init";pcl::PointCloud<ouster_ros::Point>::Ptr cloud(new pcl::PointCloud<ouster_ros::Point>);// transformed the ouster dataint size = ouster_undistort->points.size();pcl::PointCloud<ouster_ros::Point>::Ptr ousterCloudWorld(new pcl::PointCloud<ouster_ros::Point>(size,1));for (int i = 0; i < size; i++){pointBodyToWorld(&ouster_undistort->points[i], &ousterCloudWorld->points[i]);(&ouster_undistort->points[i], &ousterCloudWorld->points[i]);}// get the original Ouster datasensor_msgs::PointCloud2 ousterCloudOriMsg;pcl::toROSMsg(*ouster_undistort, ousterCloudOriMsg);ousterCloudOriMsg.header.stamp = ros::Time().fromSec(lidar_end_time);ousterCloudOriMsg.header.frame_id = "camera_init";// get the transformed Ouster datasensor_msgs::PointCloud2 ousterCloudWorldMsg;pcl::toROSMsg(*ousterCloudWorld, ousterCloudWorldMsg);ousterCloudWorldMsg.header.stamp = ros::Time().fromSec(lidar_end_time);ousterCloudWorldMsg.header.frame_id = "camera_init";// get current transtformationstd::vector<double> curr_pose = getKeyTransformation();// set the key frame Info msg data (include original data and tranformed to world)keyframeInfo.key_frame_cloud_ori = ousterCloudOriMsg;keyframeInfo.key_frame_cloud_transed = ousterCloudWorldMsg;/*** slected the keyframe at 1Hz ***/static int jjj = 0;if (jjj % 10 == 0) {// stack the pose at 1 HzPointTypePose pj;pj.x = curr_pose[0]; pj.y = curr_pose[1]; pj.z = curr_pose[2];pj.roll = curr_pose[3]; pj.pitch = curr_pose[4]; pj.yaw = curr_pose[5];key_frame_poses_data->push_back(pj);// convert the point cloud into sensor messagesensor_msgs::PointCloud2 keyFramePosesMsg;keyFramePosesMsg.header.stamp = ros::Time().fromSec(lidar_end_time);keyFramePosesMsg.header.frame_id = "camera_init";pcl::toROSMsg(*key_frame_poses_data, keyFramePosesMsg);// set the data of key frame infokeyframeInfo.key_frame_poses = keyFramePosesMsg;pubKeyFrameInfo.publish(keyframeInfo);}jjj++;
}
  1. 在Estimator.h头文件中添加转换点坐标系函数，专门针对Ouster类型进行处理：
// transform the ouster point from body to world
void pointBodyToWorld(ouster_ros::Point const * const pi, ouster_ros::Point * const po);

随后在对应的Estimator.cpp头文件中添加函数定义

// transform the ouster point from body to world
void pointBodyToWorld(ouster_ros::Point const * const pi, ouster_ros::Point * const po)
{if (pi == nullptr || po == nullptr) {std::cerr << "Error: Null pointer passed to RGBpointBodyToWorld!" << std::endl;return;}V3D p_body(pi->x, pi->y, pi->z);V3D p_global;if (extrinsic_est_en){	if (!use_imu_as_input){p_global = kf_output.x_.rot * (kf_output.x_.offset_R_L_I * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos;}else{p_global = kf_input.x_.rot * (kf_input.x_.offset_R_L_I * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos;}}else{if (!use_imu_as_input){p_global = kf_output.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; // .normalized()}else{p_global = kf_input.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; // .normalized()}}po->x = p_global(0);po->y = p_global(1);po->z = p_global(2);// copy pipo->intensity = pi->intensity;po->t = pi->t;po->reflectivity = pi->reflectivity;po->ring = pi->ring;po->ambient = pi->ambient;po->range = pi->range;}
  1. 在Estimator.h头文件中添加获取当前变换的函数，能够获取PointLIO系统当前处理的坐标系变换，输出为X,Y,Z,Roll,Pitch,Yaw，并存储在一个vector中：

声明如下：

// get current key transformation data
std::vector<double> getKeyTransformation();

在Estimator.cpp文件中的对应定义如下：

// get current key transformation data
std::vector<double> getKeyTransformation()
{SO3 ROT;vect3 TRANS;if (extrinsic_est_en){	if (!use_imu_as_input){ROT = kf_output.x_.rot * kf_output.x_.offset_R_L_I;TRANS = kf_output.x_.rot * kf_output.x_.offset_T_L_I + kf_output.x_.pos;// p_global = kf_output.x_.rot * (kf_output.x_.offset_R_L_I * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos;}else{ROT = kf_input.x_.rot * kf_input.x_.offset_R_L_I;TRANS = kf_input.x_.rot * kf_input.x_.offset_T_L_I + kf_input.x_.pos;// p_global = kf_input.x_.rot * (kf_input.x_.offset_R_L_I * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos;}}else{if (!use_imu_as_input){ROT = kf_output.x_.rot * Lidar_R_wrt_IMU;TRANS = kf_output.x_.rot * Lidar_T_wrt_IMU + kf_output.x_.pos;// p_global = kf_output.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; // .normalized()}else{ROT = kf_input.x_.rot * Lidar_R_wrt_IMU;TRANS = kf_input.x_.rot * Lidar_T_wrt_IMU + kf_input.x_.pos;// p_global = kf_input.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; // .normalized()}}// create an Affine variableEigen::Affine3d affine = Eigen::Affine3d::Identity();affine.linear() = ROT;affine.translation() = TRANS;// convert to the x y z roll pitch yaw variablesstd::vector<double> transformed_para;double x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles(affine, x, y, z, roll, pitch, yaw);// return datatransformed_para.push_back(x); transformed_para.push_back(y); transformed_para.push_back(z);transformed_para.push_back(roll); transformed_para.push_back(pitch); transformed_para.push_back(yaw);return transformed_para;
}

添加处理流程

首先，要能够用ouster_buffer 在standard_pcl_cbk回调函数中接收到ouster数据消息，修改如下：

// 修改前if (ptr->points.size() > 0){lidar_buffer.emplace_back(ptr);time_buffer.emplace_back(msg->header.stamp.toSec());}
// 修改后if (ptr->points.size() > 0){lidar_buffer.emplace_back(ptr);time_buffer.emplace_back(msg->header.stamp.toSec());// get the ouster datapcl::PointCloud<ouster_ros::Point>::Ptr ouster_data(new pcl::PointCloud<ouster_ros::Point>());pcl::fromROSMsg(*msg, *ouster_data);ouster_buffer.push_back(ouster_data);}

随后，在sync_packages函数中，从ouster_buffer提取数据，到ouster_undistort变量：

// 修改前if (lidar_buffer.empty() || imu_deque.empty()){return false;}
// 修改后if (lidar_buffer.empty() || imu_deque.empty() || ouster_buffer.empty()){return false;}

随后，提取数据：

    /*** push a lidar scan ***/if(!lidar_pushed){lose_lid = false;meas.lidar = lidar_buffer.front();meas.lidar_beg_time = time_buffer.front();//在这个位置提取数据:ouster_undistort = ouster_buffer.front();

在函数的最后，把ouster_buffer变量的头部数据pop掉：

    lidar_buffer.pop_front();time_buffer.pop_front();// 添加代码ouster_buffer.pop_front();lidar_pushed = false;imu_pushed = false;return true;

通过上述过程，我们能够在pointlio系统运行过程中不断获取ouster_undistort原始点云，回到laserMapping.cpp主函数中，我们在发布消息的位置添加我们自定义的关键帧信息发布函数publish_key_frame_info．

添加如下：

            /******* Publish points *******/if (path_en)                         publish_path(pubPath);if (scan_pub_en || pcd_save_en)      publish_frame_world(pubLaserCloudFullRes);if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body);// 添加自定义关键帧发布函数/******* Publish keyframe information *******/publish_key_frame_info(pubKeyFrameInfo);

  • 在publish_key_frame_info函数中，我们定义了一个关键帧消息point_lio::key_frame_info
    keyframeInfo;

  • 随后，使用自定义的pointBodyToWorld函数，把原始ouster_undistort数据，转换到了world坐标系，放到ousterCloudWorld变量中．

  • 分别对ouster_undistort变量和ousterCloudWorld变量，转换为ROS消息，放在keyframeInfo结构中．

  • 使用getKeyTransformation函数获取当前位姿，转换为PointTypePose类型，接着放在key_frame_poses_data变量中，同样转换为ROS消息，放在keyframeInfo结构中，以1Hz的频率发布．

我们的在线处理模块，参数文件中接收对应的"/point_lio/key_frame_info"话题，即可收到关键帧数据，进行处理，在线进行单木属性提取的工作．

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

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

关于隔离1

关于隔离1

1.隔离的目的&#xff1a; 在隔离电源设计中&#xff0c;输入与输出之间没有直接电气连接&#xff0c;提供绝缘高阻态&#xff0c;防止电流回路。这意味着输入与输出之间呈现为绝缘的高阻态&#xff0c;从而确保了无电流回路的形成。 隔离与可靠保护有关。电隔离是一种电路设…
阅读更多...
【java实现+4种变体完整例子】排序算法中【插入排序】的详细解析，包含基础实现、常见变体的完整代码示例，以及各变体的对比表格

【java实现+4种变体完整例子】排序算法中【插入排序】的详细解析，包含基础实现、常见变体的完整代码示例，以及各变体的对比表格

以下是插入排序的详细解析&#xff0c;包含基础实现、常见变体的完整代码示例&#xff0c;以及各变体的对比表格&#xff1a; 一、插入排序基础实现 原理 将元素逐个插入到已排序序列的合适位置&#xff0c;逐步构建有序序列。 代码示例 public class InsertionSort {void…
阅读更多...
清醒思考的艺术

清醒思考的艺术

成为穿越暴风雨后的幸存者 系统性错误是指系统性的偏离理性&#xff0c;偏离最理想的、合乎逻辑的、理智的思考和行为。 “系统”一词很重要&#xff0c;因为我们经常错误地走向同一方向。 幸存偏误 幸存偏误会扭曲概率&#xff0c;系统性的高估了成功概率。一旦混淆选择标准和…
阅读更多...
DSA数据结构与算法 6

DSA数据结构与算法 6

查找技术&#xff08;Searching Techniques&#xff09; 查找简介 在计算机科学中&#xff0c;“查找”指的是在某个集合或序列中寻找特定元素的过程。这个过程可以是成功的&#xff0c;也可以是失败的&#xff1a; 若目标元素存在于集合中&#xff0c;我们称之为“查找成功”…
阅读更多...
FastAPI：现代高性能Python Web框架的技术解析与实践指南

FastAPI：现代高性能Python Web框架的技术解析与实践指南

一、FastAPI的诞生背景与技术定位 在数字化转型的浪潮中，API（应用程序接口）作为连接服务与数据的核心枢纽，其性能与开发效率直接影响业务迭代速度。传统Python框架如Django和Flask虽功能丰富，但在高并发场景下面临性能瓶颈，且缺乏对异步编程的原生支持。FastAPI应运而生…
阅读更多...
VuePress 使用教程：从入门到精通

VuePress 使用教程：从入门到精通

VuePress 使用教程&#xff1a;从入门到精通 VuePress 是一个以 Vue 驱动的静态网站生成器&#xff0c;它为技术文档和技术博客的编写提供了优雅而高效的解决方案。无论你是个人开发者、团队负责人还是开源项目维护者&#xff0c;VuePress 都能帮助你轻松地创建和管理你的文档…
阅读更多...
1.Vue自动化工具安装（Vue-cli）

1.Vue自动化工具安装（Vue-cli）

目录 1.node.js 安装&#xff1a; 2 npm 安装 3 安装Vue-cli 4总结&#xff1a; 一般情况下&#xff0c;单文件组件&#xff0c;我们运行在 自动化工具vue-CLI中&#xff0c;可以帮我们编译单文件组件。所以我们在学习时一般需要在系统中先搭建vue-CLI工具 下面就是一些我…
阅读更多...
IP数据报

IP数据报

IP数据报组成 IP数据报&#xff08;IP Datagram&#xff09;是网络中传输数据的基本单位。 IP数据报头部 版本&#xff08;Version&#xff09; 4bit 告诉我们使用的是哪种IP协议。IPv4版本是“4”&#xff0c;IPv6版本是“6”。 头部长度&#xff08;IHL&#xff0c;Intern…
阅读更多...
Leetcode 2158. 每天绘制新区域的数量【Plus题】

Leetcode 2158. 每天绘制新区域的数量【Plus题】

1.题目基本信息 1.1.题目描述 有一幅细长的画&#xff0c;可以用数轴来表示。 给你一个长度为 n 、下标从 0 开始的二维整数数组 paint &#xff0c;其中 paint[i] [starti, endi] 表示在第 i 天你需要绘制 starti 和 endi 之间的区域。 多次绘制同一区域会导致不均匀&…
阅读更多...
Git Flow

Git Flow

Git Flow深度解析&#xff1a;企业级分支管理实战指南 前言 在持续交付时代&#xff0c;分支策略决定团队协作效率。Git Flow作为经典的分支管理模型&#xff0c;被Apache、Spring等知名项目采用。2023年JetBrains开发者调查报告显示&#xff0c;Git Flow仍是中大型项目最常用…
阅读更多...
[Swift]pod install成功后运行项目报错问题error: Sandbox: bash(84760) deny(1)

[Swift]pod install成功后运行项目报错问题error: Sandbox: bash(84760) deny(1)

操作&#xff1a; platform :ios, 14.0target ZKMKAPP do# Comment the next line if you dont want to use dynamic frameworksuse_frameworks!# Pods for ZKMKAPPpod Moyaend pod install成功后运行报错 报错&#xff1a; error: Sandbox: bash(84760) deny(1) file-writ…
阅读更多...
[管理与领导-129]：向上管理-组织架构、股权架构、业务架构、流程架构，看每个人在组织中的位置和重要性

[管理与领导-129]：向上管理-组织架构、股权架构、业务架构、流程架构，看每个人在组织中的位置和重要性

目录 一、股权架构&#xff1a;反映所有权与控制权 二、组织架构&#xff1a;定义角色与汇报关系 三、业务架构&#xff1a;定义业务单元与价值链 四、流程架构&#xff1a;规范业务运作与协作 五、综合分析&#xff1a;个人在组织中的综合影响力 六、案例&#xff1a;某…
阅读更多...
小红书爬虫，小红书api，小红书数据挖掘

小红书爬虫，小红书api，小红书数据挖掘

背景&#xff1a; 小红书&#xff08;Xiaohongshu&#xff09;是一款结合社交、购物和内容分享的移动应用&#xff0c;近年来在中国以及全球范围内拥有大量的用户群体。小红书上的内容包括用户的消费体验、生活方式、旅行分享、时尚搭配等。通过这些内容&#xff0c;用户可以了…
阅读更多...
玩转Docker | 使用Docker部署tududi任务管理工具

玩转Docker | 使用Docker部署tududi任务管理工具

玩转Docker | 使用Docker部署tududi任务管理工具 前言一、tududi介绍Tududi简介核心功能特点二、系统要求环境要求环境检查Docker版本检查检查操作系统版本三、部署tududi服务下载镜像创建容器创建容器检查容器状态检查服务端口安全设置四、访问tududi服务访问tududi首页登录tu…
阅读更多...
大屏设计与汇报：政务服务可视化实践

大屏设计与汇报：政务服务可视化实践

大屏设计与汇报：政务服务可视化实践 引言 在政务服务数字化转型浪潮中，大屏设计成为展现业务能力与数据价值的关键手段。本文围绕政务大屏设计，从设计要点、业务逻辑到汇报技巧展开深入探讨，为相关从业者提供全面参考。 一、大屏设计核心要点 （一）多维度考量 设计大…
阅读更多...
字节（抖音）golang后端

字节（抖音）golang后端

Golang知道哪些并发模式&#xff0c;你觉得哪个更好&#xff0c;为什么 在使用channel的时候有哪些需要考虑和注意的地方 进程和线程的区别 线程里有哪些字段 TCP和UDP的区别&#xff0c;各自的优劣势 TCP 更适合需要可靠性、顺序和连接管理的场景&#xff0c;如文件传输和网页…
阅读更多...
Python语法系列博客 · 第6期[特殊字符] 文件读写与文本处理基础

Python语法系列博客 · 第6期[特殊字符] 文件读写与文本处理基础

上一期小练习解答&#xff08;第5期回顾&#xff09; ✅ 练习1&#xff1a;字符串反转模块 string_tools.py # string_tools.py def reverse_string(s):return s[::-1]调用&#xff1a; import string_tools print(string_tools.reverse_string("Hello")) # 输出…
阅读更多...
Unity运行时查看日志插件 (IngameDebugConsole)

Unity运行时查看日志插件 (IngameDebugConsole)

Unity运行时查看日志插件 (IngameDebugConsole) 文章目录 Unity运行时查看日志插件 (IngameDebugConsole)一、介绍二、使用步骤1.导入插件2.开始使用 结束 一、介绍 In-game Debug Console插件可以在打包发布以后&#xff0c;程序运行时方便的看到控制台信息&#xff0c;在一些…
阅读更多...
spark-SQL核心编程课后总结

spark-SQL核心编程课后总结

通用加载与保存方式 加载数据&#xff1a;Spark-SQL的 spark.read.load 是通用加载方法&#xff0c;借助 format 指定数据格式&#xff0c;如 csv 、 jdbc 、 json 等&#xff1b; load 用于指定数据路径&#xff1b; option 在 jdbc 格式时传入数据库连接参数。此外&#xff0…
阅读更多...
蔡浩宇的AIGC游戏革命：从《原神》到《Whispers》的技术跨越

蔡浩宇的AIGC游戏革命：从《原神》到《Whispers》的技术跨越

目录 引言&#xff1a;游戏行业的AI革命前夜 一、《Whispers》的技术突破与市场挑战 1.1 多模态AI技术的集成应用 1.2 与传统游戏的差异化体验 1.3 面临的商业化难题 二、从《原神》到《Whispers》的技术演进 2.1 《原神》成功的时代因素分析 2.2 蔡浩宇的技术路线转变 …
阅读更多...
最新文章

Copyright @ 2022~2023