pcl 代码——实验一

学习出处:

1、基于Kinect v2+PCL的模型奶牛重建(上)——数据获取_3d 重建奶牛-优快云博客

#define vtkRenderingCore_AUTOINIT 4(vtkInteractionStyle,vtkRenderingFreeType,vtkRenderingFreeType,vtkRenderingOpenGL) 
#define vtkRenderingVolume_AUTOINIT 1(vtkRenderingVolumeOpenGL)
#include <Windows.h>
#include <iostream>
#include <kinect.h>
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件
#include <pcl/point_types.h> //PCL对各种格式的点的支持头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/radius_outlier_removal.h>  
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/icp.h> //ICP(iterative closest point)配准
#include <pcl/console/parse.h>  //pcl控制台解析
//kd树
#include <pcl/kdtree/kdtree_flann.h>
//特征提取
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/normal_3d.h>
//重构
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>

#include <boost/thread/thread.hpp>
#include <string.h>
using namespace cv;
using namespace std;

typedef pcl::PointXYZ  MyPointDataType;

// 安全释放指针
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
	if (pInterfaceToRelease != NULL)
	{
		pInterfaceToRelease->Release();
		pInterfaceToRelease = NULL;
	}
}

string num2str(int i)
{
	stringstream ss;
	ss << i;
	return ss.str();
}
const string targetFolderPath = "C:\\A0fangfang\\shiyan\\8.19\\";
int main()
{
	// 获取Kinect设备
	IKinectSensor* m_pKinectSensor;
	HRESULT hr;
	hr = GetDefaultKinectSensor(&m_pKinectSensor);
	if (FAILED(hr))
	{
		return hr;
	}

	IMultiSourceFrameReader* m_pMultiFrameReader;
	if (m_pKinectSensor)
	{
		hr = m_pKinectSensor->Open();
		if (SUCCEEDED(hr))
		{
			// 获取多数据源到读取器  
			hr = m_pKinectSensor->OpenMultiSourceFrameReader(
				//FrameSourceTypes::FrameSourceTypes_Color |
				//FrameSourceTypes::FrameSourceTypes_Infrared |
				FrameSourceTypes::FrameSourceTypes_Depth,
				&m_pMultiFrameReader);
		}
	}

	if (!m_pKinectSensor || FAILED(hr))
	{
		return E_FAIL;
	}

	UINT16 *depthData = new UINT16[424 * 512];//用于存储深度图数据
	Mat i_rgb(1080, 1920, CV_8UC4);
	Mat i_depthWrite(424, 512, CV_16UC1);
	UINT nColorBufferSize = 1920 * 1080 * 4;

	// 三个数据帧及引用
	IDepthFrameReference* m_pDepthFrameReference = nullptr;
	IColorFrameReference* m_pColorFrameReference = nullptr;
	IDepthFrame* m_pDepthFrame = nullptr;
	IColorFrame* m_pColorFrame = nullptr;
	IMultiSourceFrame* m_pMultiFrame = nullptr;

	ICoordinateMapper* m_pCoordinateMapper = nullptr;


	int count = 0;
	while (count <= 30)
	{

		Sleep(5000);
		while (true)
		{
			// 获取新的一个多源数据帧
			hr = m_pMultiFrameReader->AcquireLatestFrame(&m_pMultiFrame);
			if (FAILED(hr) || !m_pMultiFrame)
			{
				continue;
			}
			break;
		}

		// 从多源数据帧中分离出彩色数据,深度数据
		if (SUCCEEDED(hr))
			hr = m_pMultiFrame->get_DepthFrameReference(&m_pDepthFrameReference);
		if (SUCCEEDED(hr))
			hr = m_pDepthFrameReference->AcquireFrame(&m_pDepthFrame);
		//if (SUCCEEDED(hr))
		//	hr = m_pMultiFrame->get_ColorFrameReference(&m_pColorFrameReference);
		//if (SUCCEEDED(hr))
		//	hr = m_pColorFrameReference->AcquireFrame(&m_pColorFrame);

		hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);

		//if (SUCCEEDED(hr))
		//	hr = m_pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, i_rgb.data, ColorImageFormat::ColorImageFormat_Bgra);

		// 定义相关变量
		pcl::PointCloud<MyPointDataType>::Ptr cloud(new pcl::PointCloud<MyPointDataType>);
		pcl::PointCloud<MyPointDataType>::Ptr cloud_filtered(new pcl::PointCloud<MyPointDataType>);
		//初始化点云数据PCD文件头
		cloud->width = 512 * 424;
		cloud->height = 1;
		cloud->is_dense = false;
		cloud->points.resize(cloud->width * cloud->height);

		if (SUCCEEDED(hr))
		{
			hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, depthData);
			/*hr = m_pDepthFrame->CopyFrameDataToArray(424 * 512, reinterpret_cast<UINT16*>(i_depthWrite.data));
			imwrite("depth_" + num2str(count) + ".png", i_depthWrite);*/
			CameraSpacePoint* m_pCameraCoordinates = new CameraSpacePoint[512 * 424];
			hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(512 * 424, depthData, 512 * 424, m_pCameraCoordinates);
			//ColorSpacePoint* m_pColorCoordinates = new ColorSpacePoint[512 * 424];
			//hr = m_pCoordinateMapper->MapDepthFrameToColorSpace(512 * 424, depthData, 512 * 424, m_pColorCoordinates);
			for (int i = 0; i < 512 * 424; i++)
			{
				//------写入RGB------
				/*ColorSpacePoint colorP = m_pColorCoordinates[i];
				if (colorP.X != -std::numeric_limits<float>::infinity() && colorP.Y != -std::numeric_limits<float>::infinity())
				{
				int colorX = static_cast<int>(colorP.X + 0.5f);
				int colorY = static_cast<int>(colorP.Y + 0.5f);
				if ((colorX >= 0 && colorX < 1920) && (colorY >= 0 && colorY < 1080))
				{
				cloud->points[i].b = i_rgb.data[(colorY * 1920 + colorX) * 4];
				cloud->points[i].g = i_rgb.data[(colorY * 1920 + colorX) * 4 + 1];
				cloud->points[i].r = i_rgb.data[(colorY * 1920 + colorX) * 4 + 2];
				}
				}*/

				//------写入XYZ------
				CameraSpacePoint cameraP = m_pCameraCoordinates[i];
				if (cameraP.X != -std::numeric_limits<float>::infinity() && cameraP.Y != -std::numeric_limits<float>::infinity() && cameraP.Z != -std::numeric_limits<float>::infinity())
				{
					float cameraX = static_cast<float>(cameraP.X);
					float cameraY = static_cast<float>(cameraP.Y);
					float cameraZ = static_cast<float>(cameraP.Z);

					cloud->points[i].x = cameraX;
					cloud->points[i].y = cameraY;
					cloud->points[i].z = cameraZ;

				}
			}
		}
		//-----------------------提取范围内的点------------------------
		pcl::ConditionAnd<MyPointDataType>::Ptr range_cond(new pcl::ConditionAnd<MyPointDataType>());
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("z", pcl::ComparisonOps::GT, 0.001)));
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("z", pcl::ComparisonOps::LT, 2.0)));
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("x", pcl::ComparisonOps::GT, -0.5)));
		range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("x", pcl::ComparisonOps::LT, 0.5)));
		//range_cond->addComparison(pcl::FieldComparison<MyPointDataType>::ConstPtr(new	pcl::FieldComparison<MyPointDataType>("y", pcl::ComparisonOps::GT, -0.85)));
		pcl::ConditionalRemoval<MyPointDataType> condrem(range_cond, false);
		condrem.setInputCloud(cloud);
		condrem.setKeepOrganized(false);
		condrem.filter(*cloud_filtered);
		//--------------------------------------------------------------

		//-----------------------去除离群点------------------------
		//pcl::RadiusOutlierRemoval<MyPointDataType> outrem;
		//outrem.setInputCloud(cloud_filtered);
		//outrem.setRadiusSearch(0.03);
		//outrem.setMinNeighborsInRadius(15);
		//outrem.filter(*cloud_filtered);

		//pcl::StatisticalOutlierRemoval<MyPointDataType> sor;
		//sor.setInputCloud(cloud_filtered);
		//sor.setMeanK(10);
		//sor.setStddevMulThresh(1.0);
		//sor.filter(*cloud_filtered);
		//--------------------------------------------------------------

		string s = targetFolderPath + "cow"; // 在文件名前加上目标文件夹路径
		s += num2str(count);
		s += ".pcd";
		pcl::io::savePCDFile(s, *cloud_filtered, false); // 将点云保存到PCD文件中//将点云保存到PCD文件中
		std::cerr << "Saved " << cloud_filtered->points.size() << " data points." << std::endl;
		s.clear();

		//Beep(1046, 1000);

		// 显示结果图
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
		viewer->addPointCloud(cloud_filtered);
		viewer->resetCamera();

		viewer->addCoordinateSystem(0.1);
		viewer->initCameraParameters();
		while (!viewer->wasStopped()) {
			viewer->spinOnce();
		}

		count++;
		cout << "test" << endl;

		// 释放资源
		SafeRelease(m_pDepthFrame);
		SafeRelease(m_pDepthFrameReference);
		SafeRelease(m_pColorFrame);
		SafeRelease(m_pColorFrameReference);
		SafeRelease(m_pMultiFrame);
	}


	// 关闭窗口,设备
	m_pKinectSensor->Close();
	SafeRelease(m_pKinectSensor);
	std::system("pause");
	return 0;
}

2、基于Kinect v2+PCL的模型奶牛重建(中)——地面去除与法向量计算_pcl计算地面法向量-优快云博客

#define vtkRenderingCore_AUTOINIT 4(vtkInteractionStyle,vtkRenderingFreeType,vtkRenderingFreeType,vtkRenderingOpenGL) 
#define vtkRenderingVolume_AUTOINIT 1(vtkRenderingVolumeOpenGL)
#include <string>
#include <kinect.h>
#include <iostream>
#include <fstream>  
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL对各种格式的点的支持头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/radius_outlier_removal.h>  
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/icp.h> //ICP(iterative closest point)配准
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/console/parse.h>  //pcl控制台解析
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>  
//kd树
#include <pcl/kdtree/kdtree_flann.h>
//特征提取
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/normal_3d.h>
//重构
#include <boost/thread/thread.hpp>
#include <Eigen/Dense>

using namespace std;

string num2str(int i)
{
	stringstream ss;
	ss << i;
	return ss.str();
}

//定义结构体,用于处理点云
struct PCD
{
	std::string f_name; //文件名
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; //点云指针
	pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormal;//存储估计的法线的指针
														   //构造函数
	PCD() : cloud(new pcl::PointCloud<pcl::PointXYZ>), cloudWithNormal(new pcl::PointCloud<pcl::PointNormal>) {}; //初始化
};

int main()
{
	const int numberOfViews = 8;//点云数量
	std::vector<PCD, Eigen::aligned_allocator<PCD> > data; //点云数据
	std::string prefix("cow");
	std::string extension(".pcd"); //声明并初始化string类型变量extension,表示文件后缀名
	std::string folderPath = "C:\\A0fangfang\\shiyan\\8.19\\"; // 指定工作目录路径

															   // 通过遍历文件名,读取pcd文件
	for (int i = 0; i < numberOfViews; i++) //遍历所有的文件名
	{
		std::string fname = folderPath + prefix + num2str(i) + extension;
		// 读取点云,并保存到models
		PCD m;
		m.f_name = fname;
		if (pcl::io::loadPCDFile(fname, *m.cloud) == -1) //* 读入PCD格式的文件,如果文件不存在,返回-1
		{
			cout << "Couldn't read file " + fname + "." << endl; //文件不存在时,返回错误,终止程序。
			return (-1);
		}
		data.push_back(m);
	}

	//去除离群点
	for (int i = 0; i <= 1; ++i) {
		pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
		outrem.setInputCloud(data[i].cloud);
		outrem.setRadiusSearch(0.01);
		outrem.setMinNeighborsInRadius(10);
		outrem.filter(*data[i].cloud);
	}

	float yMax1 = -1000;
	for (int i = 0; i < data[0].cloud->size(); ++i)
	{
		if (data[0].cloud->points[i].y>yMax1)
			yMax1 = data[0].cloud->points[i].y;
	}
	cout << "yMax1:" << yMax1 << endl;

	float yMax2 = -1000;
	for (int i = 0; i < data[1].cloud->size(); ++i)
	{
		if (data[1].cloud->points[i].y>yMax2)
			yMax2 = data[1].cloud->points[i].y;
	}
	cout << "yMax2:" << yMax2 << endl;

	ofstream out("yMax.txt");
	if (out.is_open())
	{
		out << "yMax1:" << yMax1 << endl;
		out << "yMax2:" << yMax2 << endl;
		out.close();
	}

	for (int i = 2; i < numberOfViews; ++i)
	{
		//-----------------------去除地面点云--------------------------
		pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());
		/*range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new	pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GT, -0.4)));
		range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new	pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, 0.4)));
		range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new	pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 1.3)));*/
		range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new	pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::GT, yMax1)));
		pcl::ConditionalRemoval<pcl::PointXYZ> condrem(range_cond, false);
		condrem.setInputCloud(data[i].cloud);
		condrem.setKeepOrganized(false);
		condrem.filter(*data[i].cloud);
		//--------------------------------------------------------------

		//-----------------------去除离群点------------------------
		pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
		outrem.setInputCloud(data[i].cloud);
		outrem.setRadiusSearch(0.01);
		outrem.setMinNeighborsInRadius(10);
		outrem.filter(*data[i].cloud);

		//pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
		//sor.setInputCloud(data[i].cloud);
		//sor.setMeanK(10);
		//sor.setStddevMulThresh(1.0);
		//sor.filter(*data[i].cloud);
		//--------------------------------------------------------------

		// -------------------计算法向量----------------------
		//pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>); //法向量点云对象指针
		pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;//法线估计对象
		pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);//存储估计的法线的指针
		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
		tree->setInputCloud(data[i].cloud);
		n.setInputCloud(data[i].cloud);
		n.setSearchMethod(tree);
		n.setRadiusSearch(0.03);
		//n.setKSearch(25);
		n.compute(*normals); //计算法线,结果存储在normals中
		pcl::concatenateFields(*data[i].cloud, *normals, *data[i].cloudWithNormal);//将点云和法线放到一起
																				   // ---------------------------------------------------

		string fileName = folderPath + "cow" + num2str(i - 2) + "_withNormal.pcd";
		pcl::io::savePCDFile(fileName, *data[i].cloudWithNormal, true);
		cout << fileName << " has been saved." << endl;

		// 显示结果图
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
		int v1;
		viewer->createViewPort(0.0, 0.0, 1.0, 1.0, v1);
		viewer->setBackgroundColor(0, 0, 0);
		viewer->initCameraParameters();
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> cloud_color1(data[i].cloudWithNormal, 255, 255, 255);
		viewer->addPointCloud(data[i].cloudWithNormal, cloud_color1, "cloud_color1", v1);
		viewer->addPointCloudNormals<pcl::PointNormal>(data[i].cloudWithNormal, 50, 0.05, "source_normals", v1); //第2个参数表示多少个点显示一次向量,第3个参数表示向量长度,单位m
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "source_normals", v1);
		while (!viewer->wasStopped()) {
			viewer->spinOnce();
		}
	}
	std::system("pause");
	return 0;
}

3、基于Kinect v2+PCL的模型奶牛重建(下)——点云融合_奶牛的点云数据-优快云博客

错误1

#define vtkRenderingCore_AUTOINIT 4(vtkInteractionStyle,vtkRenderingFreeType,vtkRenderingFreeType,vtkRenderingOpenGL) 
#define vtkRenderingVolume_AUTOINIT 1(vtkRenderingVolumeOpenGL)
#include <string>
#include <kinect.h>
#include <iostream>
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL对各种格式的点的支持头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/radius_outlier_removal.h>  
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/icp.h> //ICP(iterative closest point)配准
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/registration/correspondence_rejection_one_to_one.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/console/parse.h>  //pcl控制台解析
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>  
//kd树
#include <pcl/kdtree/kdtree_flann.h>
//特征提取
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/normal_3d.h>
//重构
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>
#include <boost/thread/thread.hpp>
#include <Eigen/Dense>


using namespace std;

const float yMax = -0.218759;

string num2str(int i)
{
	stringstream ss;
	ss << i;
	return ss.str();
}

// Returns the rotation matrix around a vector  placed at a point , rotate by angle t
Eigen::Matrix4f rot_mat(const Eigen::Vector3f& point, const Eigen::Vector3f& vector, const float t)
{
	float u = vector(0);
	float v = vector(1);
	float w = vector(2);
	float a = point(0);
	float b = point(1);
	float c = point(2);

	Eigen::Matrix4f matrix;
	matrix << u*u + (v*v + w*w)*cos(t), u*v*(1 - cos(t)) - w*sin(t), u*w*(1 - cos(t)) + v*sin(t), (a*(v*v + w*w) - u*(b*v + c*w))*(1 - cos(t)) + (b*w - c*v)*sin(t),
		u*v*(1 - cos(t)) + w*sin(t), v*v + (u*u + w*w)*cos(t), v*w*(1 - cos(t)) - u*sin(t), (b*(u*u + w*w) - v*(a*u + c*w))*(1 - cos(t)) + (c*u - a*w)*sin(t),
		u*w*(1 - cos(t)) - v*sin(t), v*w*(1 - cos(t)) + u*sin(t), w*w + (u*u + v*v)*cos(t), (c*(u*u + v*v) - w*(a*u + b*v))*(1 - cos(t)) + (a*v - b*u)*sin(t),
		0, 0, 0, 1;
	return matrix;
}

//定义结构体,用于处理点云
struct PCD
{
	std::string f_name; //文件名
	pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormal;//存储估计的法线的指针
	Eigen::Vector3f mass;//存储点云的质心
						 //构造函数
	PCD() : cloudWithNormal(new pcl::PointCloud<pcl::PointNormal>), mass(Eigen::Vector3f::Zero()) {}; //初始化
};

void UpdatePCDMass(PCD& m)
{
	m.mass.Zero();
	for (int j = 0; j < m.cloudWithNormal->size(); ++j)
	{
		m.mass(0) += m.cloudWithNormal->points[j].x;
		m.mass(1) += m.cloudWithNormal->points[j].y;
		m.mass(2) += m.cloudWithNormal->points[j].z;
	}
	m.mass /= m.cloudWithNormal->size();
}

struct AABB
{
	Eigen::Vector3f center;
	Eigen::Vector3f min;
	Eigen::Vector3f max;
};

AABB computerAABB(pcl::PointCloud<pcl::PointNormal>::Ptr p)
{
	AABB aabb;
	aabb.min(0) = +10000;
	aabb.min(1) = +10000;
	aabb.min(2) = +10000;
	aabb.max(0) = -10000;
	aabb.max(1) = -10000;
	aabb.max(2) = -10000;
	for (int i = 0; i < p->size(); ++i)
	{
		if (p->points[i].x < aabb.min(0))
			aabb.min(0) = p->points[i].x;
		if (p->points[i].y < aabb.min(1))
			aabb.min(1) = p->points[i].y;
		if (p->points[i].z < aabb.min(2))
			aabb.min(2) = p->points[i].z;

		if (p->points[i].x > aabb.max(0))
			aabb.max(0) = p->points[i].x;
		if (p->points[i].y > aabb.max(1))
			aabb.max(1) = p->points[i].y;
		if (p->points[i].z > aabb.max(2))
			aabb.max(2) = p->points[i].z;
	}
	aabb.center = 0.5f*(aabb.max + aabb.min);
	return aabb;
}

int main()
{
	const int numberOfViews = 6;//点云数量
	std::vector<PCD, Eigen::aligned_allocator<PCD> > data; //点云数据
	std::string prefix("C:\\A0fangfang\\shiyan\\8.19\\cow");
	std::string extension("_withNormal.pcd"); // 声明并初始化string类型变量extension,表示文件后缀名
											  // 通过遍历文件名,读取pcd文件

	for (int i = 0; i < numberOfViews; i++) //遍历所有的文件名
	{
		std::string fname = prefix + num2str(i) + extension;
		// 读取点云,并保存到models
		PCD m;
		m.f_name = fname;
		if (pcl::io::loadPCDFile(fname, *m.cloudWithNormal) == -1) //* 读入PCD格式的文件,如果文件不存在,返回-1
		{
			cout << "Couldn't read file " + fname + "." << endl; //文件不存在时,返回错误,终止程序。
			return (-1);
		}
		for (int j = 0; j < m.cloudWithNormal->size(); ++j)
		{
			m.mass(0) += m.cloudWithNormal->points[j].x;
			m.mass(1) += m.cloudWithNormal->points[j].y;
			m.mass(2) += m.cloudWithNormal->points[j].z;
		}
		m.mass /= m.cloudWithNormal->size();
		data.push_back(m);
	}

	//-----------------------去除离群点------------------------
	pcl::RadiusOutlierRemoval<pcl::PointNormal> outrem;
	outrem.setRadiusSearch(0.01);
	outrem.setMinNeighborsInRadius(15);
	for (int i = 0; i < numberOfViews; ++i)
	{
		outrem.setInputCloud(data[i].cloudWithNormal);
		outrem.filter(*data[i].cloudWithNormal);
		UpdatePCDMass(data[i]);
	}
	//--------------------------------------------------------------

	Eigen::Vector3f totalMass1 = Eigen::Vector3f::Zero();//记录所有点云的质心
	int totalNumberOfPoints1 = 0;//记录所有点云的点个数
	for (int i = 0; i < 3; ++i)
	{
		totalNumberOfPoints1 += data[i].cloudWithNormal->size();
		totalMass1 += data[i].mass * data[i].cloudWithNormal->size();
	}
	totalMass1 /= totalNumberOfPoints1;

	Eigen::Vector3f totalMass2 = Eigen::Vector3f::Zero();//记录所有点云的质心
	int totalNumberOfPoints2 = 0;//记录所有点云的点个数
	for (int i = 3; i < 6; ++i)
	{
		totalNumberOfPoints2 += data[i].cloudWithNormal->size();
		totalMass2 += data[i].mass * data[i].cloudWithNormal->size();
	}
	totalMass2 /= totalNumberOfPoints2;

	Eigen::Vector3f upVector(0, 1.0, 0);
	Eigen::Matrix4f rotationMatrix = rot_mat(totalMass1, upVector, M_PI / 3);
	pcl::transformPointCloudWithNormals(*data[1].cloudWithNormal, *data[1].cloudWithNormal, rotationMatrix);
	UpdatePCDMass(data[1]);

	rotationMatrix = rot_mat(totalMass1, upVector, -M_PI / 3);
	pcl::transformPointCloudWithNormals(*data[2].cloudWithNormal, *data[2].cloudWithNormal, rotationMatrix);
	UpdatePCDMass(data[2]);

	rotationMatrix = rot_mat(totalMass2, upVector, M_PI / 3);
	pcl::transformPointCloudWithNormals(*data[4].cloudWithNormal, *data[4].cloudWithNormal, rotationMatrix);
	UpdatePCDMass(data[4]);

	rotationMatrix = rot_mat(totalMass2, upVector, -M_PI / 3);
	pcl::transformPointCloudWithNormals(*data[5].cloudWithNormal, *data[5].cloudWithNormal, rotationMatrix);
	UpdatePCDMass(data[5]);


	pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icpWithNormals;
	icpWithNormals.setMaxCorrespondenceDistance(0.5);
	icpWithNormals.setMaximumIterations(100);
	icpWithNormals.setTransformationEpsilon(1e-10);
	icpWithNormals.setEuclideanFitnessEpsilon(0.01);

	icpWithNormals.setInputCloud(data[1].cloudWithNormal);
	icpWithNormals.setInputTarget(data[0].cloudWithNormal);
	icpWithNormals.align(*data[1].cloudWithNormal);

	icpWithNormals.setInputCloud(data[2].cloudWithNormal);
	icpWithNormals.setInputTarget(data[0].cloudWithNormal);
	icpWithNormals.align(*data[2].cloudWithNormal);

	icpWithNormals.setInputCloud(data[4].cloudWithNormal);
	icpWithNormals.setInputTarget(data[3].cloudWithNormal);
	icpWithNormals.align(*data[4].cloudWithNormal);

	icpWithNormals.setInputCloud(data[5].cloudWithNormal);
	icpWithNormals.setInputTarget(data[3].cloudWithNormal);
	icpWithNormals.align(*data[5].cloudWithNormal);


	pcl::PointCloud<pcl::PointNormal>::Ptr Front(new pcl::PointCloud<pcl::PointNormal>);
	pcl::PointCloud<pcl::PointNormal>::Ptr Back(new pcl::PointCloud<pcl::PointNormal>);
	// ----------------------------------------------------
	*Front += *data[0].cloudWithNormal;
	*Front += *data[1].cloudWithNormal;
	*Front += *data[2].cloudWithNormal;
	UpdatePCDMass(data[1]);
	UpdatePCDMass(data[2]);

	*Back += *data[3].cloudWithNormal;
	*Back += *data[4].cloudWithNormal;
	*Back += *data[5].cloudWithNormal;
	UpdatePCDMass(data[4]);
	UpdatePCDMass(data[5]);
	// ----------------------------------------------------

	//---------------前后配准------------------------------
	Eigen::Vector3f mass = Eigen::Vector3f::Zero();
	for (int i = 0; i < Back->points.size(); ++i) {
		mass(0) += Back->points[i].x;
		mass(1) += Back->points[i].y;
		mass(2) += Back->points[i].z;
	}
	mass /= Back->points.size();
	rotationMatrix = rot_mat(mass, upVector, M_PI);
	pcl::transformPointCloudWithNormals(*Back, *Back, rotationMatrix);

	Eigen::Vector3f plane_left(-1, 0, 0);
	Eigen::Vector3f plane_right(1, 0, 0);
	float cos_angle = cos(M_PI * 10 / 180);

	AABB BackAABB = computerAABB(Back);
	cout << "BackAABB's center:\n" << BackAABB.center << endl;
	cout << "BackAABB's Z Length:\n" << BackAABB.max(2) - BackAABB.min(2) << endl;
	AABB FrontAABB = computerAABB(Front);
	cout << "FrontAABB's center:\n" << FrontAABB.center << endl;
	cout << "FrontAABB's Z Length:\n" << FrontAABB.max(2) - FrontAABB.min(2) << endl;
	Eigen::Vector3f diff = FrontAABB.center - BackAABB.center;
	Eigen::Matrix4f translationMatrix = Eigen::Matrix4f::Identity();
	translationMatrix(0, 3) = diff(0);
	translationMatrix(1, 3) = diff(1);
	translationMatrix(2, 3) = FrontAABB.max(2) - BackAABB.min(2) - (FrontAABB.max(2) - FrontAABB.min(2))*0.8;
	pcl::transformPointCloudWithNormals(*Back, *Back, translationMatrix);

	cout << "左右面配准" << endl;
	int iteration = 100;
	for (int iter = 0; iter < iteration; ++iter)
	{
		pcl::IndicesPtr source_indices(new std::vector<int>());
		for (int i = 0; i < Back->points.size(); ++i) {
			if (Back->points[i].y>yMax)
				continue;
			Eigen::Vector3f n = Back->points[i].getNormalVector3fMap();
			n.normalize();
			if (n.transpose() * plane_left > cos_angle) {
				source_indices->push_back(i);
				continue;
			}
			if (n.transpose() * plane_right > cos_angle) {
				source_indices->push_back(i);
			}
		}
		//cout << "Source Indices: " << source_indices->size() << endl;

		pcl::IndicesPtr target_indices(new std::vector<int>());
		for (int i = 0; i < Front->points.size(); ++i) {
			if (Front->points[i].y>yMax)
				continue;
			Eigen::Vector3f n = Front->points[i].getNormalVector3fMap();
			n.normalize();
			if (n.transpose() * plane_left > cos_angle) {
				target_indices->push_back(i);
				continue;
			}
			if (n.transpose() * plane_right > cos_angle) {
				target_indices->push_back(i);
			}
		}
		//cout << "Target Indices: " << target_indices->size() << endl;

		pcl::registration::CorrespondenceEstimationNormalShooting<pcl::PointNormal, pcl::PointNormal, pcl::PointNormal> correst;
		correst.setInputCloud(Back);
		correst.setSourceNormals(Back);
		correst.setInputTarget(Front);
		correst.setIndicesSource(source_indices);
		correst.setIndicesTarget(target_indices);
		correst.setKSearch(15);
		pcl::CorrespondencesPtr all_correspondences(new pcl::Correspondences);
		correst.determineReciprocalCorrespondences(*all_correspondences);
		//cout << "Correspondences (Before) : " << all_correspondences->size() << "\n";

		pcl::registration::CorrespondenceRejectorSurfaceNormal rejector;
		rejector.initializeDataContainer<pcl::PointNormal, pcl::PointNormal>();
		rejector.setInputSource<pcl::PointNormal>(Back);
		rejector.setInputTarget<pcl::PointNormal>(Front);
		rejector.setInputNormals<pcl::PointNormal, pcl::PointNormal>(Back);
		rejector.setTargetNormals<pcl::PointNormal, pcl::PointNormal>(Front);
		rejector.setInputCorrespondences(all_correspondences);
		rejector.setThreshold(M_PI * 10 / 180);
		pcl::CorrespondencesPtr correspondences_after_rejector(new pcl::Correspondences);
		rejector.getCorrespondences(*correspondences_after_rejector);
		//cout << "Correspondences (After) : " << correspondences_after_rejector->size() << "\n";


		Eigen::Matrix4f transformation;

		//pcl::registration::TransformationEstimationSVD<pcl::PointNormal, pcl::PointNormal> trans_est_svd;
		//trans_est_svd.estimateRigidTransformation(*Back, *Front, *correspondences_after_rejector, transformation);

		pcl::registration::TransformationEstimationLM<pcl::PointNormal, pcl::PointNormal> trans_est_lm;
		trans_est_lm.estimateRigidTransformation(*Back, *Front, *correspondences_after_rejector, transformation);

		//pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> trans_est_PointToPlane;
		//trans_est_PointToPlane.estimateRigidTransformation(*Back, *Front, *correspondences_after_rejector, transformation);

		pcl::transformPointCloudWithNormals(*Back, *Back, transformation);
		cout << "Iteration: " << iter << endl;
		//cout << "Matrix " << iter << ":\n" << transformation << endl;
		if (transformation.isIdentity())
			break;
	}

	cout << "上下面配准" << endl;
	for (int iter = 0; iter < iteration; ++iter)
	{
		pcl::IndicesPtr source_indices(new std::vector<int>());
		for (int i = 0; i < Back->points.size(); ++i) {
			if (Back->points[i].y>yMax)
				continue;
			Eigen::Vector3f n = Back->points[i].getNormalVector3fMap();
			n.normalize();
			if (n.transpose() * upVector > cos_angle) {
				source_indices->push_back(i);
				continue;
			}
		}
		//cout << "Source Indices: " << source_indices->size() << endl;

		pcl::IndicesPtr target_indices(new std::vector<int>());
		for (int i = 0; i < Front->points.size(); ++i) {
			if (Front->points[i].y>yMax)
				continue;
			Eigen::Vector3f n = Front->points[i].getNormalVector3fMap();
			n.normalize();
			if (n.transpose() * upVector > cos_angle) {
				target_indices->push_back(i);
				continue;
			}
		}
		//cout << "Target Indices: " << target_indices->size() << endl;

		pcl::registration::CorrespondenceEstimationNormalShooting<pcl::PointNormal, pcl::PointNormal, pcl::PointNormal> correst;
		correst.setInputCloud(Back);
		correst.setSourceNormals(Back);
		correst.setInputTarget(Front);
		correst.setIndicesSource(source_indices);
		correst.setIndicesTarget(target_indices);
		correst.setKSearch(15);
		pcl::CorrespondencesPtr all_correspondences(new pcl::Correspondences);
		correst.determineReciprocalCorrespondences(*all_correspondences);
		//cout << "Correspondences (Before) : " << all_correspondences->size() << "\n";

		pcl::registration::CorrespondenceRejectorSurfaceNormal rejector;
		rejector.initializeDataContainer<pcl::PointNormal, pcl::PointNormal>();
		rejector.setInputSource<pcl::PointNormal>(Back);
		rejector.setInputTarget<pcl::PointNormal>(Front);
		rejector.setInputNormals<pcl::PointNormal, pcl::PointNormal>(Back);
		rejector.setTargetNormals<pcl::PointNormal, pcl::PointNormal>(Front);
		rejector.setInputCorrespondences(all_correspondences);
		rejector.setThreshold(M_PI * 10 / 180);
		pcl::CorrespondencesPtr correspondences_after_rejector(new pcl::Correspondences);
		rejector.getCorrespondences(*correspondences_after_rejector);
		//cout << "Correspondences (After) : " << correspondences_after_rejector->size() << "\n";

		Eigen::Matrix4f transformation;

		pcl::registration::TransformationEstimationSVD<pcl::PointNormal, pcl::PointNormal> trans_est_svd;
		trans_est_svd.estimateRigidTransformation(*Back, *Front, *correspondences_after_rejector, transformation);

		//pcl::registration::TransformationEstimationLM<pcl::PointNormal, pcl::PointNormal> trans_est_lm;
		//trans_est_lm.estimateRigidTransformation(*Back, *Front, *correspondences_after_rejector, transformation);

		//pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> trans_est_PointToPlane;
		//trans_est_PointToPlane.estimateRigidTransformation(*Back, *Front, *correspondences_after_rejector, transformation);

		pcl::transformPointCloudWithNormals(*Back, *Back, transformation);
		cout << "Iteration: " << iter << endl;
		//cout << "Matrix " << iter << ":\n" << transformation << endl;
		if (transformation.isIdentity())
			break;
	}

	BackAABB = computerAABB(Back);
	cout << "BackAABB's center:\n" << BackAABB.center << endl;
	cout << "BackAABB's Z Length:\n" << BackAABB.max(2) - BackAABB.min(2) << endl;
	FrontAABB = computerAABB(Front);
	cout << "FrontAABB's center:\n" << FrontAABB.center << endl;
	cout << "FrontAABB's Z Length:\n" << FrontAABB.max(2) - FrontAABB.min(2) << endl;

	cout << "Z Length:\n" << FrontAABB.max(2) - BackAABB.min(2) << endl;

	pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
	*cloud = *Back;
	*cloud += *Front;

	
	    std::string outputFilename = "C:\\A0fangfang\\shiyan\\8.19\\cow.ply";
	    pcl::io::savePLYFile(outputFilename, *cloud, true);
	

	// 显示结果图
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
	int v1; //定义两个窗口v1,v2,窗口v1用来显示初始位置,v2用以显示配准过程
	int v2;
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);  //四个窗口参数分别对应x_min,y_min,x_max.y_max.
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(0, 0, 0);
	//viewer->addPolygonMesh(mesh, "mesh2", v2);

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> cloud_color4(Front, 0, 255, 0);
	viewer->addPointCloud(Front, cloud_color4, "cloud_color4", v1);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> cloud_color5(Back, 255, 0, 0);
	viewer->addPointCloud(Back, cloud_color5, "cloud_color5", v1);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> cloud_color6(Front, 255, 255, 255);
	viewer->addPointCloud(Front, cloud_color6, "cloud_color6", v2);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> cloud_color7(Back, 255, 255, 255);
	viewer->addPointCloud(Back, cloud_color7, "cloud_color7", v2);

	viewer->initCameraParameters();
	while (!viewer->wasStopped()) {
		viewer->spinOnce();
	}

	std::system("pause");
	return 0;
}

如果报错

C1128 节数超过对象文件格式限制: 请使用 /bigobj 进行编译

C1128 节数超过对象文件格式限制: 请使用 /bigobj 进行编译_严重性代码说明项目文件行禁止显示状态 错误c1128节数超过对象文件格式限-优快云博客 

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值