從0開始的智慧車程式碼（3）
阿新 • • 發佈：2020-07-25
環島是處理起來相對比較困難的一種賽道元素。條條大路通環島，解決環島問題也有許多不同的方案。在我參考過環島的程式碼時，複雜的程式碼邏輯和龐大的程式碼量使我望而卻步，見到巨人的肩膀魁梧卻佈滿荊棘，我決定自己動手豐衣足食。我的方法是 MELP 四步過環島，即遇環（Meet）、入環（Enter）、出環（Leave）、過環（Pass）。
首先，在尋找左右邊線時留意邊線橫座標（所在列數）突然變化的點，我將這種點稱為跳變點。我將跳變點分為兩類，賽道交匯形成的跳變點稱為 A 字跳變點，賽道分支形成的跳變點稱為 V 字跳變點。所有跳變點可以分成左側 A 字跳變點、左側 V 字跳變點、右側 A 字跳變點、右側 V 字跳變點這4種。
void imageProcess(void){ uint16 i,j; uint16 edgeL[MT9V032_H]; uint16 edgeR[MT9V032_H]; uint16 midline[MT9V032_H]; edgeL[MT9V032_H-1]=0; edgeR[MT9V032_H-1]=MT9V032_W-1; midline[MT9V032_H-1]=MT9V032_W/2; uint8 jumpFlagL=0; uint8 jumpFlagR=0; uint16 pointX=0;//跳變點X座標 uint16 pointY=0;//跳變點Y座標uint8 pointSide=0;//跳變點方向，1表示左，2表示右 uint8 pointType=0;//跳變點分類，1表示A字跳變點，2表示V字跳變點 for(i=MT9V032_H-2;;i--){ edgeL[i]=0; edgeR[i]=MT9V032_W-1; for(j=midline[i+1];j>0;j--){ if(binaryImage[i][j]==0){ edgeL[i]=j; break; } } for(j=midline[i+1];j<MT9V032_W-1;j++){ if(binaryImage[i][j]==0){ edgeR[i]=j; break; } } //環島處理開始（見到巨人的肩膀魁梧卻佈滿荊棘，我決定自己動手豐衣足食） //環島處理所有數都是非負數 //bingo：和十字區分：掃描到一側的跳變點後（下半部分），對另一側的邊線進行判斷（下3/4部分） //尋找跳變點 if(i>MT9V032_H*1/3){//只判斷下2/3部分影象的跳變點（賽道突然變窄） //左側A字跳變點 if(edgeL[i+1]-edgeL[i]>20){ jumpFlagL=1; pointX=edgeL[i+1]; pointY=i+1; pointSide=1; pointType=1; } //左側V字跳變點 else if(edgeL[i]-edgeL[i+1]>20){ jumpFlagL=1; pointX=edgeL[i]; pointY=i; pointSide=1; pointType=2; } //右側A字跳變點 if(edgeR[i]-edgeR[i+1]>20){ jumpFlagR=1; pointX=edgeR[i+1]; pointY=i+1; pointSide=2; pointType=1; } //右側V字跳變點 else if(edgeR[i+1]-edgeR[i]>20){ jumpFlagR=1; pointX=edgeR[i]; pointY=i; pointSide=2; pointType=2; } } midline[i]=(edgeL[i]+edgeR[i])/2;//考慮環島補線後不能邊判斷邊線邊計算中線，這個中線只用來下一步尋找邊界 if(i==0){ break; } } //四步過環，遇環、入環、出環、過環 static uint8 meetRingFlag=0;//遇環標誌位 static uint8 enterRingFlag=0;//入環標誌位 static uint8 exitRingFlag=0;//出環標誌位 static uint8 ringSide=0;//環島型別，1表示左，2表示右 uint16 pointLLCX=0; uint16 pointLLCY=MT9V032_H-1; uint16 pointLRCX=MT9V032_W-1; uint16 pointLRCY=MT9V032_H-1; float stepLength;//橫座標插值步長 if(jumpFlagL^jumpFlagR){//若有一側出現跳變點 if(pointType==1){//若為A字跳變點，說明出環或遇環 if(enterRingFlag==1){//出環 exitRingFlag=1; } else if(meetRingFlag==0){//遇環 meetRingFlag=1; ringSide=pointSide; } //目前A字跳變點可以自動處理，無需補線 } else if(pointType==2){//若為V字跳變點，說明入環或過環 if(exitRingFlag==1){//過環//與入環相反//須先判斷過環 meetRingFlag=0; enterRingFlag=0; exitRingFlag=0; if(ringSide==2){ stepLength=(float)(pointLRCX-pointX)/(float)(pointLRCY-pointY); for(i=0;i<pointLRCY-pointY;i++){ edgeR[pointLRCY-i]=pointLRCX-(int)(i*stepLength); binaryImage[pointLRCY-i][edgeR[pointLRCY-i]]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]-1]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]+1]=0;//粗線 } } else if(ringSide==1){ stepLength=(float)(pointX-pointLLCX)/(float)(pointLLCY-pointY); for(i=0;i<pointLLCY-pointY;i++){ edgeL[pointLLCY-i]=pointLLCX+(int)(i*stepLength); binaryImage[pointLLCY-i][edgeL[pointLLCY-i]]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]-1]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]+1]=0;//粗線 } } } else if(meetRingFlag==1){//入環 enterRingFlag=1; //入環補線 //認為跳變點方向為圓環方向 //入環標誌點（pointAX，pointAY），影象左下角（pointLLCX，pointLLCY），影象右下角（pointLRCX，pointLRCY） if(ringSide==1){//入環標誌點在左邊，從右下角點到入環標誌點連線，補右邊線 stepLength=(float)(pointLRCX-pointX)/(float)(pointLRCY-pointY); for(i=0;i<pointLRCY-pointY;i++){ edgeR[pointLRCY-i]=pointLRCX-(int)(i*stepLength); binaryImage[pointLRCY-i][edgeR[pointLRCY-i]]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]-1]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]+1]=0;//粗線 } } else if(ringSide==2){//入環標誌點在右邊，從左下角點到入環標誌點連線，補左邊線 stepLength=(float)(pointX-pointLLCX)/(float)(pointLLCY-pointY); for(i=0;i<pointLLCY-pointY;i++){ edgeL[pointLLCY-i]=pointLLCX+(int)(i*stepLength); binaryImage[pointLLCY-i][edgeL[pointLLCY-i]]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]-1]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]+1]=0;//粗線 } } } } } for(i=0;i<MT9V032_H;i++){//重新計算中線 midline[i]=(edgeL[i]+edgeR[i])/2; binaryImage[i][midline[i]]=binaryImage[i][midline[i]-1]=binaryImage[i][midline[i]+1]=0;//粗線 } offset=MT9V032_W/2-midline[98]; }