点云匹配方法 NDT（正态分布变换）

1. 正态分布变换 (NDT)在点云匹配中ICP基于距离直接最优化变换矩阵的参数由于是欠定方程且旋转矩阵的约束使得结果很难优化为此在新的维度优化变换矩阵的参数被很好的提出先将参考点云目标点云转换为多维变量的正态分布匹配的点云如果采用某组变换参数后使得新的点云和目标点云的正态分布参数匹配很好那么变换点在参考系中的概率密度将会很大。因此可以考虑用优化的方法求出使得概率密度之和最大的变换参数此时两幅激光点云数据将匹配的最好。2. 多元正态分布我们知道如果随机变量X满足正态分布则其概率密度函数为对于多元正态分布而言其概率密度函数可以表示为2.1. NDT算法流程将参考点云网格化计算每个网格的概率密度函数网格的概率密度函数则为2.2. 变换参数和最大似然我们需要优化的参数就是对当前点云的坐标变换旋转平移等转换函数表示使用姿态变换来变换结合之前的一组状态密度函数那么最好的变换参数应该是最大化似然函数的姿态变换那么最大化似然也就相当于最小化负对数似然就到了我们最熟悉的最优化的部分了。现在的任务就是使用优化算法来调整变换参数来最小化这个负对数似然。NDT算法使用牛顿法进行参数优化。3.算法流程输入两帧点云数据,输出位姿变换参数 R, ta. 对计算正太分布变换 (NDT)b. 初始化需要估计的参数 R, t设置为0或者采用里程计数据c. 根据参数 R, t 将中的所有样本点投影到对应的坐标系下d. 确定每一个映射点对应的正态分布即映射点属于中的哪一个celle. 通过评估每个映射点的分布并对结果求和来确定参数的得分 scoref. 通过尝试优化分数来计算新的参数估计。这是通过执行牛顿算法非线性最小二乘中一种方法的一个步骤来实现的g. 转到c直到满足收敛条件。4. 代码#include iostream #include thread #include pcl/io/pcd_io.h #include pcl/point_types.h #include pcl/registration/ndt.h #include pcl/filters/approximate_voxel_grid.h #include pcl/visualization/pcl_visualizer.h using namespace std::chrono_literals; int main () { // Loading first scan of room. pcl::PointCloudpcl::PointXYZ::Ptr target_cloud (new pcl::PointCloudpcl::PointXYZ); if (pcl::io::loadPCDFilepcl::PointXYZ (room_scan1.pcd, *target_cloud) -1) { PCL_ERROR (Couldnt read file room_scan1.pcd \n); return (-1); } std::cout Loaded target_cloud-size () data points from room_scan1.pcd std::endl; // Loading second scan of room from new perspective. pcl::PointCloudpcl::PointXYZ::Ptr input_cloud (new pcl::PointCloudpcl::PointXYZ); if (pcl::io::loadPCDFilepcl::PointXYZ (room_scan2.pcd, *input_cloud) -1) { PCL_ERROR (Couldnt read file room_scan2.pcd \n); return (-1); } std::cout Loaded input_cloud-size () data points from room_scan2.pcd std::endl; // Filtering input scan to roughly 10% of original size to increase speed of registration. pcl::PointCloudpcl::PointXYZ::Ptr filtered_cloud (new pcl::PointCloudpcl::PointXYZ); pcl::ApproximateVoxelGridpcl::PointXYZ approximate_voxel_filter; approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2); approximate_voxel_filter.setInputCloud (input_cloud); approximate_voxel_filter.filter (*filtered_cloud); std::cout Filtered cloud contains filtered_cloud-size () data points from room_scan2.pcd std::endl; // Initializing Normal Distributions Transform (NDT). pcl::NormalDistributionsTransformpcl::PointXYZ, pcl::PointXYZ ndt; // Setting scale dependent NDT parameters // Setting minimum transformation difference for termination condition. ndt.setTransformationEpsilon (0.01); // Setting maximum step size for More-Thuente line search. ndt.setStepSize (0.1); //Setting Resolution of NDT grid structure (VoxelGridCovariance). ndt.setResolution (1.0); // Setting max number of registration iterations. ndt.setMaximumIterations (35); // Setting point cloud to be aligned. ndt.setInputSource (filtered_cloud); // Setting point cloud to be aligned to. ndt.setInputTarget (target_cloud); // Set initial alignment estimate found using robot odometry. 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 (); // Calculating required rigid transform to align the input cloud to the target cloud. pcl::PointCloudpcl::PointXYZ::Ptr output_cloud (new pcl::PointCloudpcl::PointXYZ); ndt.align (*output_cloud, init_guess); std::cout Normal Distributions Transform has converged: ndt.hasConverged () score: ndt.getFitnessScore () std::endl; // Transforming unfiltered, input cloud using found transform. pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ()); // Saving transformed input cloud. pcl::io::savePCDFileASCII (room_scan2_transformed.pcd, *output_cloud); // Initializing point cloud visualizer pcl::visualization::PCLVisualizer::Ptr viewer_final (new pcl::visualization::PCLVisualizer (3D Viewer)); viewer_final-setBackgroundColor (0, 0, 0); // Coloring and visualizing target cloud (red). pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ target_color (target_cloud, 255, 0, 0); viewer_final-addPointCloudpcl::PointXYZ (target_cloud, target_color, target cloud); viewer_final-setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, target cloud); // Coloring and visualizing transformed input cloud (green). pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ output_color (output_cloud, 0, 255, 0); viewer_final-addPointCloudpcl::PointXYZ (output_cloud, output_color, output cloud); viewer_final-setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, output cloud); // Starting visualizer viewer_final-addCoordinateSystem (1.0, global); viewer_final-initCameraParameters (); // Wait until visualizer window is closed. while (!viewer_final-wasStopped ()) { viewer_final-spinOnce (100); std::this_thread::sleep_for(100ms); } return (0); }参考文献自动驾驶十---------正态分布变换NDT_一实相印的博客-CSDN博客NDT点云配准_有了个相册的博客-CSDN博客_ndt点云配准3d激光雷达开发ndt匹配_嵌入式-老费的博客-CSDN博客_ndt匹配