NDT和ICP構建點云地圖 |【點云建圖、Ubuntu、ROS】

###

本博客記錄學習NDT，ICP構建點云地圖的實驗過程，參考的以下兩篇博客：

無人駕駛汽車系統入門（十三）——正態分布變換（NDT）配準與無人車定位_settransformationepsilon-CSDN博客

PCL中點云配準（非常詳細，建議收藏）_pcl點云配準-CSDN博客

代碼可自取：Xz/NDT構建點云地圖 - Gitee.com

###

一、bag包點云數據處理

1.1 首先查看bag包的信息

rosbag info <包名>

通過rosbag info 可以查看bag數據包的信息，主要是一些話題名稱和消息類型。

1.2 對點云數據轉化為pcd格式保存

????????PCD（Point Cloud Data）是專為點云設計的文件格式，支持存儲多維數據（如坐標、顏色、強度、法向量、時間戳等）。其核心優勢在于：

標準化
高效存儲
完整保留屬性
生態支持廣泛

rosrun pcl_ros bag_to_pcd <bag數據包名稱>.bag <點云話題名稱> <保存的文件夾名>.pcd

隨后，在同目錄下生成data.pcd文件夾，存放點云數據數據：

這樣，點云數據處理完成。

二、NDT構建點云地圖

2.1 NDT介紹

????????正態分布變換（Normal Distributions Transform，NDT）是一種依賴于高精地圖和激光雷達的定位技術。是一類利用已有的高精度地圖和激光雷達實時測量數據實現高精度定位的技術。其核心思想是將點云空間劃分為多個網格（Cell），對每個網格中的點云擬合一個正態分布（高斯分布），然后用概率模型描述整個點云。通過優化匹配概率，找到兩個點云之間的最佳變換（旋轉和平移）。

2.2 NDT算法步驟

2.2.1 劃分體素網格

????????將參考點云劃分為均勻的立方體網格（體素）。每個體素的大小需根據場景調整通常在0.1m到數米之間。對每個體素內的點云，計算均值向量 $\mu$ 和協方差矩陣 $\sum$ ：

2.2.2 初始化變換參數

????????給定初始猜測變換 $T_{init}$ （如里程計估計或前一幀位姿），將當前點云 $P_{current}$ ?投影到參考坐標系。

2.2.3 構建目標函數

????????對于變換后的當前點 $T_{init}$ ，找到其所在參考體素，計算其在該體素分布下的概率密度：

????????其中k為維度（2D為3，3D為6）。

????????目標函數：

2.2.4 優化求解變換參數

????????在NDT（正態分布變換）算法中，構建完目標函數后，意味著將點云配準問題轉化為一個數值優化問題，接下來需要通過優化算法求解最優的變換參數（旋轉和平移），使得目標函數值最大化（或最小化，取決于定義）。這一步驟是NDT的核心，直接決定了配準的精度和效率。

????????使用迭代優化，在每次迭代中，首先計算目標函數的梯度和Hessian矩陣以確定優化方向與步長，隨后利用牛頓法或Levenberg-Marquardt等算法更新變換參數，并通過收斂條件（如梯度閾值、迭代次數或參數變化量）判斷是否終止。這一過程逐步將當前點云對齊至參考點云的高概率區域，最終輸出最優變換矩陣，完成配準。迭代優化的效率與精度高度依賴初始位姿、體素大小及優化算法的選擇。

三、代碼介紹

3.1 讀取點云與NaN過濾

????????從PCD文件加載點云，并移除無效點（NaN）

pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path) {pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1) {PCL_ERROR("Couldn't read the pcd file\n");return nullptr;}std::vector<int> indices;pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);return cloud;
}

3.2 可視化函數

????????顯示目標點云（紅色）和配準結果點云（綠色）。

void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud) {// ...（顏色設置、坐標系、相機參數等）while (!viewer_final->wasStopped()) {viewer_final->spinOnce(100);std::this_thread::sleep_for(std::chrono::milliseconds(100));}
}

3.3 主函數：數據加載與預處理

????????加載目標點云和待配準點云。對輸入點云進行體素濾波降采樣（減少計算量）。

auto target_cloud = read_cloud_point(argv[1]);
auto input_cloud = read_cloud_point(argv[2]);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
approximate_voxel_filter.filter(*filtered_cloud);

3.4 主函數：NDT配準核心配置

????????首先創建NDT配準對象并設置關鍵參數（包括收斂閾值0.01、優化步長0.05、網格分辨率0.5和最大迭代次數50），然后指定待配準的輸入點云（降采樣后的filtered_cloud）和目標點云（target_cloud）；接著通過初始旋轉（繞Z軸0.6931弧度）和平移（X=1.79387,Y=0.720047）構造初始位姿猜測init_guess，最后調用align方法執行配準，輸出配準后的點云output_cloud，其中配準過程會不斷優化變換矩陣直到滿足收斂條件或達到最大迭代次數。

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setTransformationEpsilon(0.01);? // 收斂閾值
ndt.setStepSize(0.05);????????????? // 優化步長
ndt.setResolution(0.5);???????????? // NDT網格分辨率
ndt.setMaximumIterations(50);?????? // 最大迭代次數
ndt.setInputSource(filtered_cloud); // 輸入點云（降采樣后）
ndt.setInputTarget(target_cloud);?? // 目標點云// 初始位姿猜測
Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();// 執行配準
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud, init_guess);

四、操作步驟

????????首先創建一個工作空間NDT_ws，并在該工作空間下創建src文件夾存放源碼：

 mkdir -p NDT_ws/src

????????然后再src文件夾下創建ndt_node.cpp文件，將代碼寫入改文件中，然后在NDT_ws目錄下編寫CMakeLists.txt文件：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(ndt_node)set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)find_package(PCL 1.5 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)? include_directories(${PCL_INCLUDE_DIRS}${Boost_INCLUDE_DIRS} 
)link_directories(${PCL_LIBRARY_DIRS})add_executable(ndt src/ndt_node.cpp)target_link_libraries(ndt${PCL_LIBRARIES}${Boost_LIBRARIES} 
)

????????在工作空間目錄下創建build文件夾，存放編譯過程文件，然后進行編譯：

mkdir build
cd build
Cmake ..
make

????????編譯完成后生成可執行文件ndt，并將之前處理過的pcd文件放到該目錄下：

????????隨后執行可執行文件即可：

????????輸出顯示，從文件1.pcd加載了51574個數據點，從文件2.pcd加載了51626個數據點，原始點云可能經過體素網格濾波（Voxel Grid Filter）或統計離群值濾波（Statistical Outlier Removal）等處理，最終保留了 22,837 個數據點（減少約 55%）。濾波的目的是去除冗余或噪聲數據，提升后續計算效率。

????????NDT配準，NDT converged ：1表示算法成功收斂，找到了最優變換矩陣。配準分數為56.2459。若是最終匹配的結果不準確，可調整NDT參數。

????????通過匹配結果可知，其匹配的效果并不好。隨后我也修改了NDT參數設置，但是最終的結果依舊不理想。然后我又利用ICP算法進行點云地圖構建，如下圖所示，為點云逐漸迭代的過程。其創建工作空間及操作過程與前面ICP操作相同。這里不在重復該過程，結果如下：

隨著不斷匹配新的點云，其點云范圍逐漸增大。

????????程序執行完之后，會在該目錄下生成配準后的pcd文件，可用pcl_viewer命令查看pcd文件。取出其中一個pcd文件查看，左邊為配準前的點云，右邊為匹配了一個pcd文件后的點云，還是存在一些區別。

pcl_viewer <pcd文件名>.pcd

參考博客：

無人駕駛汽車系統入門（十三）——正態分布變換（NDT）配準與無人車定位_settransformationepsilon-CSDN博客

PCL中點云配準（非常詳細，建議收藏）_pcl點云配準-CSDN博客

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/diannao/78796.shtml
繁體地址，請注明出處：http://hk.pswp.cn/diannao/78796.shtml
英文地址，請注明出處：http://en.pswp.cn/diannao/78796.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！