立方体点云长宽高提取

场景描述：将长方体工件放置在场地中央，利用扫描仪扫描箱子的除上下两个面的剩下四个面，将四个面的点云通过标定到一个坐标系下，将坐标的z轴建立在与四个面平行的位置。

具体思路：

1.利用Ransac算法提取点云，将四个面提取出来，保留平面参数，和点云质心，和点云包围盒。

2.计算四个平面的法向量夹角，如果超过10°则认为是非平行平面，获取四个点云平面的匹配关系。

3.计算两个平面的距离，采用中心点的方法互相计算到相对平面的距离，如果差距过大则认为距离计算失败。

4.通过Z轴来计算扫描点云的高度，也就是立方体点云的高度。

具体实现代码如下

double CalculateAngle(double  x1, double y1, double z1, double x2, double y2, double z2)
{
	double dAngle = -99999;
	double dcosangle = (x1 * x2 + y1 * y2 + z1 * z2) / (sqrt(x1 * x1 + y1 * y1 + z1 * z1) * sqrt(x2 * x2 + y2 * y2 + z2 * z2));
	dAngle = acos(dcosangle);
	return dAngle;
}
void TestCuboid(std::string strpath,std::vector<double> & vecDistance)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(strpath, *cloud) == -1)
	{
		PCL_ERROR("点云读取失败 \n");
		return ;
	}
	vecDistance.clear();;
	std::cout << "当前点云数量:" << cloud->points.size() << "当前点云宽度:" << cloud->width << "当前点云高度:" << cloud->height << std::endl;
	// ---------------------------RANSAC过程----------------------------

	std::vector<int> totalInliers;
	std::vector<int> indices(cloud->points.size());
	std::iota(std::begin(indices), std::end(indices), (int)0);
	float dist = 0.1; // 距离阈值
	int planeMinNum = 200;   // 平面上点的最小个数
	double dThreshold = 10 * (M_PI / 180.0);
	std::vector<CuboidParameter> vecParameter;
	int i = 1;
	do
	{
		std::vector<int> inliers;
		pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model
		(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud, indices));//创建平面模型
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model);
		ransac.setDistanceThreshold(dist);
		ransac.computeModel();
		ransac.getInliers(inliers);
		totalInliers.insert(totalInliers.end(), inliers.begin(), inliers.end());
		//获取平面参数
		Eigen::VectorXf planeparameter;
		ransac.getModelCoefficients(planeparameter);

		//提取剩余的点云数据,这种方式有点粗糙，但可以达到自己想要的结果
		std::vector<int> newIndices;
		pcl::ExtractIndices<pcl::PointXYZ> ex(true);
		ex.setInputCloud(cloud);
		ex.setNegative(true); // false代表提取指定的索引点，true则代表提取索引外的点
		const pcl::IndicesPtr ptrImport = boost::make_shared<std::vector<int>>(totalInliers);
		ex.setIndices(ptrImport);// 指定点的索引
		ex.filter(newIndices);
		indices = newIndices;
		pcl::PointCloud<pcl::PointXYZ>::Ptr planar_segment(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud, inliers, *planar_segment);
		Eigen::Vector4f centroid;
		pcl::compute3DCentroid(*planar_segment, centroid);
		pcl::PointXYZ minTest, maxTest;
		pcl::getMinMax3D(*planar_segment, minTest, maxTest);
		CuboidParameter cubuid;
		cubuid.A = planeparameter[0];
		cubuid.B = planeparameter[1];
		cubuid.C = planeparameter[2];
		cubuid.D = planeparameter[3];
		cubuid.Cx = centroid[0];
		cubuid.Cy = centroid[1];
		cubuid.Cz = centroid[2];
		cubuid.dMinx = minTest.x;
		cubuid.dMiny = minTest.y;
		cubuid.dMinz = minTest.z;
		cubuid.dMaxx = maxTest.x;
		cubuid.dMaxy = maxTest.y;
		cubuid.dMaxz = maxTest.z;
		vecParameter.push_back(cubuid);
		pcl::io::savePCDFileBinary("plane_" + std::to_string(i) + ".pcd", *planar_segment);
		i++;
	} while (indices.size() > planeMinNum);	// 判断剩余点数
	std::vector<int> vecMatch;
	for (int i = 0; i < vecParameter.size(); ++i)
	{
		bool bFind = false;
		if (!vecMatch.empty())
		{
			for (int j = 0; j < vecMatch.size(); ++j)
			{
				if (i == vecMatch[j])
					bFind = true;
			}
		}
		if (bFind)
			continue;
		for (int n = i + 1; n < vecParameter.size();++n)
		{
			bool bFind2 = false;
			if (!vecMatch.empty())
			{
				for (int j = 0; j < vecMatch.size(); ++j)
				{
					if (i == vecMatch[j])
						bFind2 = true;
				}
			}
			if (bFind2)
				continue;
			//
			double dAngle = (vecParameter[i].A, vecParameter[i].B, vecParameter[i].C, vecParameter[n].A, vecParameter[n].B, vecParameter[n].C);
			if (std::abs(dAngle) < dThreshold)
			{
				vecMatch.push_back(i);
				vecMatch.push_back(n);
			}
		}
	}
	//计算距离
	if (vecMatch.size() % 2 > 0)
		return;


	
	for (int i = 0; i < vecMatch.size(); i+=2)
	{
		double dT1 = std::abs(vecParameter[vecMatch[i]].Cx * vecParameter[vecMatch[i + 1]].A + vecParameter[vecMatch[i]].Cy * vecParameter[vecMatch[i + 1]].B + vecParameter[vecMatch[i]].Cz * vecParameter[vecMatch[i + 1]].C + vecParameter[vecMatch[i + 1]].D);
		double dT2 = sqrt(vecParameter[vecMatch[i + 1]].A * vecParameter[vecMatch[i + 1]].A + vecParameter[vecMatch[i + 1]].B * vecParameter[vecMatch[i + 1]].B + vecParameter[vecMatch[i + 1]].C * vecParameter[vecMatch[i + 1]].C);
		double dDistance1 = dT1 / dT2;
		double dT3 = std::abs(vecParameter[vecMatch[i+1]].Cx * vecParameter[vecMatch[i]].A + vecParameter[vecMatch[i+1]].Cy * vecParameter[vecMatch[i]].B + vecParameter[vecMatch[i+1]].Cz * vecParameter[vecMatch[i ]].C + vecParameter[vecMatch[i]].D);
		double dT4 = sqrt(vecParameter[vecMatch[i]].A * vecParameter[vecMatch[i]].A + vecParameter[vecMatch[i]].B * vecParameter[vecMatch[i]].B + vecParameter[vecMatch[i]].C * vecParameter[vecMatch[i]].C);
		double dDistance2 = dT3 / dT4;
		if (std::abs(dDistance1 - dDistance2) < 5)
		{
			vecDistance.push_back((dDistance1 + dDistance2) / 2);
		}
	}
	//计算高
	double dTotal=0;
	for (int i = 0; i < vecParameter.size(); ++i)
	{
		dTotal += (vecParameter[i].dMaxz - vecParameter[i].dMinz);
	}
	double dHeight = dTotal / vecParameter.size();
	vecDistance.push_back(dHeight);

    int kkk = 100;







}