polar_match.cpp

利用极坐标系统对机器人所携带的激光传感器所测得的数据进行匹配

  {

    if( (fabs(dx)+fabs(dy)+fabs(dth)*PM_R2D)<0.1 )
      small_corr_cnt++;
    else
      small_corr_cnt=0;

    #ifdef  PM_GENERATE_RESULTS
       end_tick =rdtsc();
       fprintf(f,"%i %lf %lf %lf %lf\n",iter,
          (double)(end_tick-start_tick-dead_tick)*1000.0/CPU_FREQ,ax,ay,ath*PM_R2D);
       end_tick2 =rdtsc();
       dead_tick += end_tick2- end_tick;
    #endif

    #ifdef GR
      dr_erase();
      dr_circle(ax,ay,5.0,"green");
      dr_line(0,-100,200,-100,"black");
      dr_line(0,-200,200,-200,"black");
    #endif
    act.rx = ax;act.ry = ay;act.th = ath;
    co = cos(ath);
    si = sin(ath);

    //scan projection


    for(i=0;i<PM_L_POINTS;i++)
      new_r[i] = 20000;//initializing "z" buffer for occlusion check

    
    int r0,r1,fi0,fi1;//for occlusion detection purpose
    //transformation of points:
    for(i=0;i<PM_L_POINTS;i++)
    {
      x         = act.x[i]*co - act.y[i]*si + ax;
      y         = act.x[i]*si + act.y[i]*co + ay;
      nx[i]     = x;
      ny[i]     = y;
      new_bad[i]= act.bad[i];      
      r[i]      = sqrt(x*x+y*y);
      fi[i]     = atan2(y,x);

      //fill up new_r -> contains the smallest range of actual scan in ref.frame
      // for detecting occlusion of the actual scan by the actual scan
      r1        = (int)r[i];
      if(fi[i]<-M_PI/2.0) //negative numbers around -180 are a problem with indexing
        fi1       = (int)roundf((fi[i]+2.0*M_PI) *PM_R2D);
      else
        fi1       = (int)roundf(fi[i]*PM_R2D);
      if(i!=0)
      {
        if((fi1-fi0)!=0 && act.seg[i]!=0 && act.seg[i]==act.seg[i-1])
        {// did they project into 2 different points?
         //are they part of the same segment? - so a big gap doesn't cover up a lot of points
          int rr,dr,s = (fi1>=fi0)?1:-1;
          dr = (r1-r0)/(fi1-fi0);
          for(int j=fi0; j<=fi1; j+=s)
          {
            rr = r0 + dr*(j-fi0);
            if(j>=0 && j<PM_L_POINTS)
              if(rr<new_r[j])
                new_r[j] = rr;
          }//for j
        }//if
        else //fi0=fi1
          if(fi1>=0 && fi1<PM_L_POINTS)
             new_r[fi1] = (r0>r1)?r0:r1;//set the longer range to not to remove too many points later        
      }
      r0        = r1;
      fi0       = fi1;

    #ifdef GR
      if(ref.bad[i])
        dr_circle(ref.x[i],ref.y[i],4,"yellow");
      else
        dr_circle(ref.x[i],ref.y[i],4,"black");
      if(new_bad[i])
        dr_circle(nx[i],ny[i],4,"green");
      else
        dr_circle(nx[i],ny[i],4,"blue");
    #endif

    }//for i
//    dr_zoom();
    #ifdef GR
      cout <<"interpolated ranges. press enter"<<endl;
      for(i=0;i<PM_L_POINTS;i++)
        dr_circle(new_r[i]*pm_co[i],new_r[i]*pm_si[i],6,"red");
      dr_zoom();
    #endif

    //removing covered points
    for(i=0;i<PM_L_POINTS;i++)
    {
      //is it out of range?
      if(fi[i]<0 || fi[i]>M_PI)
      {
        new_bad[i] = 1;
        continue;
      }
      //was it taken from behind?
      if(i>0 && i<(PM_L_POINTS-1))
      {
        if((fi[i+1]-fi[i-1]) < 0) //wrong order
        {
          new_bad[i] = 1;
          continue;
        }
      }
      //searching if the reference scan is occluding some parts actual scan
      int fi_l = (int)floor(fi[i]*180/M_PI);
      int fi_h = (int)ceil(fi[i]*180/M_PI);

      if(fi_l>=0 && fi_l<=PM_L_POINTS && fi_h>=0 && fi_h<=PM_L_POINTS)
      {
        //interpolate a range reading from ref scan & compare
        //1 deg assumption!
        PM_TYPE rr;
        rr = ref.r[fi_l] + (ref.r[fi_h]-ref.r[fi_l])*(fi[i]-pm_fi[fi_l]);
        if((r[i]-rr)>100) // is the reference scan covering out the current scan?
        {
          new_bad[i] = 1;
          continue;
        }
        //check if any actual scan point is occluding
        int idx = (int)roundf(fi[i]*PM_R2D);//index into new_r
        if(idx>=0 && idx<PM_L_POINTS && (r[i]-new_r[i])>100)
        {
          new_bad[i] = 1;
          continue;
        }
      }
      
      //BUG: remove the scanpoints covered up by the current scan too!
    }//for i

    #ifdef GR
      for(i=0;i<PM_L_POINTS;i++)
      if(new_bad[i])
        dr_circle(nx[i],ny[i],4,"green");
    #endif


    //correspondence search ...
    n=0;
    PM_TYPE d,min_d;
    int min_idx;

    for(i=0;i<PM_L_POINTS;i++)
    {
      min_d = 1000000;
      min_idx = -1;
      if(!new_bad[i])
      {

        int imin,imax;
        imin = (int)(fi[i]*180/M_PI-window);
        if(imin<0)
          imin =0;
        imax = (int)(fi[i]*180/M_PI+window);
        if(imax>PM_L_POINTS)
          imax =PM_L_POINTS;


//        for(j=0;j<PM_L_POINTS;j++)
        for(j=imin;j<imax;j++)
        {
          if(!ref.bad[j])
          {
            d =  SQ(nx[i]-ref.x[j]) + SQ(ny[i]-ref.y[j]);//square distance
            if(d<min_d)
            {
              min_d  = d;
              min_idx = j;
            }
          }
        }//for
        if(min_idx>=0 && sqrt(min_d)<PM_MAX_ERROR) // was there any match closer than 1m?
        {
          index[n][0] = i;
          index[n][1] = min_idx;
          dist[n] = sqrt(min_d);
          n++;
          #ifdef GR
              dr_line(nx[i],ny[i],ref.x[min_idx],ref.y[min_idx],"blue");
          #endif

        }
      }//if
    }//for
//    dr_zoom();

    if(n<PM_MIN_VALID_POINTS)
    {
      cerr <<"pm_icp: ERROR not enough points"<<endl;
      #ifdef  PM_GENERATE_RESULTS
          fclose(f);
      #endif      
      throw 1;
    }

    //sort the matches with bubble sort
    //put the largest 20 percent to the end
    imax = (int)((double)n*0.2);
    for(i=0;i<imax;i++)
      for(j=1;j<(n-i);j++)
      {
        if(dist[j]<dist[j-1]) //are they in the wrong order?
        { //swap them
          k             = index[j][0];
          index[j][0]   = index[j-1][0];
          index[j-1][0] = k;

          k             = index[j][1];
          index[j][1]   = index[j-1][1];
          index[j-1][1] = k;

          d             = dist[j];
          dist[j]       = dist[j-1];
          dist[j-1]     = d;
        }
      }//for j


    //pose estimation
    //------------------------------------------translation-------------

    // do the weighted linear regression on the linearized ...
    // include angle as well
    PM_TYPE dr;

    //computation of the new dx1,dy1,dtheta1
    PM_TYPE dx1,dy1,dtheta1;
    PM_TYPE sxx=0,sxy=0,syx=0,syy=0;
    PM_TYPE meanpx,meanpy,meanppx,meanppy;
    PM_TYPE apx[PM_L_POINTS], apy[PM_L_POINTS],ppx[PM_L_POINTS], ppy[PM_L_POINTS];
    meanpx = 0;meanpy = 0;
    meanppx= 0;meanppy= 0;

    abs_err=0;
    imax = n-imax;
    for(i=0;i<imax;i++)
    {
      //weight calculation
      // do the cartesian calculations....
      meanpx +=  nx[index[i][0]];
      meanpy +=  ny[index[i][0]];

      meanppx +=  ref.x[index[i][1]];
      meanppy +=  ref.y[index[i][1]];
      #ifdef GR
        dr_line(nx[index[i][0]],ny[index[i][0]],ref.x[index[i][1]],ref.y[index[i][1]],"red");
      #endif
    }//for
    meanpx /= imax;
    meanpy /= imax;

    meanppx /= imax;
    meanppy /= imax;

    for(int i=0;i<imax;i++)
    {
        sxx += (nx[index[i][0]] - meanpx)*(ref.x[index[i][1]] - meanppx);
        sxy += (nx[index[i][0]] - meanpx)*(ref.y[index[i][1]] - meanppy);
        syx += (ny[index[i][0]] - meanpy)*(ref.x[index[i][1]] - meanppx);
        syy += (ny[index[i][0]] - meanpy)*(ref.y[index[i][1]] - meanppy);
    }
      //computation of the resulting translation and rotation
      //for method closest point match
    dth = atan2(sxy-syx,sxx+syy);
    dx  = meanppx - ax - (cos(dth)*(meanpx- ax) - sin(dth)*(meanpy - ay));
    dy  = meanppy - ay - (sin(dth)*(meanpx- ax) + cos(dth)*(meanpy - ay));

    ax += dx;
    ay += dy;
    ath+= dth;
    ath = norm_a(ath);

//    //for SIMULATION iteration results..
//    cout <<iter<<"     "<<ax<<"    "<<ay<<"    "<<ath*PM_R2D<<" ;"<<endl;
    #ifdef GR
      cout <<"iter "<<iter<<" "<<ax<<" "<<ay<<" "<<ath*PM_R2D<<" "<<dx<<" "<<dy<<endl;
//      if(iter==0)
        dr_zoom();
      usleep(10000);

    #endif

  }//for iter
  #ifdef  PM_GENERATE_RESULTS
     end_tick =rdtsc();
     fprintf(f,"%i %lf %lf %lf %lf\n",iter,
        (double)(end_tick-start_tick-dead_tick)*1000.0/CPU_FREQ,ax,ay,ath*PM_R2D);
     fclose(f);
  #endif

  lsa->rx =ax;lsa->ry=ay;lsa->th=ath;
  return(abs_err/n);
}//pm_icp
//-------------------------------------------------------------------------
//to interface with matlab
void pm_save_scan(PMScan *act,char *filename)
{
  FILE *f;
  f=fopen(filename,"w");
  for(int i=0;i<PM_L_POINTS;i++)
    fprintf(f,"%f %i %i\n",act->r[i],act->bad[i],act->seg[i]);
  fclose(f);
}
//-------------------------------------------------------------------------
// plots actual and reference scan in a way how it should appear in the thesis
void pm_plotScan4Thesis(PMScan *lsr,PMScan *lsa)
{
  float x,y,xw,yw,last_xw,last_yw;
  float si_th;
  float co_th;
  char *col;
  int i,j;
   PMScan *ls;

  dr_erase(); 
  //reference scan
  ls  = lsr;
  col = (char *)"black";
  dr_circle(ls->rx,ls->ry,10.0,col);
  dr_line(ls->rx,ls->ry,ls->rx+10.0*cos(ls->th+M_PI/2.0),
          ls->ry+10.0*sin(ls->th+M_PI/2.0),col);
  for(i=0;i<PM_L_POINTS;i++)
  {
    x  = ls->r[i]*pm_co[i];
    y  = ls->r[i]*pm_si[i];
    xw = x;//x*co_th - y*si_th  + ls->rx;
    yw = y;//x*si_th + y*co_th  + ls->ry;

    if(ls->r[i]<PM_MAX_RANGE)
    {
      dr_circle(xw,yw,4,col);
      if(i>0 && ls->r[i-1]<PM_MAX_RANGE)
      dr_line(xw,yw,last_xw,last_yw,col);
    }
    last_xw = xw;
    last_yw = yw;
  }

  //actual scan
  ls  = lsa;
  si_th = sin(ls->th);
  co_th = cos(ls->th);
  col = (char *)"blue";
  dr_circle(ls->rx,ls->ry,10.0,col);
  dr_line(ls->rx,ls->ry,ls->rx+10.0*cos(ls->th+M_PI/2.0),
          ls->ry+10.0*sin(ls->th+M_PI/2.0),col);

  for(i=0;i<PM_L_POINTS;i++)
  {
    x  = ls->r[i]*pm_co[i];
    y  = ls->r[i]*pm_si[i];
    xw = x*co_th - y*si_th  + ls->rx;
    yw = x*si_th + y*co_th  + ls->ry;

    if(ls->r[i]<PM_MAX_RANGE)
    {
      dr_circle(xw,yw,4,col);
      if(i>0 && ls->r[i-1]<PM_MAX_RANGE)
      dr_line(xw,yw,last_xw,last_yw,col);
    }
    last_xw = xw;
    last_yw = yw;
  }
  dr_fit();
}
//-------------------------------------------------------------------------
// plots contents of timing file - for convergence measurements
// x,y,th are the true results; tht is in degrees!
void pm_plotTime4Thesis(PM_TYPE xt, PM_TYPE yt, PM_TYPE tht,int *iter,double *time)
{
  FILE *f;
  int cnt =5;
  int i=-1;
  double t=-1,xx,yy,th;
  double x,y1,y2,y3,lx=-1,ly1,ly2,ly3;
  char *c1="red",*c2="black",*c3 = "blue",s[1000];
  
  f=fopen(PM_TIME_FILE,"r");
  if(f==NULL)
  {
    cerr <<"pm_plotTime4Thesis: Error: could not open file "<<PM_TIME_FILE<<endl;
    throw 1;
  }

  dr_erase();  
  while(cnt==5)
  {  
    cnt=fscanf(f,"%i %lf %lf %lf %lf\n",&i,&t,&xx,&yy,&th);
    x  = t;
    y1 = fabs(xx-xt);
    y2 = fabs(yy-yt);
    y3 = fabs(th-tht);
    if(i%10)
      dr_line(x,0.4,x,-0.4,"black");
    else
    {
      dr_line(x,2,x,-2,"black");
      sprintf(s,"%02i",i);
      dr_text(x,2,1,1,s,"black");
    }
      dr_marker(x,y1,1,c1);
      dr_marker(x,y2,2,c2);
      dr_marker(x,y3,3,c3);
    if(lx>0)
    {
      dr_line(lx,ly1,x,y1,c1);
      dr_line(lx,ly2,x,y2,c2);
      dr_line(lx,ly3,x,y3,c3);      
    }
    lx = x;
    ly1 = y1;
    ly2 = y2;
    ly3 = y3;
    
  }//whi
  dr_fit();
  if(iter!=NULL && time!=NULL)
  {
     *iter  = i;*time = t;
  }
}//void pm_plotTime4Thesis(void)

//-------------------------------------------------------------------------
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------