1.代码
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimation<PointNT, PointNT, FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
//自定义颜色句柄
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;
int main(int argc,char **argv) {
PointCloudT::Ptr object(new PointCloudT);
PointCloudT::Ptr object_aligned(new PointCloudT);
PointCloudT::Ptr scene(new PointCloudT);
FeatureCloudT::Ptr object_features(new FeatureCloudT);
FeatureCloudT::Ptr scene_features(new FeatureCloudT);
// Get input object and scene
if (argc != 3)
{
pcl::console::print_error("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
//加载目标物体和场景点云
pcl::console::print_highlight("Loading point clouds...\n");
if (pcl::io::loadPCDFile<PointNT>(argv[1], *object) < 0 ||
pcl::io::loadPCDFile<PointNT>(argv[2], *scene) < 0)
{
pcl::console::print_error("Error loading object/scene file!\n");
return (1);
}
//下采样
pcl::console::print_highlight("Downsample...\n");
pcl::VoxelGrid<PointNT>grid;
const float leaf = 0.005f;//下采样使用的空间分辨率
grid.setLeafSize(leaf,leaf,leaf);
grid.setInputCloud(object);
grid.filter(*object);
grid.setInputCloud(scene);
grid.filter(*scene);
//估计场景法线
pcl::console::print_highlight("Estimating scene normals...\n");
pcl::NormalEstimation<PointNT, PointNT>nest;
nest.setRadiusSearch(0.01);
nest.setInputCloud(scene);
nest.compute(*scene);
//特征估计
pcl::console::print_highlight("Estimating features...\n");
FeatureEstimationT fest;
fest.setRadiusSearch(0.025);
fest.setInputCloud(object);
fest.setInputNormals(object);
fest.compute(*object_features);
fest.setInputCloud(scene);
fest.setInputNormals(scene);
fest.compute(*scene_features);
//实施配准
pcl::console::print_highlight("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT, PointNT, FeatureT> align;
align.setInputSource(object);
align.setSourceFeatures(object_features);
align.setInputTarget(scene);
align.setTargetFeatures(scene_features);
align.setMaximumIterations(50000);//采样一致性迭代次数
align.setNumberOfSamples(3);//创建假设所需的样本数
align.setCorrespondenceRandomness(5);//使用的临近特征点的数目
align.setSimilarityThreshold(0.9f);//多边形边长度相似度阈值
align.setMaxCorrespondenceDistance(2.5f*0.005);//判断是否为内点的距离阈值
align.setInlierFraction(0.25f);//接受位姿假设所需的内点比例
{
pcl::ScopeTime t("Alignment");
align.align(*object_aligned);//配准后的点云数据存储在object_aligned
}
if (align.hasConverged())
{
//打印结果
printf("\n");
Eigen::Matrix4f transformation = align.getFinalTransformation();
pcl::console::print_info(" | %6.3f %6.3f %6.3f | \n", transformation(0, 0), transformation(0, 1), transformation(0, 2));
pcl::console::print_info("R = | %6.3f %6.3f %6.3f | \n", transformation(1, 0), transformation(1, 1), transformation(1, 2));
pcl::console::print_info(" | %6.3f %6.3f %6.3f | \n", transformation(2, 0), transformation(2, 1), transformation(2, 2));
pcl::console::print_info("\n");
pcl::console::print_info("t = < %0.3f, %0.3f, %0.3f >\n", transformation(0, 3), transformation(1, 3), transformation(2, 3));
pcl::console::print_info("\n");
pcl::console::print_info("Inliers: %i/%i\n", align.getInliers().size(), object->size());;
//show alignment
pcl::visualization::PCLVisualizer visu("PCL鲁棒位姿估计");
int v1(0), v2(0);
visu.createViewPort(0,0,0.5,1,v1);
visu.createViewPort(0.5, 0, 1, 1, v2);
visu.setBackgroundColor(0,0,0,v1);
visu.addPointCloud(scene, ColorHandlerT(scene, 0.0, 255.0, 0.0), "scene", v1);
visu.addPointCloud(object_aligned, ColorHandlerT(object_aligned, 0.0, 0.0, 255.0), "object_aligned", v1);
visu.addPointCloud(object, ColorHandlerT(object, 0.0, 255.0, 0.0), "object_before_aligned", v2);
visu.addPointCloud(scene, ColorHandlerT(scene, 0.0, 0.0, 255.0), "scene_v2", v2);
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scene");
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "object_aligned");
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "object_before_aligned");
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scene_v2");
visu.spin();
}
else
{
pcl::console::print_error("Alignment failed!\n");
return (1);
}
return 0;
}
2.显示
左边是配准,右边是原始。