点云配准PFH、FPFH、icp、NDT、3Dsc.zip

几个点云配准的算法：点云配准PFH、FPFH、icp、NDT、3Dsc：粗配准算法，并计算出误差
几个点云配准的算法：点云配准PFH、FPFH、icp、NDT、3Dsc：粗配准算法，并计算出误差

#include //采样一致性
#include //采取固定数量的点云
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include //
#include //
#include //icp配准
#include //可视化
#include //时间

using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;

//点云可视化
void visualize_pcd（PointCloud::Ptr pcd_src PointCloud::Ptr pcd_tgt PointCloud::Ptr pcd_final）
{

	//创建初始化目标
	pcl::visualization::PCLVisualizer viewer（“registration Viewer“）;


	pcl::visualization::PointCloudColorHandlerCustom src_h（pcd_src 0 255 0）;
	pcl::visualization::PointCloudColorHandlerCustom tgt_h（pcd_tgt 255 0 0）;
	pcl::visualization::PointCloudColorHandlerCustom final_h（pcd_final 0 0 255）;
	viewer.setBackgroundColor（255 255 255）;
	viewer.addPointCloud（pcd_src src_h “source cloud“）;
	viewer.addPointCloud（pcd_tgt tgt_h “tgt cloud“）;
	viewer.addPointCloud（pcd_final final_h “final cloud“）;

	while （!viewer.wasStopped（））
	{
		viewer.spinOnce（100）;
		boost::this_thread::sleep（boost::posix_time::microseconds（100000））;
	}
}
//由旋转平移矩阵计算旋转角度
void matrix2angle（Eigen::Matrix4f &result_trans Eigen::Vector3f &result_angle）
{
	double ax ay az;
	if （result_trans（2 0） == 1 || result_trans（2 0） == -1）
	{
		az = 0;
		double dlta;
		dlta = atan2（result_trans（0 1） result_trans（0 2））;
		if （result_trans（2 0） == -1）
		{
			ay = M_PI / 2;
			ax = az + dlta;
		}
		else
		{
			ay = -M_PI / 2;
			ax = -az + dlta;
		}
	}
	else
	{
		ay = -asin（result_trans（2 0））;
		ax = atan2（result_trans（2 1） / cos（ay） result_trans（2 2） / cos（ay））;
		az = atan2（result_trans（1 0） / cos（ay） result_trans（0 0） / cos（ay））;
	}
	result_angle << ax ay az;

	cout << “x轴旋转角度：“ << ax << endl;
	cout << “y轴旋转角度：“ << ay << endl;
	cout << “z轴旋转角度：“ << az << endl;
}


int main（int argc char** argv）
{
	//加载点云文件
	PointCloud::Ptr cloud_src_o（new PointCloud）;//原点云，待配准
	pcl::io::loadPCDFile（“E:/vs13/pcldata/bun/rabbit.pcd“ *cloud_src_o）;
	PointCloud::Ptr cloud_tgt_o（new PointCloud）;//目标点云
	pcl::io::loadPCDFile（“E:/vs13/pcldata/bun/rabbit_1.pcd“ *cloud_tgt_o）;

	clock_t start = clock（）;

	//去除NAN点
	std::vector indices_src; //保存去除的点的索引
	pcl::removeNaNFromPointCloud（*cloud_src_o *cloud_src_o indices_src）;
	std::cout << “remove *cloud_src_o nan“ << endl;

	std::vector indices_tgt;
	pcl::removeNaNFromPointCloud（*cloud_tgt_o *cloud_tgt_o indices_tgt）;
	std::cout << “remove *cloud_tgt_o nan“ << endl;


	//采样固定的点云数量

	p

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        8921  2019-03-04 20:18  点云配准PFH、FPFH、icp、NDT、3Dsc\3Dsc+icp.cpp
     文件        8813  2019-03-04 20:04  点云配准PFH、FPFH、icp、NDT、3Dsc\fpfh+icp.cpp
     文件        7466  2019-03-04 20:19  点云配准PFH、FPFH、icp、NDT、3Dsc\ndt+icp.cpp
     文件        8998  2019-03-04 20:26  点云配准PFH、FPFH、icp、NDT、3Dsc\pfh+icp.cpp
     文件           0  2020-02-24 21:14  点云配准PFH、FPFH、icp、NDT、3Dsc\写了几个点云配准的算法，主要包括PFH、FPFH、icp、NDT、3Dsc几种粗配准算法，并计算出误差。.txt
     目录           0  2020-02-24 21:14  点云配准PFH、FPFH、icp、NDT、3Dsc\