配准结果

红色为目标点云，天蓝色为待配准点云，绿色是配准后点云，效果较好。

点云配准系列

点云配准1：配准基础及icp算法
点云配准2：icp算法在PCL1.10.0上的实现+源码解析
点云配准3：3d-ndt算法在pcl上的实现以及参数设置
点云配准4：cloudcompare的使用以及点云配准功能
点云配准5：4pcs算法在pcl上的实现
点云配准6：tricp算法在pcl上的实现
点云配准论文阅读笔记–Efficient Variants of the ICP Algorithm
点云配准论文阅读笔记–Comparing ICP variants on real-world data sets
点云配准论文阅读笔记–(4PCS)4-Points Congruent Sets for Robust Pairwise Surface Registration
点云配准论文阅读笔记–3d-dnt博士论文

准备

本实例是在visual studio2019+pcl1.10.0上进行，环境配置方法在之前的博客已进行详细说明：
vs2019配置pcl1.10.0+点云可视化示例

完整项目文件

免费下载地址：share_noel/PCL/ndt/3dndt_registration_carlos202012.zip
https://blog.csdn.net/qq_41102371/article/details/125646840
愿意用c币支持的朋友在这里下载：3dndt_registration_carlos202012.zip

参数设定及说明

数据

配准点云来自斯坦福的兔子点云，本文使用其中0度及45度扫描点云，但经过放大处理，xyz都放大了100倍。原因是原点云太小，导致阈值不好控制。比如对于原点云来说，点间距离本来就特别小，即使是非正确对应点。
本文将45度点云配准至0度点云。

参数

之前使用ndt算法配准，要么就配准失败，要么就出来的是单位矩阵，发现对不同的点云要设置不同的参数，不然没法配准，所以笔者觉得这也是3d-ndt不好的地方。
经过多次实验，得出ndt几个重要参数设置的经验：

1、两点云差不多大小

体素降采样网格大小

voxel_grid.setLeafSize(0.5, 0.5, 0.5);

待配准点云的xyz中跨度最大的维度长度除以30，比如本次使用的45度点云的三个维度（用cloudcompare可以看到点云的信息，见点云配准四：cloudcompare的使用以及点云配准功能）：

选最大的除以30直接得到格子大小为0.5，若降采样的点太少，比如都小于500了，考虑减小格子大小以增加降采样点云，不然点太少会配准失败或精度太差。

步长setStepSize

ndt.setStepSize(0.5);

步长设置为与降采样网格相同大小

网格分辨率Resolution

ndt.setResolution(1.0);

Resolution设置为步长的两倍
若配准结果不正确，增大步长，网格分辨率仍为步长的两倍
若配准结果矩阵为单位矩阵，同样地增大步长与网格分辨率

2、待配准点云体积明显小于目标点云

体素降采样网格大小与上相同，步长设置为目标点云xyz中跨度最大的维度长度除以30，网格分辨率仍为步长的两倍。

结果是单位矩阵

网格分辨率太小，加大

上述方法测试了3种不同的点云，均成功配准
（个人经验，仅供参考，因为若觉得本文理解有误或者有更好的确定参数的方法感谢大家不吝赐教）

代码

#include <iostream>
#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>
#include <boost/thread/thread.hpp>typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;int
main(int argc, char** argv)
{//加载目标点云pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);std::string filename = "point_source/bun000_s100.pcd";//0度扫描的兔子if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *target_cloud) == -1){PCL_ERROR("Error loading cloud %s.\n", filename);return (-1);}std::cout << "Loaded " << target_cloud->size() << " data points from "<< filename << std::endl;//显示点云的数量//加载源点云（待配准点云）pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);filename = "point_source/bun045_s100.pcd";if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *input_cloud) == -1){PCL_ERROR("Error loading cloud %s.\n", filename);return (-1);}std::cout << "Loaded " << input_cloud->size() << " data points from " << filename << std::endl;//显示点云的数量pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);//定义降采样后点云*filtered_cloud = *input_cloud;//体素降采样pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;voxel_grid.setLeafSize(0.5, 0.5, 0.5);//bun0.5voxel_grid.setInputCloud(input_cloud);voxel_grid.filter(*filtered_cloud);std::cout << "down size input_cloud to" << filtered_cloud->size() << endl;//显示降采样后的点云数量//pcl::io::savePCDFileBinary("point_source/bun045_voxel_0.5.pcd", *filtered_cloud);//储存降采样点clock_t start = clock();//算法计时//初始化正态分布变换（NDT）pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;//设置依赖尺度NDT参数//为终止条件设置最小转换差异ndt.setTransformationEpsilon(0.001);//为More-Thuente线搜索设置最大步长ndt.setStepSize(0.5);//设置NDT网格结构的分辨率（VoxelGridCovariance）ndt.setResolution(1.0);//设置匹配迭代的最大次数ndt.setMaximumIterations(200);// 设置要配准的点云ndt.setInputCloud(filtered_cloud);//设置点云配准目标ndt.setInputTarget(target_cloud);//计算需要的刚体变换以便将输入的点云匹配到目标点云pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);ndt.align(*output_cloud);//配准，将配准后的点云放至output_cloudclock_t end1 = clock();cout << "ndt time" << (double)(end1 - start) / CLOCKS_PER_SEC << endl;//打印配准时间std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()<< " score: " << ndt.getFitnessScore() << std::endl;cout << "ndt matrix:" << endl << ndt.getFinalTransformation() << endl;//打印变换矩阵// 初始化点云可视化界面boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer_final->setBackgroundColor(0, 0, 0);//背景颜色黑//添加目标点云到viewerpcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);//红色viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");//添加源点云到viewerpcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_color(input_cloud, 0, 255, 255);//天蓝色viewer_final->addPointCloud<pcl::PointXYZ>(input_cloud, input_color, "input cloud");viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud");//使用求得的变换的矩阵对源点云进行变换，结果放至output_cloudpcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());//添加配准后的点云到viewerpcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>  output_color(output_cloud, 0, 255, 0);//绿色viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");//pcl::io::savePCDFileBinary("point_source/bun045to000_ndt.pcd", *output_cloud);//储存降采样点// 启动可视化viewer_final->addCoordinateSystem(1.0);viewer_final->initCameraParameters();//等待直到可视化窗口关闭。while (!viewer_final->wasStopped()){viewer_final->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return (0);
}

结果

红色为目标点云，天蓝色为待配准点云，绿色是配准后点云。
Bunny


hippo

参考及感谢

paper:
Magnusson, Martin. (2009). The Three-Dimensional Normal-Distributions Transform — an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
blog:
文中已列出

完

边学边用，如有错漏，敬请指正
圣诞节快乐-^ _ ^-
考研的朋友们加油！！！
--------------------------------------------------------------------------------------------诺有缸的高飞鸟20201225