从0开始的智能车代码（4）

环岛是处理起来相对比较困难的一种赛道元素。条条大路通环岛，解决环岛问题也有许多不同的方案。在我参考过环岛的代码时，复杂的代码逻辑和庞大的代码量使我望而却步，见到巨人的肩膀魁梧却布满荆棘，我决定自己动手丰衣足食。我的方法是 MELP 四步过环岛，即遇环（Meet）、入环（Enter）、出环（Leave）、过环（Pass）。

首先，在寻找左右边线时留意边线横坐标（所在列数）突然变化的点，我将这种点称为跳变点。我将跳变点分为两类，赛道交汇形成的跳变点称为 A 字跳变点，赛道分支形成的跳变点称为 V 字跳变点。所有跳变点可以分成左侧 A 字跳变点、左侧 V 字跳变点、右侧 A 字跳变点、右侧 V 字跳变点这4种。

遇环：图像中出现 A 字跳变点，说明小车遇到一个圆环。此时将遇环标志位置1，同时记录下圆环的方向。

入环：在遇环标志位为1的情况下，图像中出现 V 字跳变点，说明小车即将驶入圆环。此时将入环标志位置1。根据遇环时记录下的圆环方向进行补线：若圆环为左侧环，将跳变点与图像右下角连线作为右边线；若圆环为右侧环，将跳变点与图像左下角连线作为左边线。

出环：在入环标志位为1的情况下，图像中出现 A 字跳变点，说明小车即将驶出圆环。此时将出环标志位置1。根据遇环时记录下的圆环方向进行补线：若圆环为左侧环，将跳变点与图像顶部中点相连作为右边线；若圆环为右侧环，将跳变点与图像顶部中点连线作为左边线。

过环：与入环的情况相反。在出环标志位为1的情况下，图像中出现 V 字跳变点，说明小车即将通过圆环。此时将遇环标志位、入环标志位、出环标志位清0。根据遇环时记录下的圆环方向进行补线：若圆环为左侧环，将跳变点与图像左下角连线作为左边线；若圆环为右侧环，将跳变点与图像右下角连线作为右边线。

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 leaveRingFlag=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;
  
  uint16 pointTMX=MT9V032_W/2;
  uint16 pointTMY=0;
  
  float stepLength;//横坐标插值步长
  
  if(jumpFlagL^jumpFlagR){//若有一侧出现跳变点
    if(pointType==1){//若为A字跳变点，说明出环或遇环
      if(enterRingFlag==1){//出环
        leaveRingFlag=1;
        if(ringSide==1){//环岛在左边，从顶部中点到出环标志点连线，补右边线
          stepLength=(float)(pointTMX-pointX)/(float)(pointTMY-pointY);
          for(i=0;i<pointTMY-pointY;i++){
            edgeR[pointTMY-i]=pointTMX-(int)(i*stepLength);
            binaryImage[pointTMY-i][edgeR[pointTMY-i]]=binaryImage[pointTMY-i][edgeR[pointTMY-i]-1]=binaryImage[pointTMY-i][edgeR[pointTMY-i]+1]=0;//粗线
          }
        }
        else if(ringSide==2){//环岛在右边，从顶部中点到出环标志点连线，补左边线
          stepLength=(float)(pointX-pointTMX)/(float)(pointTMY-pointY);
          for(i=0;i<pointTMY-pointY;i++){
            edgeL[pointTMY-i]=pointTMX+(int)(i*stepLength);
            binaryImage[pointTMY-i][edgeL[pointTMY-i]]=binaryImage[pointTMY-i][edgeL[pointTMY-i]-1]=binaryImage[pointTMY-i][edgeL[pointTMY-i]+1]=0;//粗线
          }
        }
      }
      else if(meetRingFlag==0){//遇环
        meetRingFlag=1;
        ringSide=pointSide;
      }
      //目前A字跳变点可以自动处理，无需补线
    }
    else if(pointType==2){//若为V字跳变点，说明入环或过环
      if(leaveRingFlag==1){//过环//与入环相反//须先判断过环
        meetRingFlag=0;
        enterRingFlag=0;
        leaveRingFlag=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;//粗线
  }
  
  lcd_showint8(0,4,jumpFlagL^jumpFlagR);
  lcd_showint8(0,5,ringSide);
  lcd_showint8(0,6,meetRingFlag*100+enterRingFlag*10+leaveRingFlag);
  
  offset=MT9V032_W/2-midline[98];
}

添加环岛处理后，斜入十字时图像中会出现一个 A 字跳变点，将十字误判成环岛，我设想的解决方案在检测到跳变点后在另一侧的更大范围内判断丢线情况，若出现较大范围丢线则可判断为十字。