polar_match.cpp

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

     f = fopen(PM_TIME_FILE,"w");
     dead_tick = 0;
     start_tick =rdtsc();     
  #endif


  act = *lsa;
  ref = *lsr;

  rx =  ref.rx; ry = ref.ry; rth = ref.th;
  ax =  act.rx; ay = act.ry; ath = act.th;

  //transformation of actual scan laser scanner coordinates into reference
  //laser scanner coordinates
  t13 = sin(rth-ath)*LASER_Y+cos(rth)*ax+sin(rth)*ay-sin(rth)*ry-rx*cos(rth);
  t23 = cos(rth-ath)*LASER_Y-sin(rth)*ax+cos(rth)*ay-cos(rth)*ry+rx*sin(rth)-LASER_Y;

  ref.rx = 0;   ref.ry = 0;   ref.th = 0;
  act.rx = t13; act.ry = t23; act.th = ath-rth;

  ax = act.rx; ay = act.ry; ath = act.th;
  //from now on act.rx,.. express the lasers position in the ref frame

  iter = -1;
  while(++iter<PM_MAX_ITER && small_corr_cnt<3) //has to be 5 small corrections before stop
  {
    if( (fabs(dx)+fabs(dy)+fabs(dth))<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;
    // convert range readings into ref frame
    // this can be speeded up, by connecting it with the interpolation  
    for(i=0;i<PM_L_POINTS;i++)
    {
      delta   = ath+pm_fi[i];
      x       = act.r[i]*cos(delta)+ax;
      y       = act.r[i]*sin(delta)+ay;
      r[i]    = sqrt(x*x+y*y);
      fi[i]   = atan2(y,x);
//      px[i]   = x;       
//      py[i]   = y;
      new_r[i]  = 10000;//initialize big interpolated r;
      new_bad[i]= PM_EMPTY;//for interpolated r;

      //debug
      #ifdef GR
         dr_circle(ref.r[i]*pm_co[i],ref.r[i]*pm_si[i],4.0,"black");
         dr_circle(x,y,4.0,"red");
         dr_circle(pm_fi[i]*PM_R2D,ref.r[i]/10.0-100,1,"black");
         dr_circle(fi[i]*PM_R2D,r[i]/10.0-100,1,"red");
      #endif
    }//for i

    //------------------------INTERPOLATION------------------------
    //calculate/interpolate the associations to the ref scan points
    //algorithm ignores crosings at 0 and 180 -> to make it faster
    for(i=1;i<PM_L_POINTS;i++)
    { //i points to the angles in the actual scan

      // i and i-1 has to be in the same segment, both shouldn't be bad
      // and they should be bigger than 0
      if(act.seg[i]!=0 && act.seg[i]==act.seg[i-1] && !act.bad[i] &&
         !act.bad[i-1] && fi[i]>0 && fi[i-1]>0)
      {
        //calculation of the "whole" parts of the angles
        int     fi0,fi1;
        PM_TYPE r0,r1,a0,a1;
        bool occluded;
        if(fi[i]>fi[i-1])//are the points visible?
        {//visible
          occluded = false;
          a0  = fi[i-1];
          a1  = fi[i];
          fi0 = (int) ceil(fi[i-1]*PM_R2D);//fi0 is the meas. angle!
          fi1 = (int) (fi[i]*PM_R2D);
          r0  = r[i-1];
          r1  = r[i];
        }
        else
        {//invisible - still have to calculate to filter out points which
          occluded = true; //are covered up by these!
          //flip the points-> easier to program
          a0  = fi[i];
          a1  = fi[i-1];
          fi0 = (int) ceil(fi[i]*PM_R2D);
          fi1 = (int) (fi[i-1]*PM_R2D);
          r0  = r[i];
          r1  = r[i-1];
        }
        //here fi0 is always smaller than fi1!

        //interpolate for all the measurement bearings beween fi0 and fi1
        while(fi0<=fi1)//if at least one measurement point difference, then ...
        {
          PM_TYPE ri = (r1-r0)/(a1-a0)*(((PM_TYPE)fi0*PM_D2R)-a0)+r0;

          //if fi0 -> falls into the measurement range and ri is shorter
          //than the current range then overwrite it
          if(fi0>=0 && fi0<PM_L_POINTS && new_r[fi0]>ri)
          {
            new_r[fi0]    = ri;//overwrite the previous reading
            new_bad[fi0] &=~PM_EMPTY;//clear the empty flag
            index[fi0]    = i;//store which reading was used at index fi0
            if(occluded) //check if it was occluded
              new_bad[fi0] = new_bad[fi0]|PM_OCCLUDED;//set the occluded flag
             else
              new_bad[fi0] = new_bad[fi0]&~PM_OCCLUDED;
            //the new range reading also it has to inherit the other flags
            new_bad[fi0] |= act.bad[i];//superfluos - since act.bad[i] was checked for 0
            new_bad[fi0] |= act.bad[i-1];//superfluos - since act.bad[i-1] was checked for 0
            //if(ri>PM_MAX_RANGE)        //uncomment this later
            //  new_bad[fi0] |= PM_RANGE;
          }
          fi0++;//check the next measurement angle!
        }//while
      }//if act
    }//for i

    //---------------ORIENTATION SEARCH-----------------------------------
    //search for angle correction using crosscorrelation
//    if(iter%5 ==4)
    if(iter%2 ==1)
    {
      //pm_fi,ref.r - reference points
      //r,fi        - actual points which are manipulated
      //new_r, new_bad contains the

      PM_TYPE e,err[100];       // the error rating
      PM_TYPE beta[100];// angle for the corresponding error
//      PM_TYPE C = 10000;
      PM_TYPE n;
      int k=0;

      for(int di=-window;di<=window;di++)
      {
        n=0;e=0;

        int min_i,max_i;
        if(di<=0)
          {min_i = -di;max_i=PM_L_POINTS;}
        else
          {min_i = 0;max_i=PM_L_POINTS-di;}
          
        for(i=min_i;i<max_i;i++)//searching through the actual points
        {
          //if fi[i],r[i] isn't viewed from the back, isn't moving
          // and isn't a solitary point, then try to associate it ..
          //also fi[i] is within the angle range ...
//          if(fi[i-1]<fi[i] && !act.bad[i] && act.seg[i]!=0) //can be speeded up!
//          {
//            PM_TYPE angle = fi[i]*180.0/M_PI+dfi;
//            int     fi_l  = (int)floor(angle+0.5);
//            if(fi_l>=0 ||fi_l<=180)
//            {
//              e += fabs(r[i]-ref.r[fi_l]);
//              n++;
//            }
//          }

            if(!new_bad[i] && !ref.bad[i+di])
            {
              e += fabs(new_r[i]-ref.r[i+di]);
              n++;              
            }


        }//for i

        if(n>0)
          err[k]  = e/n;//don't forget to correct with n!        
        else
          err[k]  = 10000;//don't forget to correct with n!
        beta[k] = di;
        k++;
      }//for dfi

      #ifdef GR
        FILE *fo;
        fo = fopen("angles.txt","w");
        for(i=0;i<k;i++)
        {
         // cout <<beta[i]<<"\t\t"<<err[i]<<endl;
          dr_circle(beta[i],err[i],1.0,"blue");
          fprintf(fo,"%f %f\n",beta[i],err[i]);
        }
        fclose(fo);
//        if(iter>=37)
//        for(i=0;i<k;i++)
//           dr_circle(beta[i],err[i],1.0,"black");
      #endif

      //now search for the global minimum
      //later I can make it more robust
      //assumption: monomodal error function!
      PM_TYPE emin = 1000000;
      int   imin;
      for(i=0;i<k;i++)
        if(err[i]<emin)
        {
          emin = err[i];
          imin = i;
        }
      if(err[imin]>=10000)
      {
        cerr <<"Polar Match: orientation search failed" <<err[imin]<<endl;
        #ifdef  PM_GENERATE_RESULTS
          fclose(f);
        #endif
        throw 1;
      }
      dth = beta[imin];

      //interpolation
      if(imin>=1 && imin<(k-1)) //is it not on the extreme?
      {//lets try interpolation
        PM_TYPE D = err[imin-1]+err[imin+1]-2.0*err[imin];
        PM_TYPE d=1000;
        if(fabs(D)>0.01 && err[imin-1]>err[imin] && err[imin+1]>err[imin])
          d=(err[imin-1]-err[imin+1])/D/2.0;
//        cout <<"ORIENTATION REFINEMENT "<<d<<endl;
        if(fabs(d)<1)
          dth+=d;
      }//if

      ath += dth*M_PI/180.0;
      
      #ifdef GR
        cout <<"angle correction "<<dth<<endl;
        //usleep(10000);
        dr_zoom();
      #endif
      continue;
    }

    //------------------------------------------translation-------------
    if(iter>10)
       C = 100;
    // do the weighted linear regression on the linearized ...
    // include angle as well
    PM_TYPE hi1, hi2,hwi1,hwi2, hw1=0,hw2=0,hwh11=0;
    PM_TYPE hwh12=0,hwh21=0,hwh22=0,w;
    PM_TYPE dr;
    abs_err=0;
    n=0;
    for(i=0;i<PM_L_POINTS;i++)
    {
      dr = ref.r[i]-new_r[i];
      abs_err += fabs(dr);
      //weight calculation
      if(ref.bad[i]==0 && new_bad[i]==0 && new_r[i]<PM_MAX_RANGE && new_r[i]>10.0 && fabs(dr)<PM_MAX_ERROR)
      {
//        cout <<i<<" "<<dr<<";"<<endl;
        //weighting according to DUDEK00
        
        w = C/(dr*dr+C);
//        w = 1.0/fabs(dr);if(w>100)  w = 100;
        n++;

        //proper calculations of the jacobian
        hi1 = pm_co[i];//xx/new_r[i];//this the correct
        hi2 = pm_si[i];//yy/new_r[i];

        hwi1 = hi1*w;
        hwi2 = hi2*w;

        //par = (H^t*W*H)^-1*H^t*W*dr
        hw1 += hwi1*dr;//H^t*W*dr
        hw2 += hwi2*dr;

        //H^t*W*H
        hwh11 += hwi1*hi1;
        hwh12 += hwi1*hi2;
//        hwh21 += hwi2*hi1; //should take adv. of symmetricity!!
        hwh22 += hwi2*hi2;

        #ifdef GR
          //deb
          dr_circle(pm_fi[i]*PM_R2D,ref.r[i]/10.0-200,1,"black");
          dr_circle(pm_fi[i]*PM_R2D,new_r[i]/10.0-200,1,"red");
          dr_circle(pm_fi[i]*PM_R2D,dr/10.0-200,1,"blue");
//          cout <<i<<"\t"<<ref.r[i]<<"\t"<<new_r[i]<<"\t"<<dr<<"\t"<<w<<" "<<act.r[index[i]]
//          <<" "<<pm_fi[index[i]]<<";"<<endl;
          {
            double x,y;
            x = pm_fi[i]*PM_R2D;
            y = new_r[i]/10.0-200;
            dr_line(x,y,x+hwi1*dr,y+hwi2*dr,"red");
            x = new_r[i]*pm_co[i];
            y = new_r[i]*pm_si[i];
            dr_line(x,y,x+hwi1*dr,y+hwi2*dr,"red");
          }
        #endif
        
      }//if
    }//for i
    if(n<PM_MIN_VALID_POINTS) //are there enough points?
    {
      cerr <<"pm_linearized_match: ERROR not enough points"<<n<<endl;
      #ifdef  PM_GENERATE_RESULTS
        fclose(f);
      #endif     
      throw 1;//not enough points
    }

    //calculation of inverse
    PM_TYPE D;//determinant
    PM_TYPE inv11,inv21,inv12,inv22;//inverse matrix

    D = hwh11*hwh22-hwh12*hwh21;
    if(D<0.001)
    {
      cerr <<"pm_linearized_match: ERROR determinant to small! "<<D<<endl;
      #ifdef  PM_GENERATE_RESULTS
          fclose(f);
      #endif      
      throw 1;
    }
    inv11 =  hwh22/D;
    inv12 = -hwh12/D;
    inv21 = -hwh12/D;
    inv22 =  hwh11/D;

    dx = inv11*hw1+inv12*hw2;
    dy = inv21*hw1+inv22*hw2;

    ax += dx;
    ay += dy;

//    //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
  }//while 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
//  dr_zoom();
  lsa->rx =ax;lsa->ry=ay;lsa->th=ath;
  return(abs_err/n);
}//pm_linearized_match_int_angle
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------
// minimizes least square error of points through changing lsa->rx, lsa->ry,lsa->th
// by using ICP. Only measurements furher than 32m are ignored. Interpolation is
// not implemented. Only the best 80% of points are used.
// scan projection is done in each step.
PM_TYPE pm_icp(PMScan *lsr,PMScan *lsa)
{
//   #define GR //comment out if no graphics necessary
  PMScan    act,  ref;//copies of actual and reference scans
  PM_TYPE   lx,ly,lth;//laser position in the ref. frame
  PM_TYPE   rx,ry,rth,ax,ay,ath;//robot pos at ref and actual scans
  PM_TYPE   t13,t23,LASER_Y = PM_LASER_Y,delta;
  PM_TYPE   r[PM_L_POINTS],fi[PM_L_POINTS];//actual scan in ref. coord. syst.
  int       new_bad[PM_L_POINTS];//bad flags of the projected actual scan range readings
  int       new_r[PM_L_POINTS];//ranges of actual scan projected into ref. frame for occlusion check
  PM_TYPE   nx[PM_L_POINTS];//actual scanpoints in ref coord system
  PM_TYPE   ny[PM_L_POINTS];//actual scanpoints in ref coord system
  int       index[PM_L_POINTS][2];//match indices actual,refernce
  PM_TYPE   dist[PM_L_POINTS];// distance for the matches
  int       n = 0;//number of valid points
  int       iter,i,j,small_corr_cnt=0,k,imax;
  int       window       = 20;//+- width of search for correct orientation
  PM_TYPE   abs_err=0,dx=0,dy=0,dth=0;//match error, actual scan corrections
  PM_TYPE   MAX_RANGE = 3200,co,si,x,y;

  #ifdef  PM_GENERATE_RESULTS
     long long int start_tick, dead_tick,end_tick,end_tick2;
     FILE *f;
     f = fopen(PM_TIME_FILE,"w");
     dead_tick = 0;
     start_tick =rdtsc();
  #endif


  act = *lsa;
  ref = *lsr;

  rx =  ref.rx; ry = ref.ry; rth = ref.th;
  ax =  act.rx; ay = act.ry; ath = act.th;

  //transformation of actual scan laser scanner coordinates into reference
  //laser scanner coordinates
  t13 = sin(rth-ath)*LASER_Y+cos(rth)*ax+sin(rth)*ay-sin(rth)*ry-rx*cos(rth);
  t23 = cos(rth-ath)*LASER_Y-sin(rth)*ax+cos(rth)*ay-cos(rth)*ry+rx*sin(rth)-LASER_Y;

  ref.rx = 0;   ref.ry = 0;   ref.th = 0;
  act.rx = t13; act.ry = t23; act.th = ath-rth;

  ax = act.rx; ay = act.ry; ath = act.th;
  //from now on act.rx,.. express the lasers position in the ref frame
  //intializing x,y of act and ref


  for(i=0;i<PM_L_POINTS;i++)
  {
    ref.x[i] = ref.r[i]*pm_co[i];
    ref.y[i] = ref.r[i]*pm_si[i];
    if(ref.r[i]>MAX_RANGE)
      ref.bad[i]=1;
    else
      ref.bad[i]=0;

    act.x[i] = act.r[i]*pm_co[i];
    act.y[i] = act.r[i]*pm_si[i];
    if(act.r[i]>MAX_RANGE)
      act.bad[i]=1;
    else
      act.bad[i]=0;
  }//for i


  iter = -1;
  while(++iter<60 && small_corr_cnt<3) //has to be 5 small corrections before stop