逐步匹配多幅点云-pairwise_incremental_registration
1.使用迭代最近点算法,逐步对一系列点云进行两两匹配。他的思想是对所有的点云进行变换,使得都与第一个点云在统一坐标系中。在每个连贯的有重叠的点云之间找到最佳的变换,并积累这些变换到的全部点云。能够进行ICP算法的点云需要粗略的预匹配,并且一个点云与另一个点云需要有重叠部分。
2.代码
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
using namespace std;
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//简单类型定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
//创建可视化工具
pcl::visualization::PCLVisualizer *p;
//定义左右视点
int vp_1, vp_2;
//处理点云的方便的结构定义:在配准过程中可以同时接收多个点云文件输入,程序从第一个文件开始,连续两两配准
struct PCD
{
PointCloud::Ptr cloud;//点云共享指针
std::string f_name;//文件名称
PCD() : cloud(new PointCloud) {
};
};
struct PCDComparator//文件比较处理
{
bool operator () (const PCD& p1, const PCD& p2)
{
return (p1.f_name < p2.f_name);
}
};
//以< x, y, z, curvature >形式定义一个新的点
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
MyPointRepresentation()
{
//定义点的维度
nr_dimensions_ = 4;
}
//重载copyToFloatArray方法将点转化为4维数组
virtual void copyToFloatArray(const PointNormalT &p, float * out) const
{
// < x, y, z, curvature >
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
out[3] = p.curvature;
}
};
//在可视化窗口的第一视点显示未匹配源点云和目标点云
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
p->removePointCloud("vp1_target");
p->removePointCloud("vp1_source");
PointCloudColorHandlerCustom<PointT> tgt_h(cloud_target, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> src_h(cloud_source, 255, 0, 0);
p->addPointCloud(cloud_target