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

头文件.h

//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.1void 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.81AL::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);}}}//结束计算LTGint 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);}}}//结束计算LTGint 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<eelse{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>ebest_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计算Kdouble 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;elsereturn 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 matricesCV_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 matricesCV_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));//转换成角度和距离，从底部开始扫描到100double 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;}