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

//common.h
#ifndef COMMON_MY_H
#define COMMON_MY_H

#include <iostream>
#include <alerror/alerror.h>
#include <alproxies/almotionproxy.h>
#include <vector>
#include <string>
using namespace std;

#include <alproxies/alvideodeviceproxy.h>
#include <alvision/alimage.h>
#include <alvision/alvisiondefinitions.h>
#include <alerror/alerror.h>
using namespace AL;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;

#include <Windows.h>
#include <time.h>

#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<<clientName.data()<<endl;
camProxy.setActiveCamera(1);
cv::Mat imgHeader = cv::Mat(cv::Size(320, 240), CV_8UC3);
qi::os::msleep(1000);//等摄像头发布数据
int step = 0;
char buf_step[256];
memset(buf_step,0,256);
sprintf(buf_step,"%d.jpg",step);

//终点的相对方向
int n = 320;//感知精度
int i_d=n/2;//目标在正前方
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);//计算距离公式的下面
double sensor_range=0.48*shang/xia*pow(M_E,fabs(k-1.0));//最大感知距离，扫描像素值100对应的距离
double sensor_angle = 60.97/360*6.28;//感知角度范围
double collision_distance = 0.5;
double desiredDirection = 0;//方向在（M_PI/3,M_2PI/3）间
clientName = camProxy.subscribe("test",kQVGA, kBGRColorSpace, 30);
camProxy.setActiveCamera(1);
int i = 0;
while(waitKey(1) != 27)
{
//1.得到障碍物和边线信息
//1.1获取图像
ALValue img = camProxy.getImageRemote(clientName);
imgHeader.data = (uchar*) img[6].GetBinary();
//1.2显示图像
//imgHeader = imread("images\\24.jpg");
imshow("images", imgHeader);
//1.3 保存图像
//memset(buf_step,0,256);
//sprintf(buf_step,"rx_images1\\%d.jpg",step++);
//imwrite(buf_step, imgHeader);
camProxy.releaseImage(clientName);
//1.4转换为最近距离
Mat2Array(imgHeader);//输出为obstacles数组
//2.局部路径规划
//		imshow("changed",imgHeader);

static bool mode = true;//直行模式
int best_angle;

//检测碰撞
vector<int> 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<int> 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<FreeDirectionNode> freeDirectionArcs;
vector<CollisionDirectionNode> 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<int> 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<e
else
{
freedirection = (freeDirectionArcs[0].start+freeDirectionArcs[0].end+n)/2%n;
if(freedirection >=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"<<endl;
}

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

void testBlue(Mat&I)
{
MatIterator_<Vec3b> it, end;
for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); 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:"<<i_min<<endl;
// 用i_min和i_min+1计算K
double i_x,i_y,i1_x,i1_y,theta_t;
theta_t = M_PI/3+M_PI/3/320*i_min;
i_x = obstacles2[i_min].distance*cos(theta_t);
i_y = obstacles2[i_min].distance*sin(theta_t);
theta_t = M_PI/3+M_PI/3/320*(i_min+1);
i1_x = obstacles2[i_min+1].distance*cos(theta_t);
i1_y = obstacles2[i_min+1].distance*sin(theta_t);
if(atan2(i1_y-i_y,i1_x-i_x)*180/M_PI < ZERO_DOUBLE)
return atan2(i1_y-i_y,i1_x-i_x)*180/M_PI+180;
else
return atan2(i1_y-i_y,i1_x-i_x)*180/M_PI;

}

int ComputeAverage()
{
int n = 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 )
{
n++;
}
}
return 0;
}

void testMouseAndHandle()
{
img = imread("rx_images\\22.jpg");
imshow("src",img);
//cout<<"rows:"<<img.rows<<"cols:"<<img.cols<<endl;
setMouseCallback("src", on_mouse, 0 );
Mat2Array(img);
//blur(img,img,Size(3,3));
imshow("changed",img);
//cout<<ComputeTheta()<<endl;
//setMouseCallback("src", on_mouse, 0 );
while(cvWaitKey(10) != 27)
{
imshow("changed",img);
}
}

void testMove()
{
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);
char c  = 0;
clock_t start=clock();
cvNamedWindow("test");
motion.setWalkTargetVelocity(0.0f,0.4f,0.0f,0.3f);
while(c!=27)
{
c=cvWaitKey(1);
}
clock_t end = clock();
FILE* fp = fopen("time.txt","w+");
fprintf(fp,"%d\n",end-start);
fclose(fp);
motion.setWalkTargetVelocity(0.0f,0.0f,0.0f,0.3f);

}

void testAction()
{
AL::ALMotionProxy motion("192.168.1.103", 9559);
const std::string names = "Body";
float stiffnessLists = 1.0f;
float timeLists      = 1.0f;
motion.stiffnessInterpolation(names, stiffnessLists, timeLists);
motion.setWalkTargetVelocity(0.2f,0.2f,0.0f,0.5f);
while(1);
stiffnessLists = 0.0f;
motion.stiffnessInterpolation(names, stiffnessLists, timeLists);
}

Mat& ScanImageAndReduceIterator(Mat& I, const uchar* const table)
{
// accept only char type matrices
CV_Assert(I.depth() != sizeof(uchar));
const int channels = I.channels();
switch(channels)
{
case 1:
{
MatIterator_<uchar> it, end;
for( it = I.begin<uchar>(), end = I.end<uchar>(); it != end; ++it)
*it = table[*it];
break;
}
case 3:
{
MatIterator_<Vec3b> it, end;
for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); 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_<uchar> it, end;
for( it = I.begin<uchar>(), end = I.end<uchar>(); it != end; ++it)
if(*it != 255) *it = 0;
break;
}
case 3:
{
MatIterator_<Vec3b> it, end;
for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); 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_<uchar> it, end;
for( it = I.begin<uchar>(), end = I.end<uchar>(); it != end; ++it)
if(*it != 255) *it = 0;
break;
}
case 3:
{
MatIterator_<Vec3b> it, end;
for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); 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:"<<img.rows<<"cols:"<<img.cols<<endl;
if( event == CV_EVENT_LBUTTONDOWN )
{
CvPoint pt = cvPoint(x,y);
printf("(x:%d,y:%d)",pt.x,pt.y);
//cout<<"rows:"<<img.rows<<"cols:"<<img.cols<<endl;
testPixel(img,x,y);
//cv::putText(img,temp, pt, FONT_HERSHEY_SIMPLEX, 0.3f, cv::Scalar(255, 255, 255, 0));
//cv::circle( img, pt, 2,cv::Scalar(255,0,0,0) ,CV_FILLED, CV_AA, 0 );
}
}

//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;
}