nao機器人避障邊緣檢測程式碼
阿新 • • 發佈：2019-02-16
標頭檔案.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.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;
}