基于单目视觉的四旋翼定点降落——地标设计与识别算法（四）

前面讲了三讲的环境搭建，今天终于要开始讲算法啦。先来描述一下定点降落的过程，四旋翼上搭载的摄像头识别地面上的标志，然后以地面标志作为引导，降落到标志所在处。所以，我们今天先来讲讲地标的设计与识别算法。

首先是地标设计，地标设计有一个最重要的原则就是特征明显。优秀的地标能够简化识别算法，提高降落效率与鲁棒性。我的地标是这个样子滴~~~

这样设计的好处在于特征明显，黑白相间的正方形非常容易识别。而且，正方形由小到大逐级嵌套，起到分级识别的作用。比如，飞机降落到接近地标的时候，我们很有可能看不到最大的那个矩形了，但是我们依然可以识别到稍小的矩形。当然，这个地标并非是我原创，借鉴于一篇硕士论文《基于视觉的小型无人直升机自主降落导航系统的设计和研究》，有兴趣的同学可以在CSDN上搜索一下。

下面，我就来讲讲最重要的地标识别算法吧。分为几个步骤：动态阈值二值化、轮廓提取与矩形检测、矩形聚类与识别。

1 动态阈值二值化

为了提高识别算法的鲁棒性，我决定采用大津动态阈值的方法将图像二值化。具体理论就不多讲了，毕竟不是什么新的东西，我们的重点也不在这块。直接上代码吧~~~（这里啰嗦一句，这段代码我是抄CSDN上一个哥们的，但是忘了出处，没有标明，实在抱歉）

//输入图像，输出阈值，很简单吧
int otsuThreshold(IplImage* img)
{
	
	int T = 0;
	int height = img->height;
	int width  = img->width;
	int step      = img->widthStep;
	int channels  = img->nChannels;
	uchar* data  = (uchar*)img->imageData;
	double gSum0;
	double gSum1;
	double N0 = 0;
	double N1 = 0;
	double u0 = 0;
	double u1 = 0;
	double w0 = 0;
	double w1 = 0;
	double u = 0;
	double tempg = -1;
	double g = -1;
	double Histogram[256]={0};// = new double[256];
	double N = width*height;
	for(int i=0;i<height;i++)
	{
		for(int j=0;j<width;j++)
		{
			double temp =data[i*step + j * 3] * 0.114 + data[i*step + j * 3+1] * 0.587 + data[i*step + j * 3+2] * 0.299;
			temp = temp<0? 0:temp;
			temp = temp>255? 255:temp;
			Histogram[(int)temp]++;
		} 
	}
	
	for (int i = 0;i<256;i++)
	{
		gSum0 = 0;
		gSum1 = 0;
		N0 += Histogram[i];			
		N1 = N-N0;
		if(0==N1)break;
		w0 = N0/N;
		w1 = 1-w0;
		for (int j = 0;j<=i;j++)
		{
			gSum0 += j*Histogram[j];
		}
		u0 = gSum0/N0;
		for(int k = i+1;k<256;k++)
		{
			gSum1 += k*Histogram[k];
		}
		u1 = gSum1/N1;
		//u = w0*u0 + w1*u1;
		g = w0*w1*(u0-u1)*(u0-u1);
		if (tempg<g)
		{
			tempg = g;
			T = i;
		}
	}
	return T; 
}

可以对比一下原图和二值化后的图像：

2 轮廓提取与矩形检测

轮廓提取用了opencv现成的函数——cvFindContours。具体用法直接百度就行，不再赘述。主要是矩形检测，我们分为两步：多边形近似、四边形提取。首先用cvApproxPoly函数对所有提取出的轮廓多边形近似，再寻找具有四条边且面积较大的（过滤较小的图形）的轮廓，我们暂且将其判定为矩形。代码如下：

void FindSquares( IplImage* src, CvSeq* squares, CvMemStorage* storage, vector<Point> &squares_centers, vector< vector<Point> > &squares_v, Point pt )
{
	CvSeq* cv_contours;    // 边缘
	CvSeq* result;         // the result of detecting squares
	CvSeqReader reader;    // the pointer to read data of "result"
	CvPoint corner[4];
	vector<Point> corner_v;
	Point temp;
	Point center;
	cvFindContours( src, storage, &cv_contours, sizeof(CvContour),CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, pt ); //find contours
	while(cv_contours)
	{
		if( fabs(cvContourArea(cv_contours)) > MIN_SQUARE_AREA )   //neglect the small contours
		{
                        //findout the squares
                         result = cvApproxPoly( cv_contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, cvContourPerimeter(cv_contours)*0.02, 0 );
			if( result->total == 4  &&  cvCheckContourConvexity(result) )  
            {
				cvStartReadSeq( result, &reader, 0 );                      
                for( int i = 0; i < 4; i++ )
				{
					cvSeqPush( squares,(CvPoint*)cvGetSeqElem( result, i ));
					memcpy( corner + i, reader.ptr, result->elem_size ); 
        			CV_NEXT_SEQ_ELEM( result->elem_size, reader );
				}
				for(int i =0; i < 4; i++)    //save the corner points to corner_v, it will help us process the data
				{
					temp = corner[i];
					corner_v.push_back(temp);
				}
				center.x = (corner[0].x + corner[1].x + corner[2].x + corner[3].x) / 4;
				center.y = (corner[0].y + corner[1].y + corner[2].y + corner[3].y) / 4;
				squares_centers.push_back(center);   
				squares_v.push_back(corner_v);
				corner_v.clear();       
            } 
		}                                     
        cv_contours = cv_contours->h_next;    
	}
}

执行上述代码，我们发现图像中的矩形都被检测出来了（绿色比较淡，仔细看看~~~）。

3 矩形聚类与识别

为什么要做矩形聚类？因为我们要把所有矩形分类，将中心点近似重合的矩形归为一类。最终，矩形数量最多的那一类我们判定为正确目标（因为实际生活中，很难有这么多层的矩形嵌套）。话不多说，上代码。

//输入矩形中心，输出聚类后的结果。
//比如输入（1,1）（2,2）（3,3）（1.01,1.01）
//输出为第一类为0，3，第二类为1，第三类为2。（数字表示第几个矩形中心点）
void CenterClassification( vector<Point> &squares_centers, vector< vector<int> > &result)
{
	vector<int> centers_index;
	vector<int> result_temp;
	int index_i;
	int index_j;
	for(int i = 0; i < squares_centers.size(); i++)
	{
		centers_index.push_back(i);    //save the index of squares centers
	}

	for(int i = 0; i < centers_index.size(); i++)
		{
			result_temp.push_back(centers_index[i]);
			for(int j = i + 1; j < centers_index.size(); j++)
			{
				index_i = centers_index[i];
				index_j = centers_index[j];
				if( ComputeDistance( squares_centers[index_i], squares_centers[index_j] ) < MIN_CENTER_DIS )
				{
					result_temp.push_back(centers_index[j]);
					centers_index.erase(centers_index.begin() + j - 1);
					j--;
				}
			}
			result.push_back(result_temp);
			result_temp.clear();
		}
}

可以看一下最终的识别结果。

可见，第二歩中检测到的许多矩形都被我们过滤掉了。经过测试，这种识别方法的实时性和鲁棒性都不错。作者的电脑是i3-6100的CPU，但是检测算法跑到了30Hz以上，满足无人机实时性的要求。另外，在弱光，强光环境下，该算法的鲁棒性都很好。