修改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.pswp.cn/pingmian/77481.shtml
繁體地址，請注明出處：http://hk.pswp.cn/pingmian/77481.shtml
英文地址，請注明出處：http://en.pswp.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