【仓库物资识别】七、获取装载车高度

激光雷达的工作原理是向目标发射激光束，然后接受激光束反射回来的信号，来分析处理物体信息

之前直接从文档中获取的z坐标，都只是雷达距离物资某点位置的距离长度，而不是该点在三维坐标系中的具体位置信息。

所以要获取车高，也就是某一点的真实信息，还需要将距离信息转换成某点物体的真实高度。

整个系统的大体环境如下图（画功不足）：

这是一个俯视图，当装载车停放后，行吊系统会用安装好的雷达扫描装载车。前面的文章已经对扫描到的文件进行陆陆续续的处理了。下面是求物资点具体高度的代码已经配图。

代码如下：

vector<information_Y> PointY;
	float m_dbLaserHeight = 12.13f; //扫描仪高度
	float fAngleResolution = 0.1667f; //扫描角度分辨率
	float fAngleScanRange = 18.0f;    //扫描角度范围
	float EPI = 3.1415926f / 180;
	unsigned int nAngleMax = (unsigned int)ceil((90 + fAngleScanRange) / fAngleResolution);
	unsigned int nAngleMin = (unsigned int)ceil((90 - fAngleScanRange) / fAngleResolution);  //432
	for (unsigned int n = nAngleMin; n <= nAngleMax; n++)
	{
		information_Y pointy;
		float dbAngle = n * fAngleResolution;
		float db90Angle = (90 - dbAngle)*EPI;  //角度到弧长的转化
		float acty = 10.19 - m_dbLaserHeight*tan(db90Angle);
		pointy.y = acty;
		pointy.angle = sin(dbAngle * EPI);
		PointY.push_back(pointy);
	}

这里要注意的是，需要有一个弧度和角度的转换，求y坐标信息的同时，将该点的角度也求出并保存在一个存储y的结构中，以便后续将所有的信息存储到一个大结构体中，以方便后续的使用。

求车高的思路：

前面的步骤已经求出了12个物资的中心点，拿（219，132）这点来说，取中心点方向，取x=219，y的范围在100~120个点之间，这十二个中心点都这样取点，也就是12*20个点，这些点中，一定存在车面的点（假设车的装载区是平整的），将这些点的Z坐标乘以之前保存好的结构体中的对应sin值，即可得到该点距离雷达点的垂直高度。其中有些点，是车面，有些是线缆盘平面的点，还有一些是由于线缆盘老化形变的表面的点，找到这些距离的最大值，就是雷达距离车面的高度，与之前雷达距离地面的高度12.13做差，即可得到车高，下面是部分代码：

vector<double> high;
	for (int l = 0; l < POINT2.size(); l++) //遍历12个中心点集合
	{
		for (int L = 0; L < Points.size(); L++) //遍历结构体数组中所有的点
		{
			for (int n = 100; n < 120; n++) //在y轴找点的数量范围
			{
				//找到与中心点x坐标（i像素）相同，y轴坐标（j像素）在100-200之间的点
				if ( (Points[L].I == POINT2[l].i) && (Points[L].J == n) ) 
				{
					double h = Points[L].Z * (Points[L].Angle);
					high.push_back(h);
				}
			}
		}
	}

其中所有距离中的最大值为10.6426米，雷达距离地面的高度为12.13米，所以得到车高为 1.4874米

我百度了一下，装载车的高度大概在1.3~1.5米之间，具体的高度，等去现场的时候再验证，至少这个值还是挺靠谱的

虽然想法实现了，可是程序执行的时间太长，以后还是需要改进的~