PCL点云绕调平

已知条件：
1.点云所有点坐标；
2.调平后的平面的法向量，如调平后和xoy平行，法向量n2（0，0，1）；
思路：
//1.拟合点云平面，计算点云的法向量n1；
//2.计算n1和n2的夹角；
//3.叉乘计算两个法向量的旋转轴；
//4.根据旋转轴和夹角计算旋转矩阵；
//5.旋转点云；
//6.获取点云的z坐标，生成深度图；
代码如下：

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
//#include <pcl/range_image/range_image.h> // 深度图头文件
#include <opencv.hpp>
#include <boost/thread.hpp>
#include <HalconCpp.h>
#include <omp.h>
#include <thread>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>//模型系数定义头文件
#include <pcl/filters/project_inliers.h>//投影滤波类头文件
#include <pcl/sample_consensus/ransac.h>
#include <pcl/common/transforms.h>
#include <pcl\range_image\range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl/point_types_conversion.h>

using namespace HalconCpp;
using namespace cv;
using namespace std;


boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
	//创建3D窗口并添加点云    
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud); //使用cloud的rgb 或者 rgba
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZRGB> fildColor(cloud, "z"); // 按照z字段进行渲染
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); //变量名，赋值
	//viewer->addCoordinateSystem(10000.0);
	//viewer->initCameraParameters();
	return (viewer);
}


void deepImage2PCL2(std::string depthImagePath, std::string rgbImagePath)
{
	//定义点云类型
	typedef pcl::PointXYZRGB PointT;
	typedef pcl::PointCloud<PointT> PointCloud;

	// 图像矩阵
	cv::Mat rgb, depth;
	// rgb 图像是8UC3的彩色图像
	rgb = cv::imread(rgbImagePath);
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
	depth = cv::imread(depthImagePath, -1);

	// 点云变量
	// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
	PointCloud::Ptr cloud(new PointCloud);

	int emptyNum(0);


	for (int m = 0; m < depth.rows; m++)
	{
		for (int n = 0; n < depth.cols; n++)
		{
			int d = depth.ptr<int>(m)[n];
			if (d == -1000000000)
			{
				continue;
			}
			PointT p;
			p.z = (d);
			p.x = n;
			p.y = m;
			p.b = rgb.ptr<uchar>(m)[n * 3];
			p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
			p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
			cloud->points.push_back(p);
		}
	}
	// 设置并保存点云
	cloud->height = 1000;
	cloud->width = cloud->points.size();
	cout << "point cloud size =" << cloud->points.size() << endl;
	cloud->is_dense = false;

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = rgbVis(cloud);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(1);
		boost::this_thread::sleep(boost::posix_time::microseconds(100));
	}

	// 清除数据并退出
	cloud->points.clear();
	cout << "Point cloud saved." << endl;
}


void deepImage2PCL3(std::string depthImagePath, std::string rgbImagePath)
{
	//定义点云类型
	typedef pcl::PointXYZRGB PointT;
	typedef pcl::PointCloud<PointT> PointCloud;
	cv::Mat rgb, depth;
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
	depth = cv::imread(depthImagePath, -1);
	rgb = cv::imread(rgbImagePath, -1);

	PointCloud::Ptr cloud(new PointCloud);

	int emptyNum(0);


	for (int m = 0; m < depth.rows; m++)
	{
		for (int n = 0; n < depth.cols; n++)
		{
			int d = depth.ptr<int>(m)[n];
			if (d == -1000000000)
			{
				continue;
			}
			PointT p;
			p.z = (d) / 100000;
			p.y = n * 0.009;
			p.x = m * 0.012;
			p.b = rgb.ptr<uchar>(m)[n * 3];
			p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
			p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
			cloud->points.push_back(p);
		}
	}
	// 设置并保存点云
	cloud->height = 7200 * 0.035;
	cloud->width = 3200 * 0.012;
	cout << "point cloud size =" << cloud->points.size() << endl;
	cloud->is_dense = false;

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = rgbVis(cloud);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(1);
		boost::this_thread::sleep(boost::posix_time::microseconds(100));
	}

	// 清除数据并退出
	cloud->points.clear();
	cout << "Point cloud saved." << endl;
}


void showPLY(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud, "z"); // 按照z字段进行渲染
	viewer->addPointCloud<pcl::PointXYZ>(cloud, fildColor, "cloud");
	viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud"); // 设置点云大小
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1000));
	}
	return;
}

//显示两个点云
void visualize_pcd2(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_src, pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_tgt)
{
	//创建初始化目标
	pcl::visualization::PCLVisualizer viewer("registration Viewer");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(pcd_src, src_h, "source cloud");
	viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");

	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}


void scale_image_range(HObject ho_Image, HObject* ho_ImageScaled, HTuple hv_Min,
	HTuple hv_Max)
{

	// Local iconic variables
	HObject  ho_ImageSelected, ho_SelectedChannel;
	HObject  ho_LowerRegion, ho_UpperRegion, ho_ImageSelectedScaled;

	// Local control variables
	HTuple  hv_LowerLimit, hv_UpperLimit, hv_Mult;
	HTuple  hv_Add, hv_NumImages, hv_ImageIndex, hv_Channels;
	HTuple  hv_ChannelIndex, hv_MinGray, hv_MaxGray, hv_Range;

	//Convenience procedure to scale the gray values of the
	//input image Image from the interval [Min,Max]
	//to the interval [0,255] (default).
	//Gray values < 0 or > 255 (after scaling) are clipped.
	//
	//If the image shall be scaled to an interval different from [0,255],
	//this can be achieved by passing tuples with 2 values [From, To]
	//as Min and Max.
	//Example:
	//scale_image_range(Image:ImageScaled:[100,50],[200,250])
	//maps the gray values of Image from the interval [100,200] to [50,250].
	//All other gray values will be clipped.
	//
	//input parameters:
	//Image: the input image
	//Min: the minimum gray value which will be mapped to 0
	//     If a tuple with two values is given, the first value will
	//     be mapped to the second value.
	//Max: The maximum gray value which will be mapped to 255
	//     If a tuple with two values is given, the first value will
	//     be mapped to the second value.
	//
	//Output parameter:
	//ImageScale: the resulting scaled image.
	//
	if (0 != (int((hv_Min.TupleLength()) == 2)))
	{
		hv_LowerLimit = ((const HTuple&)hv_Min)[1];
		hv_Min = ((const HTuple&)hv_Min)[0];
	}
	else
	{
		hv_LowerLimit = 0.0;
	}
	if (0 != (int((hv_Max.TupleLength()) == 2)))
	{
		hv_UpperLimit = ((const HTuple&)hv_Max)[1];
		hv_Max = ((const HTuple&)hv_Max)[0];
	}
	else
	{
		hv_UpperLimit = 255.0;
	}
	//
	//Calculate scaling parameters.
	//Only scale if the scaling range is not zero.
	if (0 != (HTuple(int(((hv_Max - hv_Min).TupleAbs()) < 1.0E-6)).TupleNot()))
	{
		hv_Mult = ((hv_UpperLimit - hv_LowerLimit).TupleReal()) / (hv_Max - hv_Min);
		hv_Add = ((-hv_Mult) * hv_Min) + hv_LowerLimit;
		//Scale image.
		ScaleImage(ho_Image, &ho_Image, hv_Mult, hv_Add);
	}
	//
	//Clip gray values if necessary.
	//This must be done for each image and channel separately.
	GenEmptyObj(&(*ho_ImageScaled));
	CountObj(ho_Image, &hv_NumImages);
	{
		HTuple end_val51 = hv_NumImages;
		HTuple step_val51 = 1;
		for (hv_ImageIndex = 1; hv_ImageIndex.Continue(end_val51, step_val51); hv_ImageIndex += step_val51)
		{
			SelectObj(ho_Image, &ho_ImageSelected, hv_ImageIndex);
			CountChannels(ho_ImageSelected, &hv_Channels);
			{
				HTuple end_val54 = hv_Channels;
				HTuple step_val54 = 1;
				for (hv_ChannelIndex = 1; hv_ChannelIndex.Continue(end_val54, step_val54); hv_ChannelIndex += step_val54)
				{
					AccessChannel(ho_ImageSelected, &ho_SelectedChannel, hv_ChannelIndex);
					MinMaxGray(ho_SelectedChannel, ho_SelectedChannel, 0, &hv_MinGray, &hv_MaxGray,
						&hv_Range);
					Threshold(ho_SelectedChannel, &ho_LowerRegion, (hv_MinGray.TupleConcat(hv_LowerLimit)).TupleMin(),
						hv_LowerLimit);
					Threshold(ho_SelectedChannel, &ho_UpperRegion, hv_UpperLimit, (hv_UpperLimit.TupleConcat(hv_MaxGray)).TupleMax());
					PaintRegion(ho_LowerRegion, ho_SelectedChannel, &ho_SelectedChannel, hv_LowerLimit,
						"fill");
					PaintRegion(ho_UpperRegion, ho_SelectedChannel, &ho_SelectedChannel, hv_UpperLimit,
						"fill");
					if (0 != (int(hv_ChannelIndex == 1)))
					{
						CopyObj(ho_SelectedChannel, &ho_ImageSelectedScaled, 1, 1);
					}
					else
					{
						AppendChannel(ho_ImageSelectedScaled, ho_SelectedChannel, &ho_ImageSelectedScaled
						);
					}
				}
			}
			ConcatObj((*ho_ImageScaled), ho_ImageSelectedScaled, &(*ho_ImageScaled));
		}
	}
	return;
}


bool HTuple2Point3d(HTuple px, HTuple py, HTuple pz, int w,int h, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
	try
	{
		//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		HTuple rowLength;
		HalconCpp::TupleLength(py, &rowLength);
		cloud->width = w;  // 点云中点的数量
		cloud->height = h;        // 单个无序点云
		cloud->points.resize(cloud->width* cloud->height);

		pcl::PointXYZ point;

		omp_set_num_threads(20);
		double start = omp_get_wtime();//获取起始时间  
		#pragma omp parallel for
		for (int i = 0; i < rowLength.I(); i++)
		{
			//point.x = px[i].D();
			//point.y = py[i].D();
			//point.z = pz[i].D();
			//pointCloud.points.push_back(point);
			cloud->points[i].x = px[i].D();
			cloud->points[i].y = py[i].D();
			cloud->points[i].z = pz[i].D() * 20;
		}
		double end = omp_get_wtime();
		double time2 = end - start; //返回程序运行时间
		cout << "openmp:" << time2 << endl;
	}
	catch (exception)
	{
		return false;
	}
	return true;
}

bool HTuple2Point3dRGB(HTuple px, HTuple py, HTuple pz,HTuple gray, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
	try
	{
		HTuple w, h, z;
		TupleLength(px, &w);
		TupleLength(py, &h);
		TupleLength(pz, &z);
		cloud->width = w.I();  // 点云中点的数量
		cloud->height = h.I();        
		cloud->points.resize(cloud->width* cloud->height);
		pcl::PointXYZRGB point;

		omp_set_num_threads(20);
		double start = omp_get_wtime();//获取起始时间  
		#pragma omp parallel for
		for (int i = 0; i < w.I(); i++)
		{
			cloud->points[i].x = px[i].D()*0.012;
			cloud->points[i].y = py[i].D() * 0.012;
			cloud->points[i].z = pz[i].D();
			cloud->points[i].r = cloud->points[i].g = cloud->points[i].b = gray[i].D();
		}
		double end = omp_get_wtime();
		double time2 = end - start; //返回程序运行时间
		cout << "openmp:" << time2 << endl;
	}
	catch (exception)
	{
		return false;
	}
	return true;
}

HObject Mat2HObject(const cv::Mat& image)
{
	HObject Hobj = HObject();
	int hgt = image.rows;
	int wid = image.cols;
	int i;
	//  CV_8UC3  
	if (image.type() == CV_8UC3)
	{
		vector<cv::Mat> imgchannel;
		split(image, imgchannel);
		cv::Mat imgB = imgchannel[0];
		cv::Mat imgG = imgchannel[1];
		cv::Mat imgR = imgchannel[2];
		uchar* dataR = new uchar[hgt * wid];
		uchar* dataG = new uchar[hgt * wid];
		uchar* dataB = new uchar[hgt * wid];
		for (i = 0; i < hgt; i++)
		{
			memcpy(dataR + wid * i, imgR.data + imgR.step * i, wid);
			memcpy(dataG + wid * i, imgG.data + imgG.step * i, wid);
			memcpy(dataB + wid * i, imgB.data + imgB.step * i, wid);
		}
		GenImage3(&Hobj, "byte", wid, hgt, (Hlong)dataR, (Hlong)dataG, (Hlong)dataB);
		delete[]dataR;
		delete[]dataG;
		delete[]dataB;
		dataR = NULL;
		dataG = NULL;
		dataB = NULL;
	}
	//  CV_8UCU1  
	else if (image.type() == CV_8UC1)
	{
		uchar* data = new uchar[hgt * wid];
		for (i = 0; i < hgt; i++)
			memcpy(data + wid * i, image.data + image.step * i, wid);
		GenImage1(&Hobj, "byte", wid, hgt, (Hlong)data);
		delete[] data;
		data = NULL;
	}
	else if (image.type() == CV_32FC1)
	{
		float* data = new float[hgt * wid];
		for (i = 0; i < hgt; i++)
			memcpy(data + wid * i, image.data + image.step * i, wid);
		GenImage1(&Hobj, "real", wid, hgt, (Hlong)data);
		delete[] data;
		data = NULL;
	}
	return Hobj;
}

Eigen::Affine3f CalMatrix(Eigen::Vector3f vecbefore, Eigen::Vector3f vecafter)
{
	//【1】求两个向量间的旋转角angle(点积)
	double tem = vecbefore.dot(vecafter);//分子
	double tep = sqrt(vecbefore.dot(vecbefore) * vecafter.dot(vecafter));//分母
	double angle = acos(tem / tep);
	if (isnan(angle))//acos取值范围[-1,1],若超出范围则越界，输出-1.#IND00
	{
		angle = acos(tep / tem);
	}
	std::cout << "角度： " << angle << std::endl;

	//【2】求旋转轴（叉积）
	Eigen::Vector3f axis1 = vecbefore.cross(vecafter);
	//	std::cout << "求旋转轴： " << axis1 << std::endl;
	Eigen::Vector3f axis2 = vecafter.cross(vecbefore);
	//	std::cout << "求旋转轴： " << axis2 << std::endl;
	std::cout << "旋转轴(归一化)： " << axis2.normalized() << std::endl;
	//【3】求旋转矩阵
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	// Define a translation of 2.5 meters on the x axis.
	transform_2.translation() << 0, 0, 0;
	// The same rotation matrix as before; theta radians arround Z axis
	transform_2.rotate(Eigen::AngleAxisf(angle, axis2.normalized()));
	// Print the transformation
	std::cout << transform_2.matrix() << std::endl;
	return   transform_2;
}


double CalAngle(Eigen::Vector3f plane_normal)
{
	Eigen::Vector3f vec_z;
	vec_z << 0, 0, 1;
	return pcl::getAngle3D(vec_z, plane_normal);
}

//将点云旋转到与XOY平面平行
pcl::PointCloud<pcl::PointXYZ>::Ptr AdjustPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	// 思路：
	//1.计算法向量n1
	//2.计算n1和0 0 1的夹角 
	//3.叉乘计算两个法向量的旋转轴
	//4.根据旋转轴和夹角计算旋转矩阵
	//5.旋转点云
	//6.获取点云的z坐标，生成深度图

	//降采样

	//计算质心
	Eigen::Vector4f centroid;  //质心 
	pcl::compute3DCentroid(*cloud, centroid);

	// 拟合平面
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);
	ransac.setDistanceThreshold(0.2);
	ransac.computeModel(0);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>);
	vector<int> inliers;
	ransac.getInliers(inliers);
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_plane);
	showPLY(cloud_plane);
	//visualize_pcd2(cloud, cloud_plane);

	//3.计算法向量，并计算法向量和x  y轴的夹角
	Eigen::VectorXf coefficient;
	ransac.getModelCoefficients(coefficient);
	Eigen::Vector3f plane_normal(coefficient[0], coefficient[1], coefficient[2]);
	cout << "平面法向量：\n" << plane_normal << endl;

	//XOY平面的法向量
	Eigen::Vector3f vec_z;
	vec_z << 0, 0, 1;
	//计算旋转矩阵
	Eigen::Affine3f trans = CalMatrix(plane_normal, vec_z);
	//点云变换
	pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud, *transform_cloud, trans);
	showPLY(transform_cloud);
	return transform_cloud;
}


int main(int argc, char** argv)
{
	HObject image, region, imageReduce,imageScale;
	HalconCpp::ReadImage(&image, "C:/Users/JZ/Desktop/reel/test/crop.tif");
	HalconCpp::ZoomImageFactor(image, &image, 0.25, 0.25, "constant");
	HalconCpp::Threshold(image, &region, -10, -9);
	HalconCpp::ReduceDomain(image, region, &imageReduce);
	HTuple min, max, range, rows,cols, height,gray,w,h, rowLength, colLength;
	HalconCpp::MinMaxGray(region, imageReduce, 0, &min, &max, &range);
	scale_image_range(image,&imageScale,min,max);
	HalconCpp::GetRegionPoints(region, &rows, &cols);
	HalconCpp::GetGrayval(image, rows, cols, &height);
	HalconCpp::GetGrayval(imageScale, rows, cols, &gray);
	HalconCpp::GetImageSize(image, &w, &h);
	HalconCpp::TupleLength(rows, &rowLength);
	HalconCpp::TupleLength(rows, &colLength);

	double len1 = colLength.I();
	double len2 = rowLength.I();
	double dasdf = cols[1].D();
	double dasdf1 = cols[2].D();

	//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//HTuple2Point3d(cols, rows, height, cloud);
	//showPLY(cloud);

	pcl::PointCloud<pcl::PointXYZ>::Ptr src(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	//HTuple2Point3dRGB(cols, rows, height, gray,cloud);
	HTuple2Point3d(cols, rows, height,w,h, src);
	showPLY(src);
	pcl::io::savePCDFile("C:\\Users\\JZ\\Desktop\\reel\\test\\test.pcd", *src);


	pcl::PointCloud<pcl::PointXYZ>::Ptr dst(new pcl::PointCloud<pcl::PointXYZ>);
	//旋转
	dst = AdjustPlane(src);
	showPLY(dst);

//	pcl::io::savePCDFile("C:\\Users\\JZ\\Desktop\\reel\\test.pcd", *cloud);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer= rgbVis(cloud);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(10));
	}


	std::string filePath = "E:\\Code\\Project\\tmp\\c++\\PCL\\PCLTest\\00.ply";
	//showPCLPLY(filePath);
	//boost::thread  pcl_thread(&showPCLPLY, filePath);
	//showPCLPLY(filePath);

	std::string depthImagePath = "E:\\WorkSpace\\Code\\tmp\\c++\\PCL\\PCLTest\\00.tiff";
	std::string rgbImagePath = "E:\\WorkSpace\\Code\\tmp\\c++\\PCL\\PCLTest\\depth2grayImage.bmp";
	deepImage2PCL3(depthImagePath, rgbImagePath);
	return 0;
}