1.算法原理

已知有两幅点云，分别为源点云P和目标云Q。

1）将源点云P所在空间划分为一个一个的单元网格，（即三维空间在二维空间上的投影）。

2）根据所划分单元网格内点的分布情况，计算单元网格的正态分布PDF参数。

3）根据转移矩阵，将目标点云Q内的点进行变换。

4）统计源点云P所在空间划分网格内目标点云的个数，根据点的分布情况，对应的概率分布函数。

5）求解所有点的最优解，也即求目标云与源点云之间的刚体变换。分数通过output cloud和target cloud 对应的最近点欧式距离的平方得到，得分越小说明匹配效果越好。

2.程序代码

#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>
int
main (int argc, char** argv)
{//加载房间的第一次扫描pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1){PCL_ERROR ("Couldn't read file room_scan1.pcd \n");return (-1);}std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;//加载从新视角得到的房间的第二次扫描pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1){PCL_ERROR ("Couldn't read file room_scan2.pcd \n");return (-1);}std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;//将输入的扫描过滤到原始尺寸的大概 10% 以提高匹配的速度。pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::ApproximateVoxelGrid<pcl::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;//初始化正态分布变换（NDT）pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;//设置依赖尺度NDT参数//为终止条件设置最小转换差异ndt.setTransformationEpsilon (0.01);//为More-Thuente线搜索设置最大步长ndt.setStepSize (0.1);//设置NDT网格结构的分辨率（VoxelGridCovariance）ndt.setResolution (1.0);//设置匹配迭代的最大次数ndt.setMaximumIterations (50);// 设置要配准的点云ndt.setInputCloud (filtered_cloud);//设置点云配准目标ndt.setInputTarget (target_cloud);//设置使用机器人测距法得到的初始对准估计结果Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());Eigen::Translation3f init_translation (1.79387, 0, 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);std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()<< " score: " << ndt.getFitnessScore () << std::endl;//使用创建的变换对未过滤的输入点云进行变换pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());//保存转换的输入点云pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);// 初始化点云可视化界面boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));viewer_final->setBackgroundColor (0, 0, 0);//对目标点云着色（红色）并可视化pcl::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");//对转换后的目标点云着色（绿色）并可视化pcl::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");// 启动可视化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);
}

3.官网代码运行效果

4.切换其他数据

改变相关参数，得到的分数很小很小，可能是由于数据量小，点接触比较密集，因此得到的分数很小。但距离总是存在那一点点距离，导致匹配效果不理想。可能是我参数没有设置对，如果这一块有人做过了，麻烦各位大佬给我讲解一下。

NDT算法输出之后，我在这一块又加上了ICP算法进去，现在的配准效果还是可以的。

如果有大佬做过NDT算法配准bun.pcd这幅数据成功的，麻烦教我一下,本人将感激不尽。

点云配准之NDT算法相关推荐

点云配准的传统算法ICP与NDT概述
公众号致力于分享点云处理,SLAM,三维视觉,高精地图相关的文章与技术,欢迎各位加入我们,一起交流一起进步,有兴趣的可联系微信:920177957.本文来自点云PCL博主的分享,未经作者允许请勿转载, ...
干货 | 三维点云配准：ICP 算法原理及推导
编者荐语点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...
一文读懂自动驾驶中常用的定位算法之NDT点云配准算法
文中所用图片如无声明来源网络和论文. Paper: The Three-Dimensional Normal-Distributions Transform- an Efficient Represe ...
多视图点云配准算法综述
作者:杨佳琪,张世坤,范世超等转载自:华中科技大学学报(自然科学版) 编辑:东岸因为@一点人工一点智能原文:多视图点云配准算法综述摘要:以多视图点云配准为研究对象,对近二十余年的多视图点 ...
点云配准——（1）几种点云配准算法比较
几种点云配准算法比较参考许多博客,看了点云配准的好多算法,决定对这几天搞得点云配准做一个总结,主要也防止自己忘掉.主要参考下面这个博客,链接已经附上. https://blog.csdn.net/p ...
点云配准——经典配准算法及配准效果对比
目录点云配准基础知识什么是点云配准? 点云配准的步骤粗配准精配准点云配准的经典算法 ICP算法 NDT算法 3DSC算法 PFH FPFH 完全配准效果对比点云配准基础知识什么是点云配准 ...
三维计算机视觉（八）--点云配准
其实不想记录这一块内容,因为实在没啥新意,但又怕时间久了就忘记了.不贴代码,网上一堆. 参考:https://www.cnblogs.com/li-yao7758258/p/6489585.html ...
第七周PCL学习--点云配准（七）
目录引言一.点云配准 1.1.定义 1.2.含义 1.3.配准过程 1.4.算法原理 1.5.实验二.总结三.参考引言随着计算机辅助设计技术的发展,通过实物模型产生数字模型的逆向工程技术, ...
基于已知点云地图的NDT的激光SLAM定位
概述对于L4级自动驾驶系统而言,定位模块通常会融合GNSS.IMU.轮速计(车辆底盘).摄像头以及激光雷达odometry多种测量,使用滤波算法(EKF.UKF等)以获得平滑.厘米级别的绝对定位,其 ...

点云配准之NDT算法

1.算法原理

2.程序代码

3.官网代码运行效果

4.切换其他数据

点云配准之NDT算法相关推荐

最新文章

热门文章