任幹部落格之前段里程計

從front_end_node.cpp開始：

首先建立了類frontendflow的共享指標front_end_flow_ptr，然後再執行run函式。

#include
#include
"glog/logging.h"
#include
"lidar_localization/global_defination/global_defination.h"
#include
using
namespace lidar_localization;
intmain
(int argc,
char
*ar**)
return0;
}

下面具體看一下run()函式實現的具體過程：

首先檢查cloud_data_buff_中是否讀到資料，然後再檢查是否有資料，當資料有效的時候，更新雷射里程計updatelaserodometry()再發布資料，接下來主要研究函式updatelaserodometry()的功能：

里程計資料沒有初始化的時候，先設定初始pose，並返回更新的里程計資料

里程計初始化完成後，直接返回更新的里程計資料。

bool frontendflow::
updatelaserodometry()
return front_end_ptr_-
>
update
(current_cloud_data_, laser_odometry_)
;}

這裡設定的初始位姿是單位矩陣，init_pose_值的預設值就是單位矩陣啊，這裡不是很理解，應該可以在這裡賦值別的旋轉矩陣。

接下來詳細看一下update（）函式：

1.移除無效點：

pcl::
removenanfrompointcloud
(*cloud_data.cloud_ptr,
*current_frame_.cloud_data.cloud_ptr, indices)
;

2.進行點雲濾波處理：

clouddata::cloud_ptr filtered_cloud_ptr
(new clouddata::
cloud()
);frame_filter_ptr_-
>
filter
(current_frame_.cloud_data.cloud_ptr, filtered_cloud_ptr)
;

3.如果區域性地圖容器中沒有關鍵幀，代表是第一幀資料，此時將當前幀資料作為第乙個關鍵幀，並更新區域性地圖容器和全域性地圖容器。

if
(local_map_frames_.
size()
==0)

此處更新資料函式具體實現如下，主要完成的功能：

（1）將關鍵幀的指標指向新傳進來的指標；

frame key_frame = new_key_frame; // 這一步的目的是為了把關鍵幀的點雲儲存下來 // 由於用的是共享指標，所以直接複製只是複製了乙個指標而已 // 此時無論你放多少個關鍵幀在容器裡，這些關鍵幀點雲指標都是指向的同乙個點雲 key_frame.cloud_data.cloud_ptr. reset (new clouddata:: cloud (*new_key_frame.cloud_data.cloud_ptr)

);

（2）更新區域性地圖，當區域性地圖大於一定值時對第乙個進行刪除；

// 更新區域性地圖
local_map_frames_.
push_back
(key_frame)
;while
(local_map_frames_.
size()
>
static_cast
(local_frame_num_)
)

（3）將點雲進行座標變化並累加在區域性地圖指標上；

local_map_ptr_.
reset
(new clouddata::
cloud()
);//將之前的進行清空，並且在for迴圈中將當前區域性地圖的關鍵幀加入
for(size_t i =
0; i < local_map_frames_.
size()
;++i)

（4）更新ndt匹配的目標點雲，關鍵幀比較少的時候先不進行濾波，直接設定成目標點雲，點雲多的時候濾波完再設定成目標點雲。

// 更新ndt匹配的目標點雲
// 關鍵幀數量還比較少的時候不濾波，因為點雲本來就不多，太稀疏影響匹配效果
if(local_map_frames_.
size()
<10)
else

4.不是第一幀的時候，就正常匹配：

clouddata::cloud_ptr result_cloud_ptr
(new clouddata::
cloud()
);registration_ptr_-
>
scanmatch
(filtered_cloud_ptr, predict_pose, result_cloud_ptr, current_frame_.pose)
;    cloud_pose = current_frame_.pose;

5.更新相鄰兩幀資料的相對運動：

step_pose = last_pose.
inverse()
* current_frame_.pose;
predict_pose = current_frame_.pose * step_pose;
last_pose = current_frame_.pose;

6.匹配之後根據距離判斷是否需要生成新的關鍵幀資料，如果需要則做相應的更新：

if
(fabs
(last_key_frame_pose(0
,3)- current_frame_.
pose(0
,3))
+fabs
(last_key_frame_pose(1
,3)- current_frame_.
pose(1
,3))
+fabs
(last_key_frame_pose(2
,3)- current_frame_.
pose(2
,3))
> key_frame_distance_)

至此update()函式就結束了。

任幹部落格之前段里程計

遊戲開發之前段工作流程

寫在部落格更新之前

ubuntu系統部落格之前筆記

任幹部落格之前段里程計

遊戲開發之前段工作流程

寫在部落格更新之前

ubuntu系統 部落格之前筆記

相關推薦

ubuntu系統部落格之前筆記