openCV双目视觉测距

最近师兄在做多机器人的实验，需要用到两个机器人之间的距离信息。这个当然可以通过超声测距等硬件设备来获得，但现在机器人上边有了一个kinect，所以想直接从图像中获得机器人间的距离。原理很简单，就是利用针孔模型，得到色标立柱在图像中的像素个数，然后通过相似三角形求出距离。当然，kinect本身是具有测距的功能的，但是我们准备直接用识别机器人（不外加标志）的方法来弄，而直接识别机器人另外一个同学在弄。既然我已经弄出来了，就总结一下。(之前用棋盘格作为标志，因为它的角点非常多且易于提取，但对于棋盘格角点的提取不是非常的准确即提取的角点不在一条线上所以不太好判断，加之格子的边长太小，距离1.5m基本就失效了。并且在室内，能提取到角点的物体非常多，识别棋盘格的时候很容易跑偏到别的物体上。用棋盘格围成的立柱直径太小就不容易识别出，太大又会出现上下边缘呈弧形同样影响识别）

首先看看色标立柱，手工制作(羽毛球桶+黑色纸）。黑白相间，等宽，5cm.

简单介绍一下算法流程：

1.读入灰度图像

2.二值化

3.用opencv中的canny算子提取边缘

4.利用图形学中的腐蚀与膨胀，清除掉面积较小的区域

5.遍历每一列中像素，记录灰度值变化的次数，大于等于6时判断是否是立柱上的点(立柱用黑线条画出来了）

7.是立柱上的点，则最下边的点减去最上边的点，求出立柱的高度（这样要比单独求一个色环高度波动要小更鲁棒也更精确）

8.求出立柱的两个边界，得到中心。计算与图像中心的距离，可求得立柱与相机正方向的夹角。（这个精度受之前距离的精度影响）

(实际距离925mm)

最后是代码了

// distance.cpp : 定义控制台应用程序的入口点。
//


// distance.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <iostream>
#include <stdlib.h>
#include <opencv.hpp>
#include <math.h>

using namespace std;
#define focus 525

int main(int argc, char** argv)
{
	cout << "processing ... ..."<<endl; 
	//读入图片
	char* filename="C:\\Users\\ijamcen\\Desktop\\image_sebiao\\925.jpg";
	IplImage* imgGrey = cvLoadImage(filename,0); 
	if (imgGrey==NULL){
		cout << "No valid image input."<<endl; 
		cv::waitKey(0);
		return 1;
	}  
	int w=imgGrey->width;
	int h=imgGrey->height;

	IplImage* smoothImg = cvCreateImage(cvSize(w, h),IPL_DEPTH_8U, 1);
	IplImage* binaryImg = cvCreateImage(cvSize(w, h),IPL_DEPTH_8U, 1);	
	IplImage* erode = cvCreateImage(cvSize(w, h),IPL_DEPTH_8U, 1);
	IplImage* dilate = cvCreateImage(cvSize(w, h),IPL_DEPTH_8U, 1);

	cvSmooth(imgGrey,smoothImg,CV_MEDIAN,3,0);//CV_MEDIAN.//CV_GAUSSIAN
	cvThreshold(smoothImg,binaryImg,40,255,CV_THRESH_BINARY); 
	cvSaveImage("bin.jpg",binaryImg);



	//形态学操作
	IplConvKernel* element = 0; 
	element = cvCreateStructuringElementEx( 7, 7, 0, 0, CV_SHAPE_RECT, 0 );
	cvDilate(binaryImg,dilate,element,1);	
	element = cvCreateStructuringElementEx( 7, 7, 6, 6, CV_SHAPE_RECT, 0 );
	cvErode(dilate,erode,element,1);	
	cvReleaseStructuringElement(&element); 
	cvSaveImage("morphology.jpg",erode);

	//find the landmark
	int pace_step = erode->widthStep;
	uchar* pt;
	int head = 170;
	int end = 380;
	std::vector<int> change_idx;
	int left=0,right=0;
	std::vector<int> diff;
	int Threshold = 4;//实际检验，设为4效果较好。
	int sum = 0, num =0;
	for(int j=0;j<w;j++)//ergodic colums
	{		

		pt = (uchar*)(erode->imageData + head*pace_step + j);		
		for(int i=head;i<end;i++)//ergodic rows
		{
			unsigned short int pix1 =*pt;
			unsigned short int pix2 =*(pt + pace_step);
			if( *pt != *(pt + pace_step))			
				change_idx.push_back(i+1);
			pt += pace_step;	
		}

		if(change_idx.size()>=6)//才有可能是色标区域
		{
			for(int k=0;k<change_idx.size()-1;k++)
				diff.push_back(change_idx[k+1]-change_idx[k]);

			for(int k=0;k<diff.size()-1;k++)
				diff[k] = abs(diff[k+1]-diff[k]);

			diff.pop_back();//delete the last value

			for(int k=0;k<diff.size()-3;k++)
			{
				if((diff[k]+diff[k+1]+diff[k+2]+diff[k+3])/4<Threshold)
				{
					sum += change_idx[k+5]-change_idx[k];
					num++;					
					if(num == 1)
					{
						left = j;
						for(int k=head;k<end;k++)
							*(erode->imageData+k*erode->widthStep+left)=0;
					}
					else
					{
						right = j;
						for(int k=head;k<end;k++)
							*(erode->imageData+k*erode->widthStep+right)=0;
					}
					break;
				}
			}
		}
		change_idx.clear();
		diff.clear();

	}
	if(num != 0)
	{
		double height = 1.0*sum/num;//这样才可以保留住height的小数部分
		double distance = 250*focus/height;
		double theta = asin(height/(340-abs(right-left)));


		cout << "左起点 " << left << "   右起点 " << right << endl << endl;
		cout << "到机器人的距离为 " << distance << " mm" << endl;
		cout << "角度为 " << theta << "rad"<< endl;
	}





	cvShowImage("final",erode);
	cvSaveImage("lined.jpg",erode);

	cvWaitKey(0);
	cvReleaseImage(&imgGrey);
	cvReleaseImage(&smoothImg);	
	cvReleaseImage(&binaryImg); 
	cvReleaseImage(&dilate);
	cvReleaseImage(&erode);
	cvDestroyWindow("final");

	return 0;

	
}