《PCL点云库学习&VS2010(X64)》Part 13 PCL1.72（VTK6.2.0）ICP示例

Part 13 PCL1.72（VTK6.2.0）ICP示例




1、ICP

cpp:

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registration (new pcl::PointCloud<pcl::PointXYZ>);

// check arguments
if(argc != 3) {
std::cout << "ERROR: the number of arguments is illegal!" << std::endl;
return -1;
}

// load pcd file
if(pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud_source)==-1) {
std::cout << "load source failed!" << std::endl;
return -1;
}
std::cout << "source loaded!" << std::endl;

if(pcl::io::loadPCDFile<pcl::PointXYZ>(argv[2], *cloud_target)==-1) {
std::cout << "load target failed!" << std::endl;
return -1;
}
std::cout << "target loaded!" << std::endl;

// ICP
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ>);
tree1->setInputCloud(cloud_source);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ>);
tree2->setInputCloud(cloud_target);
icp.setSearchMethodSource(tree1);
icp.setSearchMethodTarget(tree2);
icp.setInputSource(cloud_source);
icp.setInputTarget(cloud_target);
icp.setMaxCorrespondenceDistance(1500);
icp.setTransformationEpsilon(1e-10);
icp.setEuclideanFitnessEpsilon(0.1);
icp.setMaximumIterations(300);
icp.align(*cloud_source_registration);
Eigen::Matrix4f transformation = icp.getFinalTransformation();
std::cout << transformation << std::endl;

// display
pcl::visualization::PCLVisualizer viewer;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_r_h(cloud_source_registration, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h (cloud_target, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h (cloud_source, 0, 0, 255);
viewer.addPointCloud(cloud_source_registration, src_r_h,"source_r");
viewer.addPointCloud(cloud_target, tgt_h, "target");
viewer.addPointCloud(cloud_source, src_h, "source");
viewer.spin();
return 0;
}

控制台中运行，输入icp.exe rabbit.pcd rabbit_t.pcd，可以看到配准的效果。

从倒数第6-8行代码可以知道，配准后的数据颜色设置为红色，配准目标数据为绿色，配准的源数据为蓝色。