仓库物资识别七：获取装载车高度

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

之前直接从文档中获取的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米之间，具体的高度，等去现场的时候再验证，至少这个值还是挺靠谱的

完整代码：

#include<iostream>
#include<vector>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<sstream>
#include<fstream>
#include<algorithm>
using namespace std;
using namespace cv;
struct MYPOINT2
{
	int i;
	int j;
};
struct myPOINT
{
	int N;
	//像素
	int I;
	int J;
	//三维
	float X;
	float Y;
	float Z;
	float G;
	float Angle;
};
int main()
{
	ifstream Read;
	Read.open("C:\\Users\\asus\\Desktop\\程序\\数据（角度2）.txt");
	string s;
	vector<myPOINT> Points;
	string String; //创建一个字符串 存储每行的字符串
	while (getline(Read, String)) //读文件中的一行，传到String中
	{
		myPOINT P;
		vector<string> elems;  //存放每一行的一个字符串
		stringstream ss;
		ss.str(String);
		string ele;
		char delim = ' ';
		while (getline(ss, ele, delim))
		{
			elems.push_back(ele);
		}
		P.N = atof(elems[0].c_str());
		P.I = atof(elems[1].c_str());
		P.J = atof(elems[2].c_str());
		P.X = atof(elems[3].c_str());
		P.Y = atof(elems[4].c_str());
		P.Z = atof(elems[5].c_str());
		if (P.Z > 12.13)
			P.G = 255;
		else
			P.G = atof(elems[6].c_str());
		P.Angle = atof(elems[7].c_str());
		Points.push_back(P);
	}
	Mat srcImage = imread("C:\\Users\\asus\\Desktop\\激光扫描\\行车图像识别\\闭运算.jpg", 0);
	if (!srcImage.data){ cout << "图片载入失败！！" << endl; return -1; }
	srcImage = srcImage > 195;	//srcImage 取大于阈值的那部分
	
	//查找轮廓
	vector<vector<Point>> contours;  //存放轮廓
	vector<Vec4i> hierarchy; 
	findContours(srcImage, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);

	//计算每个轮廓对象的矩
	vector<Moments> mu(contours.size());  //存储每个轮廓对象的矩
	for (size_t i = 0; i < contours.size(); i++)
	{
		mu[i] = moments(contours[i], false);  //每个轮廓存入每个矩中，第一个参数是二维数组，存放每个轮廓的二维点
	}

	//计算图像的质心 存放到 mc 之中
	vector<Point2f> mc(contours.size());  //存储每个矩的中心点
	for (int i = 0; i < contours.size(); i++)
	{
		mc[i] = Point2f(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
	}

	RNG rng(12345);
	Mat dstImage = Mat::zeros(srcImage.rows, srcImage.cols, CV_8SC3);
	vector<MYPOINT2> POINT2; //存放中心点
	for (int i = 0; i < contours.size(); i++)
	{
		Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
		drawContours(dstImage, contours, i, color, 2, 8, hierarchy, 0, Point());

		if ((contourArea(contours[i]) > 6000) && (contourArea(contours[i]) < 8000))
		{
			MYPOINT2 P;
			circle(dstImage, mc[i], 4, color, -1, 8, 0);
			P.i = mc[i].x;
			P.j = mc[i].y;
			POINT2.push_back(P); //此时 符合条件的中心点存放在POINT2数组中
			//cout << mc[i] << endl;
		}
	}

	//遍历中心点
	//cout << "中心点坐标为： " << endl;
	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);
				}
			}
		}
	}

	double max = *max_element(begin(high), end(high));
	double H = 12.13 - max;
	cout << max << endl;
	cout << "车高为 " << H << endl;
	system("pause");
	return 0;
}

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