nao机器人避障边缘检测代码

头文件.h

//common.h
#ifndef COMMON_MY_H
#define COMMON_MY_H

#include 
#include 
#include 
#include 
#include 
using namespace std;

#include 
#include 
#include 
#include 
using namespace AL;

#include 
#include 
#include 
using namespace cv;

#include 
#include 

#define M_E	 2.718281828459
#define M_PI 3.1415926
#define M_2PI (3.1415926*2)
#define ZERO_DOUBLE 0.000000001
#define ZERO_CONTINE 0.1

void testBlue();
double ComputeTheta();//计算边线斜率角;
void testMouseAndHandle();
void testMove();
void testAction();
Mat& ScanImageAndReduceIterator(Mat& I, const uchar* const table);
Mat& TestNotGreen(Mat& I);
Mat& TestWhite(Mat& I);
int computePixel(double theta);
void testPixel(Mat &img,int col,int row);
void on_mouse( int event, int x, int y, int flags, void* ustc);
void testAction();
void Mat2Array(cv::Mat& img);
void bz();

struct Dis_flag
{
	double distance;
	int flag;//1表示白色，0表示蓝色,-1表示绿色
};

extern cv::Mat img;
extern double obstacles[320];//从60度开始扫描
extern Dis_flag obstacles2[320];


struct FreeDirectionNode
{
	int start;
	int end;
};

typedef FreeDirectionNode CollisionDirectionNode;

#else


#endif

功能函数实现

//bz.cpp

#include "common.h"

void bz()
{
	//FILE* fp = fopen("test.txt","a+");\\192.168.1.81
	AL::ALMotionProxy motion("192.168.1.81", 9559);
	const std::string names = "Body";
	float stiffnessLists = 1.0f;
	float timeLists      = 1.0f;
	motion.stiffnessInterpolation(names, stiffnessLists, timeLists);

	ALVideoDeviceProxy camProxy("192.168.1.81", 9559);
	string clientName;
	//camProxy.unsubscribeAllInstances("test");
	clientName = camProxy.subscribe("test",kQVGA, kBGRColorSpace, 30);
	cout< freeDirectionPoints(n);
		int num_freeDirectionPoints = 0;//自由方向的数目
		for(int i = 0; i < n ;i++)
		{
			if(obstacles2[i].distance - collision_distance > ZERO_DOUBLE )
			{
				num_freeDirectionPoints++;
				freeDirectionPoints[i] = 0;//标记为自由方向
			}
			else
			{
				freeDirectionPoints[i] = 1;
			}
		}

		if(mode && num_freeDirectionPoints == n)//直行模式且未发生碰撞
		{
			//生成局部切线图(LTG)
			double m_F = obstacles2[i_d%n].distance;//目标方向上的自由距离
			double m_d = 5.0f;//当前机器人与目标的距离
			vector idx;
			bool isInsert = false;
			for(int i = 0; i < n ;isInsert = false,i++)
			{
				if(fabs(obstacles2[i].distance - sensor_range) < ZERO_DOUBLE &&//剔除探测边界点
					fabs(obstacles2[(i+1)%n].distance - sensor_range) < ZERO_DOUBLE && 
					fabs(obstacles2[(i-1+n)%n].distance -sensor_range) < ZERO_DOUBLE )
				{
					isInsert = true;
				}
				if(!isInsert && fabs(obstacles2[i].distance - sensor_range) > ZERO_DOUBLE && 
					((fabs(obstacles2[(i+1)%n].distance - obstacles2[i].distance) > 0.2 && 
					fabs(obstacles2[(i-1+n)%n].distance - obstacles2[i].distance) < 0.2) || 
					(fabs(obstacles2[(i+1)%n].distance-obstacles2[i].distance) < 0.2 &&
					fabs(obstacles2[(i-1+n)%n].distance-obstacles2[i].distance) > 0.2)))//检测不连续
				{
					idx.push_back(i);
					isInsert = true;
				}
				if(!isInsert && (fabs(obstacles2[i].distance - sensor_range) < ZERO_DOUBLE))//检测与障碍物的交点
				{
					bool flag_min = true;//标记是否逐渐变小
					for(int j = 0; j < 10 && flag_min; j++)//假设连续10个变小就表示逐渐变小
					{
						if(obstacles2[(i+1+j)%n].distance > obstacles2[(i+j)%n].distance)
							flag_min = false;
					}
					if(flag_min)
					{
						idx.push_back(i);
						isInsert = true;
					}
				}
				if(!isInsert && (fabs(obstacles2[i%n].distance - sensor_range) < ZERO_DOUBLE))
				{
					bool flag_max = true;//标记是否逐渐变大
					for(int j = -9; j < 1 && flag_max; j++)//假设前面连续10个逐渐变大
					{
						if(obstacles2[(i+j+n)%n].distance > obstacles2[(i+j-1+n)%n].distance)
							flag_max = false;
					}
					if(flag_max)
					{
						idx.push_back(i);
					}
				}
			}//结束计算LTG
			int num_node = idx.size();//LTG点数目
			printf("num_node:%d\n",num_node);
			//imshow("images", imgHeader);
			//cvWaitKey(1);
			if(fabs( m_F - sensor_range) < ZERO_DOUBLE )//目标方向上无障碍
			{
				//fprintf(fp,"i_d: %d\n",i_d);
				best_angle = i_d;
			}
			else//目标方向上有障碍
			{
				//fprintf(fp,"num_node: %d\n",num_node);
				if(num_node > 0)//找到LTG
				{
					printf("num_node:%d\n",num_node);
					int min_distance = 0;//用于记录最小偏差
					int best_i = 0;
					int temp = 0;
					for(int i = 0; i < num_node; i++)
					{
						//计算与目标方向最一致的方向
						temp = abs(idx[i]-n/2);
						if(i == 0)
						{
							min_distance = temp;
						}
						else
						{
							if(min_distance > temp  )
							{
								best_i = idx[i];
								min_distance = temp;
							}
						}
					}
					best_angle = best_i;
					//fprintf(fp,"best_i: %d\n",best_i);
				}
				else//这种情况不可能出现
				{
					printf("num_node = 0\n");
					best_angle = 0.0;
				}//结束未发生碰撞时找到LTG
			}//结束未发生碰撞时目标方向有障碍物
		}//结束未发生碰撞
		else//发生碰撞
		{
			double theta= ComputeTheta();
			if (theta < 90 )
			{
				best_angle = 0;
			}
			else
			{
				best_angle = 319;
			}
			//break;
			//计算自由方向弧
			/*vector freeDirectionArcs;
			vector collisionDirectionArcs;
			bool isFirst = true;
			for(int i = 0,j = 0; j < n; j++)
			{
				if(abs(freeDirectionPoints[(j+1)%n] - freeDirectionPoints[j]) == 1)
				{
					if(freeDirectionPoints[j] == 0)
					{
						FreeDirectionNode fdnode={i,j};
						freeDirectionArcs.push_back(fdnode);
					}
					else
					{
						CollisionDirectionNode cdnode={i,j};
						collisionDirectionArcs.push_back(cdnode);
					}
					i = (j+1)%n;
				}
			}
			int num_freeDirectionArcs = freeDirectionArcs.size();
			if(num_freeDirectionArcs == 0)
			{
				printf("num_freeDirectionArcs = 0\n");
				motion.setWalkTargetVelocity(0.0f,0.0f,0.0f,0.5f);
				continue;
			}
			else//存在多条自由弧
			{
				printf("collisionDirectionArcs.size = %d\n",collisionDirectionArcs.size());
				if (collisionDirectionArcs.size() == 0)
				{
					printf("collisionDirectionArcs.size = 0\n");
				}
				else if (collisionDirectionArcs.size() == 1)
				{
					if (obstacles[collisionDirectionArcs[0].start] < obstacles[collisionDirectionArcs[0].end])//右边线
					{
						//best_angle=0;
						motion.setWalkTargetVelocity(0.0f,0.0f,0.5,0.5f);
						continue;
					}
					else//左边线
					{
						motion.setWalkTargetVelocity(0.0f,0.0f,-0.5,0.5f);
						continue;
						//best_angle=319;
					}
				}
			}
			//	memset(buf_step,0,256);
			//	sprintf(buf_step,"images\\%d.jpg",step++);
			//	imwrite(buf_step, imgHeader);

			//if(!TargetInRange)
			//{
				double reachFollow;
				int min_i = 0;
				bool FOUND = false;
				//在自由区域中扫描LTG得到最近点
				int i_d = n/2;
				double m_F = obstacles[i_d%n];//目标方向上的自由距离
				double m_d = 5.0f;//当前机器人与目标的距离
				vector idx;//存放交叉或连续方向
				bool isInsert = false;
				for(int i = 0; i < n ;isInsert = false,i++)
				{
					if(fabs(obstacles[i] - 1.0) < ZERO_DOUBLE &&//剔除探测边界点
						fabs(obstacles[(i+1)%n]-1.0) < ZERO_DOUBLE && 
						fabs(obstacles[(i-1+n)%n]-1.0) < ZERO_DOUBLE )
					{
						isInsert = true;
					}
					if(!isInsert && fabs(obstacles[i] - 1.0) > ZERO_DOUBLE && 
						(fabs(obstacles[(i+1)%n]-obstacles[i]) > 0.2 || 
						fabs(obstacles[(i-1+n)%n]-obstacles[i]) > 0.2))//检测不连续
					{
						idx.push_back(i);
						isInsert = true;
					}
					if(!isInsert && (fabs(obstacles[i] - 1.0) < ZERO_DOUBLE))//检测与障碍物的交点
					{
						bool flag_min = true;//标记是否逐渐变小
						for(int j = 0; j < 5 && flag_min; j++)//假设连续10个变小就表示逐渐变小
						{
							if(obstacles[(i+1+j)%n] > obstacles[(i+j)%n])
								flag_min = false;
						}
						if(flag_min)
						{
							idx.push_back(i);
							isInsert = true;
						}
					}
					if(!isInsert && (fabs(obstacles[i%n] - 1.0) < ZERO_DOUBLE))
					{
						bool flag_max = true;//标记是否逐渐变大
						for(int j = -4; j < 1 && flag_max; j++)//假设前面连续5个逐渐变大
						{
							if(obstacles[(i+j+n)%n] > obstacles[(i+j-1+n)%n])
								flag_max = false;
						}
						if(flag_max)
						{
							idx.push_back(i);
						}
					}
				}//结束计算LTG
				int num_node = idx.size();//LTG点数目

				if(num_node > 0)//找到LTG
				{
					int min_distance = 0;
					int best_i = 0;//保存最好的方向
					int temp = 0;
					//fprintf(fp,"start: %d\n",num_node);
					for(int i = 0; i < num_node; i++)
					{
						//计算最小距离
						temp = abs(idx[i]-n/2);
						if(i == 0)
						{
							min_distance = temp;
						}
						else
						{
							if(min_distance > temp )
							{
								best_i = idx[i];
								min_distance = temp;
							}
						}	
					}//扫描完LTG

					//reachFollow = min_distance;
					if((freeDirectionArcs[0].start)%n < freeDirectionArcs[0].end &&
						best_i > (freeDirectionArcs[0].start)%n &&
						best_i < freeDirectionArcs[0].end )// &&
						//abs(best_i-i_jiyi) < 40)//在自由区域方向上
					{
						min_i = best_i;
						FOUND = true;
						//reachMin = reachFollow;
					}
					if((freeDirectionArcs[0].start)%n > freeDirectionArcs[0].end &&
						(best_i > (freeDirectionArcs[0].start)%n ||
						best_i < freeDirectionArcs[0].end))//&&
						//abs(best_i-i_jiyi) < 40)
					{
						min_i = best_i;
						FOUND = true;
						//reachMin = reachFollow;
					}
				}
				else//这种情况不可能出现
				{
				}//结束未发生碰撞时找到LTG*/
	
	/*			if(FOUND)//找到了较小点
				{
					//fprintf(fp,"reachMin:%lf,min_i:%d\n",reachMin,min_i);
					//if(reachMin < 0.001)
					//TargetInRange = true;
					//朝这个点移动
					best_angle = min_i;
					//best_Speed = 1.0;
					mode = true;//切换为直行
					//擦除相对位置记录
					//	rel_x = 0.0;
					//	rel_y = 0.0;
				}
				else
				{
					//fprintf(fp,"not found,reachMin:%lf\n",reachMin);
					mode = false;//标记为绕行
					int i_best = 0;

					//计算绕行方向
					//往左边走的策略
					i_best = (freeDirectionArcs[0].start+30)%n;

					//基于绕行方向选择函数的方法
			    	int left=-1,right=-1;
					double dis_left=0.0,dis_right=0.0;
					int freedirection ;
					if(freeDirectionArcs[0].start < freeDirectionArcs[0].end)
					{
						freedirection = (freeDirectionArcs[0].start+freeDirectionArcs[0].end)/2;
						if(freedirection >=91 && freedirection < 181)//自由方向向上
						{
							//寻找最近可行区域
							bool IsFindD = false;
							for(int i = freedirection; i > 90 && right == -1; i--)//找右边
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i])>0.2)
								{
									right  = i;
								}
							}
								for(int i = freedirection; i < 181 && left == -1; i++)//找左边
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i])>0.2)
								{
									left  = i;
								}
							}
							if(left == right )
							{
								//墙在右边走固定策略
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else
							{
								if(left == -1 && right !=-1)
								{ 
									//墙在右边
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
								else if(left != -1 && right ==-1)
								{
									//墙在左边
									i_best = (freeDirectionArcs[0].end -30+n)%n;
								}
								else//暂不讨论
								{
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
							}

						}
						else//自由方向向下
						{
							//fprintf(fp,"freeStart:%d,freeEnd:%d\n",freeDirectionArcs[0].start,freeDirectionArcs[0].end);
							//寻找可行区域
							for(int i =  freeDirectionArcs[0].end; i > 90&& right ==-1 ; i--)
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i])>0.2)
								{
									right  = i;
								}
							}
							for(int i =  freeDirectionArcs[0].start; i < 181&& left == -1; i++)
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i])>0.2)
								{
									left  = i;
								}
							}
							if(left == right )
							{
								//墙在右边走固定策略
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else if(left != -1 && right == -1)
							{
								//墙在右边
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else if (left == -1 && right != -1)
							{
								//墙在左边
								i_best = (freeDirectionArcs[0].end-30+n)%n;
							}
							else
							{
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
						}
					}//s=91 && freedirection < 181)//自由方向向上
						{
							for(int i = freedirection; i > 90 && right == -1; i--)//找右边
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i%n])>0.2)
								{
									right  = i;
								}
							}
							for(int i = freedirection; i < 181 && left == -1; i++)//找左边
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i%n])>0.2)
								{
									left  = i;
								}
							}
							if(left == right )
							{
								//墙在右边走固定策略
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else
							{
								if(left == -1 && right !=-1)
								{
									//墙在右边
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
								else if(left != -1 && right ==-1)
								{
									//墙在左边
									i_best = (freeDirectionArcs[0].end - 30+n)%n;
								}
								else
								{
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
							}
						}//end 自由方向向上
						else
						{
							//	fprintf(fp,"start:%d,end:%d\n",freeDirectionArcs[0].start,freeDirectionArcs[0].end);
							int x = freeDirectionArcs[0].end;
							for(int i =  freeDirectionArcs[0].end+20; i >= 70 && right == -1; i--)
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i%n])>0.2)
								{
									right  = i%n;
								}
							}
							for(int i =  freeDirectionArcs[0].start; i < 160 && left == -1 ; i++)
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i%n])>0.2)
								{
									left  = i;
								}
							}
							if(left == right)
							{
								//墙在右边
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else if(left == -1 && right != -1)
							{
								//墙在左边
								i_best = (freeDirectionArcs[0].end-30+n)%n;
							}
							else
							{
								//墙在右边
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
						}//end 方向向下
					}//end  s>e
					best_angle = i_best;
				}//end FOUND绕行*/
			//}
		}//end 发生碰撞
		//	fclose(fp);
		if(best_angle <0 || best_angle >=320)
			best_angle  = 0;
		//best_angle = 160;
		desiredDirection = 60/360.0*M_2PI+best_angle*60/360.0*M_2PI/320;
		//desiredSpeed = 0.01;
		//记忆绕行方向
		//i_jiyi = best_angle;
		//3.运动
		//double desiredDirection = 60/360.0*M_2PI+160*60/360.0*M_2PI/320;
		motion.setWalkTargetVelocity(0.4*sin(desiredDirection),0.5*cos(desiredDirection),0.0f,0.5);
		//motion.setWalkTargetVelocity(0.0,0.0,-0.5,0.5f);
		//motion.moveTo(sin(desiredDirection),cos(desiredDirection),0.0f);
		//motion.setWalkTargetVelocity(0.1,sin(desiredDirection)*0.0,0.0f,0.5);
	}

	motion.setWalkTargetVelocity(0,0,0.0f,0.5);
	//	fclose(fp);
	camProxy.unsubscribe(clientName);
	stiffnessLists = 0.0f;
//	motion.stiffnessInterpolation(names, stiffnessLists, timeLists);
	//cout<<"end"<

测试代码

//tools.cpp
#include "common.h"

void testBlue(Mat&I)
{
	MatIterator_ it, end;
	for( it = I.begin(), end = I.end(); it != end; ++it)
	{
		if (((*it)[0]< 190 &&  (*it)[0] > 75 )|| ((*it)[1] < 200 && (*it)[1] > 80)||((*it)[2] < 80 && (*it)[2] > 30))
		{
			(*it)[0] = 0;
			(*it)[1] = 0;
			(*it)[2] = 0;
		}
	}
}

double ComputeTheta()//计算边线theta
{
	// 寻找最近两个点
	double min_distance = obstacles2[0].distance;
	int i_min = 0;
	for (int i = 1; i < 319; i++)
	{
		if ( (obstacles2[i].flag == 0 && obstacles2[i].flag == 0)//都为障碍物
			&& fabs(obstacles2[i].distance - obstacles2[(i+1)].distance) < ZERO_CONTINE && obstacles2[i].distance < min_distance)
		{
			min_distance = obstacles2[i].distance;
			i_min = i;
		}
	}
	cout<<"i_min:"< it, end;
			for( it = I.begin(), end = I.end(); it != end; ++it)
				*it = table[*it];
			break;
		}
	case 3:
		{
			MatIterator_ it, end;
			for( it = I.begin(), end = I.end(); it != end; ++it)
			{
				(*it)[0] = table[(*it)[0]];
				(*it)[1] = table[(*it)[1]];
				(*it)[2] = table[(*it)[2]];
			}
		}
	}
	return I;
}

Mat& TestNotGreen(Mat& I)
{
	CV_Assert(I.depth() != sizeof(uchar));
	const int channels = I.channels();
	switch(channels)
	{
	case 1:
		{
			MatIterator_ it, end;
			for( it = I.begin(), end = I.end(); it != end; ++it)
				if(*it != 255) *it = 0;
			break;
		}
	case 3:
		{
			MatIterator_ it, end;
			for( it = I.begin(), end = I.end(); it != end; ++it)
			{
				if (((*it)[0]< 190 &&  (*it)[0] > 75 )|| ((*it)[1] < 200 && (*it)[1] > 80)||((*it)[2] < 80 && (*it)[2] > 30))
				{
					(*it)[0] = 0;
					(*it)[1] = 0;
					(*it)[2] = 0;
				}
			}
		}
	}
	blur(I,I,Size(3,3));
	return I;
}

Mat& TestWhite(Mat& I)
{
	// accept only char type matrices
	CV_Assert(I.depth() != sizeof(uchar));
	const int channels = I.channels();
	switch(channels)
	{
	case 1:
		{
			MatIterator_ it, end;
			for( it = I.begin(), end = I.end(); it != end; ++it)
				if(*it != 255) *it = 0;
			break;
		}
	case 3:
		{
			MatIterator_ it, end;
			for( it = I.begin(), end = I.end(); it != end; ++it)
			{
				if ((*it)[0]< 235 || (*it)[1] < 235 ||(*it)[2] < 235)
				{
					(*it)[0] = 0;
					(*it)[1] = 0;
					(*it)[2] = 0;
				}
			}
		}
	}
	return I;
}

int computePixel(double theta)
{
	double m = 1.0/tan(theta-60.0/360*2*3.1415926)*sin(60.97/360*2*3.1415926)-cos(60.97/360*2*3.1415926);
	return (320-int(320.0/(1+m)))%320;
}

/************************************************************************/
/* name:   
/* purpose: 利用图像得到距离机器人的扇形区域内障碍物的角度和距离，将结果放在obstacles数组中,并且把原图像也改变
/* paramter: 图像矩阵
/* return:                         6                                        */
/************************************************************************/
void Mat2Array(cv::Mat& img)
{
	//检测非绿色
	//TestNotGreen(img);
	//检测白色
	//TestWhite(img);
	//imshow("white",img);
	//平滑滤波
//	blur(img,img,Size(3,3));
	//转换成角度和距离，从底部开始扫描到100
	double k0 = 60/360.0*6.28,k=0.0;
	double incr =60/360.0*6.28/320;
	for (int i = 0; i < 320; i++)
	{
		k = k0+i*incr;
		int x = computePixel(k);
		bool flag = false;
		for (int j=239; j>=100 && !flag;j--)
		{
			uchar b = *(img.data + img.step[0] * j + img.step[1] * x);
			uchar g =*(img.data + img.step[0] * j + img.step[1] * x+1);
			uchar r =*(img.data + img.step[0] * j + img.step[1] * x+2);
			if(b< 235  || g < 235  || r < 235 )//非白色
			{
				if ((b< 235 && b >110) || (g < 235 && g > 105) || (r < 235 && r > 85))//绿色地毯
				{
					double k = 100/240.0;
					double theta = atan(0.3/0.48)*360/6.28;
					double shang = k*sin(theta/360*6.28)+(1-k)*sin((47.64+theta)/360*6.28);
					double xia = k*cos(theta/360*6.28)+(1-k)*cos((47.64+theta)/360*6.28);
					obstacles2[i].flag = -1;//绿色
					obstacles2[i].distance= 0.48*shang/xia*pow(M_E,fabs(k-1.0));
					*(img.data + img.step[0] * j + img.step[1] * x) = 0;
					*(img.data + img.step[0] * j + img.step[1] * x+1)=255;
					*(img.data + img.step[0] * j + img.step[1] * x+2)=0;
				}
				else//蓝色障碍物
				{
					double k = j/240.0;
					double theta = atan(0.3/0.48)*360/6.28;
					double shang = k*sin(theta/360*6.28)+(1-k)*sin((47.64+theta)/360*6.28);
					double xia = k*cos(theta/360*6.28)+(1-k)*cos((47.64+theta)/360*6.28);
					obstacles2[i].distance= 0.48*shang/xia*pow(M_E,fabs(k-1.0));
					obstacles2[i].flag = 0;//蓝色
					*(img.data + img.step[0] * j + img.step[1] * x) = 255;
					*(img.data + img.step[0] * j + img.step[1] * x+1)=0;
					*(img.data + img.step[0] * j + img.step[1] * x+2)=0;
					flag = true;
				}
			}
			else//白色边线
			{
				double k = j/240.0;
				double theta = atan(0.3/0.48)*360/6.28;
				double shang = k*sin(theta/360*6.28)+(1-k)*sin((47.64+theta)/360*6.28);
				double xia = k*cos(theta/360*6.28)+(1-k)*cos((47.64+theta)/360*6.28);
				obstacles2[i].distance= 0.48*shang/xia*pow(M_E,fabs(k-1.0));
				obstacles2[i].flag = 1;//白色

				*(img.data + img.step[0] * j + img.step[1] * x) = 255;
				*(img.data + img.step[0] * j + img.step[1] * x+1) =255;
				*(img.data + img.step[0] * j + img.step[1] * x+2) = 255;
				flag = true;
			}
		}
	}
	
	for (int l=0; l < 320; l++)
	{
		printf("%lf\t",obstacles2[l].distance);
	}
	printf("\n");

	// 	//减少颜色空间
	// 	uchar table[256];
	// 	int divideWith=30; 
	//for (int i = 0; i < 256; ++i)
	//	table[i] = divideWith* (i/divideWith);
	// 	ScanImageAndReduceIterator(img,table);

}

void testPixel(Mat &img,int col,int row)
{
 	uchar b = *(img.data + img.step[0] * row + img.step[1] * col);
	uchar g =*(img.data + img.step[0] * row + img.step[1] * col+1);
 	uchar r =*(img.data + img.step[0] * row + img.step[1] * col+2);
//	assert(fp != NULL && "fp is NULL");
	FILE* fp = fopen("color.txt","a+");
	fprintf(fp,"%d,%d,r:%d,g:%d,b:%d\n",img.step[0],img.step[1],r,g,b);
	fclose(fp);
//	fprintf(fp,"%d,%d,r:%d,g:%d,b:%d\n",img.step[0],img.step[1],r,g,b);
}

void on_mouse( int event, int x, int y, int flags, void* ustc)  
{  
	CvFont font;  
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, CV_AA);  
	//cout<<"rows:"<

主函数

//test.cpp
#include "common.h"

cv::Mat img;
double obstacles[320];//从60度开始扫描
Dis_flag obstacles2[320];

int main()
{
	try {
		//testMove();
		bz();
		//testMouseAndHandle();
	}
	catch (const AL::ALError& e) {
		std::cerr << "Caught exception: " << e.what() << std::endl;
		exit(1);
	}
	exit(0);
	return 0;
}