算法原理
代码实现
#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>
#include <pcl/console/time.h>
using namespace std;
int
main(int argc, char** argv)
{
pcl::console::TicToc time;
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("G.pcd", *target_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("G1.pcd", *input_cloud);
cout << "从目标点云中读取 " << target_cloud->size() << " 个点" << endl;
cout << "从源点云中读取 " << input_cloud->size() << " 个点" << endl;
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);
cout << "Filtered cloud contains " << filtered_cloud->size()
<< " data points " << endl;
time.tic();
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setTransformationEpsilon(0.01);
ndt.setStepSize(0.1);
ndt.setResolution(1.0);
ndt.setMaximumIterations(35);
ndt.setInputCloud(filtered_cloud);
ndt.setInputTarget(target_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud);
cout << "NDT has converged:" << ndt.hasConverged()
<< " score: " << ndt.getFitnessScore() << endl;
cout << "Applied " << 35 << " NDT iterations in " << time.toc() << " ms" << endl;
cout << "变换矩阵:\n" << ndt.getFinalTransformation() << endl;
pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
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>
source_color(input_cloud, 255, 0, 255);
viewer_final->addPointCloud<pcl::PointXYZ>(input_cloud, source_color, "source cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "source cloud");
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");
while (!viewer_final->wasStopped())
{
viewer_final->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return (0);
}
结果展示