從數據流視角解析 loam_livox

loam_livox 和 loam 的原理很多是類似的，想了解原理可以看這篇文章：LOAM 細節分析。

解析一套代碼，一般有兩種視角。一種是按類(class)解析，另一種則是從數據流向解析。本文就從數據流的視角來解析一下 loam_livox 。
代碼地址：https://github.com/hku-mars/loam_livox
配置就按它的 github 上面的教程就行了，注意 pcl 用 v1.9 的，版本低了運行時 ceres 會報錯。

這篇博客是之前寫的簡介，可以看出，loam_livox 的整體結構並不複雜，核心節點就兩個：livox_scanRegistration 主要用於讀入數據、特徵提取；livox_laserMapping 主要用於位姿估計、建圖。

下面就用 3 張流程圖來解析數據流向：

第1部分

第1步，就是用 rosbag play 讀入數據，原始數據發佈在 /livox/lidar 話題上。

第2步，livox_scanRegistration 節點，是由 laser_feature_extractor.cpp/hpp 生成的，主要是用原始數據進行特徵提取。與特徵提取緊密相關的文件還有 livox_feature_extractor.hpp.

第3步，livox_scanRegistration 節點直接通過構造函數調用init_ros_env()函數，初始化一些參數，並且創建一些發佈者和訂閱者。其中就訂閱了 /laser_points 話題，它其實在launch文件中被remap成了 /livox/lidar 話題（launch文件代碼如下），剛好就是 rosbag play 發佈的話題。

<node pkg="loam_livox" type="livox_scanRegistration" name="livox_scanRegistration">
    <remap from="/laser_points" to="/livox/lidar" />
</node>

不熟悉remap的戳這。

第4步，laserCloudHandler()是 /laser_points 話題的回調函數，如下所示：

m_sub_input_laser_cloud = nh.subscribe<sensor_msgs::PointCloud2>(
  "/laser_points", 10000, &Laser_feature::laserCloudHandler, this );

這個函數兩個作用：把ros消息轉換爲點雲，存放到變量laserCloudIn中；調用extract_laser_features()和get_features()，把變量laserCloudIn進行特徵提取，並且分爲三類，livox_corners、livox_surface、livox_full，大概流程就是：

把特徵分成corners與surface兩類，這和 LOAM 是類似的。

LOAM的特徵提取基於曲率，只提取兩種特徵點，corner和surface，分別對應場景的平面區域和曲折區域。LOAM沒有使用特徵描述子(連曲率都沒有參與後續的匹配)。從代碼中的corner與surface的曲率判斷閾值可以看出，LOAM提取的corner和surface特徵點的曲率， 並沒有特別大的差別，這使得LOAM有較強的場景適應性，在場景中比較曲折的區域，corner點會佔據主導，而在較爲平緩的區域，surface點佔據主導. 在激光掃描到的一塊區域，總會提取出幾個特徵點。（引自：LOAM 細節分析）

livox_full 包含了所有的點雲，是經過篩選的 good points。篩點的策略用原文中的圖來簡單說明一下：

篩點策略：

• 在視角範圍內的邊緣處（ fringe ），如大於17度。這類點有強畸變，所以捨去
• 強度（ intensity ），太大或太小
• 與平面的夾角接近0或180度的，如上圖的點f
• 即將被遮擋的點，如上圖點e

第5步，把上一步獲得的3類數據轉換爲ros消息，再把ros消息又發佈出去，轉換過程如下圖：

發佈出來的3類消息，供下一部分的節點 livox_laserMapping 使用。

第1部分總結：主要就是對原始數據進行壞點剔除、特徵提取，獲取兩類特徵（coners 特徵和 surface 特徵）、三組數據（ /pc2_coners、/pc2_surface、/pc2_full）

第2部分

第1步，節點 livox_laserMapping 中的3個訂閱者（m_sub_laser_cloud_corner_last、m_sub_laser_cloud_surf_last、m_sub_laser_cloud_full_res ）訂閱了第1部分產生的3個話題（/pc2_coners、/pc2_surface、/pc2_full），並調用各自對應的函數形成數據對（data_pair）。其中都要調用 get_data_pair 函數。然後把獲取到的 data_pair 推進隊列 m_queue_avail_data 中。

第2步，核心函數 process ，處理從上一步得到的數據隊列（m_queue_avail_data）。進入 while 循環，逐個處理隊列中的數據，當前數據是 current_data_pair 。然後再把ROS消息轉換爲點雲，流程如圖所示。對應的代碼在 process 函數中，如下：

m_mutex_querypointcloud.lock();

m_laser_cloud_corner_last->clear();
pcl::fromROSMsg( *( current_data_pair->m_pc_corner ), *m_laser_cloud_corner_last );

m_laser_cloud_surf_last->clear();
pcl::fromROSMsg( *( current_data_pair->m_pc_plane ), *m_laser_cloud_surf_last );

m_laser_cloud_full_res->clear();
pcl::fromROSMsg( *( current_data_pair->m_pc_full ), *m_laser_cloud_full_res );
m_mutex_querypointcloud.unlock();

delete current_data_pair;

注意，圖中的 m_laser_cloud_full 意思不是指歷史的全部點雲，而是當前幀的全部點雲數據。是相比於 corner、surf 的 full 。

第3步，還是在 process 函數的 while 循環中。上一步逐個處理了隊列中的數據，把ROS消息轉換爲點雲變量。前面的步驟就是把數據轉成ros消息，再把ros消息轉成變量，再把變量轉成ros消息……主要用到的兩個函數就是 pcl::fromROSMsg 和 pcl::toROSMsg。如此反覆折騰，有必要嗎？應該是有它的道理的，只不過我暫時不懂……
那麼此步繼續折騰，調用 process_new_scan 函數，又把數據轉換到 current_laser_cloud_corner_last、current_laser_cloud_surf_last、current_laser_cloud_full 這三個變量中，具體如上圖所示。當然，process_new_scan 函數還做了其他事，我這隻關心數據流向，就只列出了這部分。調用 process_new_scan 函數的代碼如下，在 process 函數的 while 循環中。

std::future<int> *thd = new std::future<int>(
  std::async( std::launch::async, &Laser_mapping::process_new_scan, this ) );

第4步，繼續“折騰”，此步是最最最原始的數據進化到優秀原始數據的大結局，從上圖可以看到，優秀原始數據是上一步經過濾波、降採樣得到的（似乎稱爲精英原始數據更好），終於不用反覆折騰了。這裏所謂的優秀原始數據，就是指：已經經過壞點剔除的，分好了類的（分爲coners、surface、full）。那麼，優秀原始數據就可以享有兩項特權：

• 被保存的特權，被處理好的當前幀的所有點雲，都保存到 raw_xxx.pcd 文件中（xxx 是當前幀的ID）。代碼如下，這句代碼就是馬上下一部分要講到的關鍵語句之一。

m_pcl_tools_raw.save_to_pcd_files( "raw", current_laser_cloud_full, m_current_frame_index );

• 進入下一關的特權。只有優秀的數據才能進入下一步，繼續被處理、做里程計、迴環檢測等等。

第2部分總結：把第1部分拿到的ros消息推進隊列，在循環中逐個進行精化處理，獲得優秀原始數據，保存它們並進入下一部分。

第3部分

引言： 這部分關鍵的兩條語句就是上圖中標黃的部分，分別是保存 raw 數據和 aft_map 數據的。如果你運行過 loam_livox 的迴環檢測的程序，就會發現在 Loam_livox_loop 目錄下會生成 raw 和 aft_map 這兩類數據。raw 數據就是上一部分提到的“優秀”原始數據，aft_map 數據就是經過後端優化。
注意，注意， aft_map 數據只是經過後端優化的，如果你開了迴環檢測，它並不是經過迴環檢測優化後的點雲。想獲得經過迴環檢測優化的點雲可以自己改代碼，它保存在 laser_mapping.hpp 中的變量 pts_opm 中，代碼修改如下：

auto refined_pt = map_rfn.refine_pointcloud( map_id_pc, pose3d_map_ori, temp_pose_3d_map, pc_idx, 0 );
auto pts_opm = map_rfn.refine_pointcloud( map_id_pc_keyframes, pose3d_map_ori, temp_pose_3d_map, pc_idx);
if( m_if_save_to_pcd_files && PCD_SAVE_RAW)
{
    m_pcl_tools_aftmap.save_to_pcd_files("keyframe_opm", pts_opm, pc_idx);
}

上面這樣即把經過了迴環檢測優化的點雲也保存在 Loam_livox_loop 目錄下，並以 keyframe_opm 爲前綴。
因爲迴環檢測優化是針對關鍵幀的，所以上面保存的也只有關鍵幀的，並沒有所有幀的。上面代碼中 pts_opm 是該關鍵幀經過迴環優化後的點雲數據， pc_idx 是關鍵幀的ID號，./Loam_livox_loop/file_name.txt 文件中有關鍵幀的ID號與所有幀的ID號的映射關係。
數量關係：raw 數據和 aft_map 數據的數量與最最最原始的數據的幀數是一樣的，比如最最最原始的數據有4800幀，raw 和 aft_map 也有4800幀，但是關鍵幀可能只有48幀，程序設定爲間隔100幀選一個關鍵幀，代碼爲證如下：

// 在 laser_mapping.hpp 中的 service_pub_surround_pts 函數中
while ( 1 )
{
    while ( m_current_frame_index - last_update_index < 100 )
    {
        std::this_thread::sleep_for( std::chrono::milliseconds( 1500 ) );
    }
    last_update_index = m_current_frame_index;
    // ……
}

當然了，程序中的選關鍵幀策略除了這個固定間隔100幀的，還有其他策略，感興趣的可以自行研究。

第1步，從第2部分獲得了 laserCloudCornerStack、laserCloudSurfStack、current_laser_cloud_full 三類數據，除了把它們保存爲 raw 數據，當然還要做里程計，就用 Corner 特徵和 Surf 特徵，對應的就是兩個數據棧 laserCloudCornerStack、laserCloudSurfStack ，它們兩個是作爲當前幀的數據輸入到 find_out_incremental_transfrom 函數中，這個函數需要三類參數共6個，每類參數都是一樣的，分爲Corner 和 Surf 兩類。find_out_incremental_transfrom 函數需要的參數，除了剛剛提到的當前幀數據，還需要歷史的地圖數據，就是用當前幀數據與歷史地圖數據進行配準，得到當前幀相對於歷史地圖座標系的最優位姿，也就是相對於全局座標系的。爲了加快索引速度，還會把歷史數據建立一個 kd 樹。此步的求得的位姿結果，就是用 ceres 優化過的位姿。

第2步，用 pointAssociateToMap 函數把當前幀的數據對齊到全局座標系下，並把它們加入到歷史地圖中，並更新歷史地圖。具體過程如上圖所示。

第3步，隨着數據的不斷讀入，歷史地圖也不斷更新，所以第1步中 find_out_incremental_transfrom 函數的歷史地圖那類參數也是不斷更新。

第4步，最後獲得 aft_map 數據，注意，注意， aft_map 數據只是經過後端優化的，並沒有經過迴環優化。

第3部分總結：這兩條關鍵語句是在 process_new_scan 函數中，其中包含了前端激光里程計、後端優化、迴環檢測等關鍵步驟。是算法原理的核心，需要好好研究。

結尾：從數據流視角解析 loam_livox 就到這了，相信大家現在可以比較清晰地在腦海中想像出原始數據是怎麼被反覆“折騰”的。

最後的最後，看代碼時發現一個問題有點疑惑，於是在github上面提了一個 issue，結果作者沒理我就關了問題。麻煩知道的朋友可以在評論區給我個解答，謝謝～

按文件、函數、原理解析，to be continued…