small_gicp_relocalization（先驗點雲地圖重定位）

1.small_gicp_relocalization（先驗點雲地圖重定位）

1.1.實車先看：地圖、起點和雲臺零位

這個節點在已有地圖導航模式 slam:=False 下，把當前配準點雲與先驗PCD地圖對齊，輸出 map → odom 的位置、朝向修正。它不直接控制車輪或雲臺，也不是重新建立地形障礙的節點；slam:=True 的整套啟動不執行它。

當前沒有實物，也沒有本節點的有效重定位測試。下文分別說明“程式碼如何解釋這些值”和“實車是否適用”，不把原始碼流程當作已經驗證的定位效果。

1.2.引數

1.2.1.ros__parameters（ROS節點引數）

1.2.1.1.參數列

前12項對應當前實物 YAML；prior_pcd_file 雖被註釋，卻由 launch另行傳入；input_cloud_topic 沒寫在這份 YAML中，仍使用原始碼預設值。這兩項也列出，便於完整理解輸入來源。

small_gicp_relocalization:
  ros__parameters:
    use_sim_time: False
    num_threads: 4
    num_neighbors: 20
    global_leaf_size: 0.1
    registered_leaf_size: 0.05
    max_dist_sq: 3.0
    map_frame: "map"
    odom_frame: "odom"
    base_frame: "base_footprint"
    robot_base_frame: "gimbal_yaw"
    lidar_frame: "front_mid360"
    init_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]   # [x, y, z, roll, pitch, yaw]
    # The prior_pcd_file does not need to be specified since it going to be set by defaults in launch.
    # prior_pcd_file: ""

本塊沒有當前完全不使用的自定義引數：robot_base_frame 只在收到手動初始位姿時參與，init_pose 只在啟動時設初值，它們不是持續調節項。話題、座標系和路徑不能按數值大小調參。

1.2.1.2.引數詳解

1.2.1.2.1.輸入與輸出：匹配的是哪兩份點雲

即時點雲已經由Point-LIO與介面節點配準到 odom，本節點直接累加座標，不再逐幀轉雷達系，也不額外按雲臺yaw旋轉這些點。雲臺旋轉時，靜態環境應仍在共同的odom座標下對齊；這個結論依賴上游位姿、安裝外參和時間介面正確，目前沒有獨立yaw驗證。

回撥只儲存最近輸入的frame_id與時間戳，沒有檢查多幀是否同系，也沒有在frame_id變化時清空累計點。PCD沒有ROS訊息那樣的frame_id，本節點按自己的來源假設處理；檔名和座標系字串不能證明地圖已經對應。

每500 ms真即時間嘗試匹配一次，有點才處理；每輪降取樣、估計區域性形狀、搜尋地圖對應點，最多做10次最佳化迭代。匹配報告收斂才更新結果，未收斂則沿用舊結果，但本輪累計點仍會清空。收斂表示演算法停止條件滿足，不證明匹配到了場地中正確的位置；原始碼沒有再按匹配誤差、有效對應比例等檢查可信度。

1.2.1.2.2.prior_pcd_file：先確認用的是自己的場地地圖

整套啟動的預設路徑在安裝後的 pb2025_nav_bringup 共享目錄：

相應實物預設二維地圖 map/reality/rmul_2024.yaml 也不存在；模擬預設二維地圖存在。這只是當前預設檔案檢查，沒有讀取執行節點引數，也不排除使用者另傳world、PCD或二維地圖路徑。上實物前需要準備當前場地的對應地圖，不能把模擬PCD存在當成實物地圖已準備。

localization_launch.py 在引數檔案之後，額外傳入 {"prior_pcd_file": prior_pcd_file}，因此註釋掉這一行不表示節點不使用先驗PCD。它與Point-LIO是否啟用 prior_pcd_en 是不同功能：Point-LIO可不載入先驗，重定位節點仍需要先驗PCD。

檔案載入失敗只記錄錯誤並返回，建構函式仍繼續準備匹配目標，沒有明確阻止空目標進入後續匹配；不能把“節點起來了”視為地圖載入成功。原始碼不會在執行時重新讀取PCD，也不會替你核對它與Nav2二維地圖的位置、朝向、米制尺度或場地版本是否一致。

1.2.1.2.3.base_frame 與 lidar_frame：啟動時會轉動整份地圖

PCD讀入後，原始碼查詢 base_footprint ← front_mid360，再把這個矩陣左乘到整份地圖的每個點。它按節點當前時間查詢，失敗後每秒重試，成功一次就繼續；沒有查詢次數上限，也沒有在後續雲臺轉動時更新目標地圖。

這對應原始碼的一個來源假設：PCD基準是初始 lidar_odom，用初始底盤與雷達關係把地圖轉換到所需參考中。但當前Point-LIO實際儲存的是 camera_init 下的配準世界點，且其位姿原點涉及內部IMU；不是每一幀的雷達自身座標。把這類PCD當初始雷達系直接套機械變換，是否與實際建圖基準一致，必須複核，不能僅照README的命名認定轉換正確。

雷達在雲臺上，這一步特別依賴零位。 同一份PCD若建圖時與重新啟動時雲臺相對yaw不同，當前 base←lidar 會含不同角度；按原始碼計算，目標地圖可能被不同矩陣轉動，而獨立載入的二維地圖不會自動跟著調整。是否實際造成錯位，需要結合PCD生成時的起點、雲臺零位、底盤方向與地圖原點判斷，目前沒有測試結論。

該查詢與 loam_interface 的一次性基準轉換意圖相關，但兩節點分別查詢，不保證取到同一時刻的雲臺角。這裡的機械關係始終保留：MID360掛在 gimbal_yaw，只有實際yaw會帶動它，實際pitch位於後續層；兩個odom關節固定。重力參考、內部IMU→雷達關係或地圖來源錯誤，不能通過增大匹配距離掩蓋。

包內README還寫過“未設定時使用空變換”，當前實現沒有對應的單位變換旁路：若PCD能讀取但frame為空或TF缺失，會進入重試。應以當前原始碼為準，本輪不修改節點或地圖轉換。

1.2.1.2.4.init_pose 與 robot_base_frame：兩種初始位姿不是同一個物件

YAML的 init_pose 直接寫入匹配矩陣。 在當前輸入為odom、目標為map的約定下，它是 map←odom 初值，即“odom原點在map中的位置和方向”。

[0,0,0,0,0,0] 表示先猜兩系完全重合；這隻有在起始參考關係吻合時才合適，不表示演算法已知道機器人在地圖中的真實位置。位置單位m，角度rad；偏航90°應為約1.5708，而不是填90。旋轉按yaw、pitch、roll組成 Rz×Ry×Rx。

例如：若odom起點恰好與所描述的機器人原點重合且朝向一致，那麼機器人起始地圖位姿可能就是map←odom；若odom原點與雲臺存在高度或安裝偏移，或機器人已在odom內移動，就不能直接照抄機器人位置。當前底盤、雲臺、雷達原點不同，需按實際參考關係計算。

話題 initialpose 則被當作 robot_base_frame=gimbal_yaw 在地圖中的位姿。 回撥再查詢最新 gimbal_yaw←输入扫描系，當前掃描系應為odom，兩者相乘得到map←odom：

map←odom = 人工给定的 map←gimbal_yaw × TF中的 gimbal_yaw←odom

所以RViz箭頭若被你理解成底盤車頭，而當前雲臺相對底盤左轉90°，就可能少算一個雲臺角。位置也對應雲臺原點：若人工訊息給z=0，原始碼把它當作雲臺原點位於map的z=0，而不是“底盤地面高度為0”；高度應按這份地圖的基準核對。只改 robot_base_frame 的名字也需要重新核對整條關係，不能把“機器人”三個字自動等同於底盤。本引數僅該回呼叫，不在每次點雲匹配時扣機械角。

回撥沒有按訊息header.frame_id轉換位姿、沒有使用其協方差，也沒有按訊息時間查TF，而是取最新變換；收到首幀掃描前，掃描frame_id還為空，查詢可能失敗。手動設初值只改當前和下一輪匹配猜測，不會重轉PCD，也不清空已累計點。雲臺快速轉動時，人工位姿對應的朝向與最新TF是否同一時刻也要核對。

1.2.1.2.5.max_dist_sq：1.73 m是配對門檻，不是定位誤差指標

當前構建使用的small_gicp程式碼直接判斷“對應點距離平方是否大於 max_dist_sq”；超出就拒絕。源點先按當前猜測變換到地圖，再找最近地圖點，才檢查這項，因此在米制點雲中單位為m²：

max_dist_sq = 3.0 m² → 最大对应点距离约 sqrt(3) = 1.73 m
希望门槛为0.5 m → 平方值为0.25 m²

第二行僅解釋單位換算，不是建議修改當前值。它不限制機器人總移動距離，不表示允許定位偏差1.73 m，更不是掃描量程。錯誤初值下，源點仍可能碰巧靠近另一面牆並通過門檻。

實車中，放寬可留下更多點對，但重複柱子、相似走廊和動態物體也可能參與誤配；收緊會減少遠距離錯配，卻可能讓真實初值偏差稍大時無足夠點對。先確認地圖、初值和參考系，再結合正確匹配／誤匹配情況調整，不能單獨把“更小”理解成“更準”。

該單位按當前構建快取庫原始碼核實，庫提交為 fa0cfc983417c044b6036f71dfa448038bb9c820。CMake引用master，重新獲取依賴後應複核版本，不把當前快取行為當成所有版本的固定承諾。

1.2.1.2.6.兩個leaf_size、num_neighbors 與 num_threads（點雲精簡、鄰點數量與計算執行緒）

global_leaf_size=0.1 m 處理先驗地圖；registered_leaf_size=0.05 m 處理每輪累計即時點雲。兩者不要求相同，也不等於地形分析格子、Nav2地圖解析度或定位精度。地圖處理不會改原PCD，即時處理不會改變Point-LIO本身的輸出。

調小通常保留更多細節，代價是計算量增加；原始資料沒有的細節不會因調小而出現。遠處點稀疏、牆角或小柱子能否穩定描述，需看實際資料；人員、敵車等動態點在本節點沒有專門篩除，即時點更多也可能帶來更多幹擾。

num_neighbors=20 指每點用於描述區域性形狀的鄰點數，地圖與即時點都用它。它不是20個匹配候選或20幀累計。範圍太大可能混入牆角兩面、坡面與障礙等不同結構，太小又可能受噪聲影響；當前庫實際找到的鄰點不足5時，會設為無效區域性特徵的預設值，因此稀疏區域與降取樣尺度要一起看。

num_threads=4 控制鄰域估計、建樹及匹配的平行計算。降取樣呼叫沒有傳這個引數，當前庫該函式預設4執行緒；把引數改大，不等於所有步驟都跟著增加執行緒。它不是節點頻率，也不保證執行緒翻倍就快一倍，先沿用當前值。

1.2.1.2.7.map_frame、odom_frame、use_sim_time 與輸出邊界（地圖、里程計參考系與時間基準）

地圖匹配結果是“目標地圖←輸入累計點雲”的變換；輸出直接命名為 map→odom。要使這個名字正確，必須先保證目標PCD和輸入實際座標符合約定；本節點沒有另行變換到這兩個名稱指定的系。

輸出約20 Hz真即時間重複當前結果，點雲匹配則約2 Hz真即時間嘗試一次；20 Hz不表示每次都得到新定位。TF時間取最後收到的掃描時間加0.1秒，固定在原始碼中，不是本塊引數；也沒有“已成功匹配過”的輸出保護，啟動初值同樣可被髮布。這裡只記錄輸出介面，仍不展開此前擱置的導航走停或TF時效故障排查。

use_sim_time 應與輸入、機械TF和啟動模式一致。True影響節點當前時間與TF快取，不會讓兩個wall timer隨模擬倍速自動調整，也不會修復此前Point-LIO點時間單位或內部位姿轉換問題。

1.2.1.2.8.啟動來源與當前驗證範圍

13項自定義引數均在構造時宣告和讀取，沒有動態引數更新回撥；init_pose 用於啟動，人工初值走 initialpose 訊息。13項之外的ROS標準 use_sim_time 由節點時鐘處理，不能混為演算法引數動態更新。

• 原始碼能確認： 引數讀取、地圖一次性轉換、累積配準點雲、平方距離門檻、啟動初值與人工初值的區別、匹配收斂後的更新規則和輸出介面。
• 模擬已觀察： 2026-10-08僅驗證獨立Point-LIO靜止和約20 cm往返；未驗證本節點、先驗地圖匹配、獨立雲臺yaw、人工初值或全域性定位。
• 必須實物測量／檔案來源核實： PCD與二維地圖的共同基準、建圖與啟動雲臺零位、雷達／內部IMU／底盤外參、地圖米制尺度及實際起點、可見結構與動態干擾、正確位置下的匹配穩定性。當前重力方向和點時間問題繼續保留待核實。

1.3.開發問題

1.3.1.關掉small_gicp

主要調整是關閉 small_gicp、增加靜態 map → odom；導航節點繼續複用原來的 navigation_launch.py

1.3.2.日誌某些報錯

1.3.2.1.GICP did not converge.

點雲對不上，要自己跑一邊slam建出來PCD，然後再使用這個PCD進行匹配。

1.3.2.2.No accumulated points to process.

這條提示表示 small_gicp 的即時點雲快取為空，發生在匹配之前。現在先查資料有沒有送到，暫時不用調匹配引數或初始位姿。

它預設訂閱的是 /registered_scan，資料鏈是：

MID360 → Point-LIO → /cloud_registered
                         ↓ loam_interface
                   /registered_scan → small_gicp