polar_match.cpp

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

/***************************************************************************
             polar_match.cpp  - matching laser scans in polar coord system
             designed for use with SICK LMS in cm res./361 meas. mode
             only 181 readings (1 deg res) are used (less accuracy but higher
             speed)
                             -------------------
begin           : Tue Nov 9 2004
version         : 0.1
copyright       : (C) 2005 by Albert Diosi and Lindsay Kleeman
email           : [e-mail]
comments        : - range units are cm; angle units are deg
                  - don't forget to set the optimization switch!
                  - if it works with float->chage fabs-fabsf, floor->floorf
                  - why not add angle as well to the iterative least squares?
                     => bad convergence, error tends to be fixed with rotation
                  - try to make the C - coef for sigmoid function smaller with
                     with the as scanmatching converges, to increase the precision
                     as in dudek
changed:
                26/05/2005- projection filter of ICP fixed
                7/12/2004 - dx,dy estimation interleaved with dth estimation
                            seems to work OK
                3/12/2004 - iterative range least squares seems to work (for
                           dx,dy only), though it needs to be thoroughly tested

                26/11/2004
***************************************************************************/
/****************************************************************************
    This file is part of polar_matching.

    polar_matching is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    polar_matching is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with polar_matching; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
****************************************************************************/

#include <iostream>
#include <unistd.h>
#include "polar_match.h"
#include "draw.h"

using namespace std;


PM_TYPE   pm_fi[PM_L_POINTS];//contains precomputed angles (0-180)
PM_TYPE   pm_si[PM_L_POINTS];//contains sinus of angles
PM_TYPE   pm_co[PM_L_POINTS];//contains cos of angles
PM_TYPE   PM_D2R = M_PI/180.0; // degrees to rad
PM_TYPE   PM_R2D = 180.0/M_PI; // rad to degrees


void      pm_init(char* filename, FILE **fin);
int       pm_readScan(FILE *fin, PMScan *ls);
void      pm_plotScan(PMScan *ls, char * col);

double const CPU_FREQ =  896363000.0;//Hz
__inline__ unsigned long long int rdtsc()
{
   unsigned long long int x;
   __asm__ volatile (".byte 0x0f, 0x31" : "=A" (x));
   return x;
}


inline PM_TYPE norm_a(PM_TYPE a)
{ int m;
  m=(int)(a/(2.0*M_PI));
  a=a-(PM_TYPE)m*M_PI;
  if(a<(-M_PI))
    a+=2.0*M_PI;
  if(a>=M_PI)
    a-=2.0*M_PI;
  return(a);
}

//opens the scanfile and reads out the first line
//the file pointer will point to the file
//this fuction has to be called, because some global variables
//are initialized here
void pm_init(char* filename, FILE **fin)
{
  size_t len=2000;
  char *line;
  line = new char[len];

  *fin=fopen(filename,"rt");
  if(*fin==NULL)
  {
    cerr <<"pm_init: unable to open file:"<<filename<<endl;
    throw 1;
  }
  getline(&line,&len,*fin);
  cout <<line<<endl;
  delete[] line;
  for(int i=0;i<PM_L_POINTS;i++)
  {
    pm_fi[i] = ((float)i)*M_PI/180.0;
    pm_si[i] = sin(pm_fi[i]);
    pm_co[i] = cos(pm_fi[i]);
  }
  
}

//reads on scan from file fin using the laserparameters stored in lp
//and fills in the values into ls
//returns 0 if OK
//-1 if no more scans
int pm_readScan(FILE *fin, PMScan *ls)
{
  int n=0;
  n+=fscanf(fin,"%lf %f %f %f\n",&(ls->t),&(ls->rx),&(ls->ry),&(ls->th));
  ls->t     -=PM_TIME_DELAY;
  ls->rx    *=100.0;//con. into cm
  ls->ry    *=100.0;
  ls->th    = norm_a(ls->th-M_PI/2.0);


  for(int i=0; i<PM_L_POINTS; i++)
  {
    n+=fscanf(fin,"%f\n",&(ls->r[i]));
    ls->x[i] = (ls->r[i])*pm_co[i];
    ls->y[i] = (ls->r[i])*pm_si[i];
    ls->bad[i] = 0;
    if(i!=(PM_L_POINTS-1))
    { double dummy;
      fscanf(fin,"%f\n",&dummy);
    }
    
  }
  if(n!=(PM_L_POINTS+4))
    return -1;
  return 0;    
}

void pm_plotScanAt(PMScan *ls, PM_TYPE x,PM_TYPE y,PM_TYPE th,char *col,double diameter)
{
  float xx,yy,xw,yw,last_xw,last_yw;
  float si_th = sin(th);
  float co_th = cos(th);
  char *color;

  dr_circle(x,y,10.0,col);
  dr_line(x,y,x+10.0*cos(th+M_PI/2.0), y+10.0*sin(th+M_PI/2.0),col);
  for(int i=0;i<PM_L_POINTS;i++)
  {
    xx  = ls->r[i]*pm_co[i];
    yy  = ls->r[i]*pm_si[i]  + PM_LASER_Y;
    xw = xx*co_th - yy*si_th  + x;
    yw = xx*si_th + yy*co_th  + y;
    if(ls->bad[i]==0)
      color = col;
    else //bad points are drawn green
      color = col;//"green";
    dr_circle(xw,yw,diameter,color);
    if(i!=0)
    {
      dr_line(xw,yw,last_xw,last_yw,color);
    }
    last_xw = xw;
    last_yw = yw;
  }
}


// draws current scan in world and robot(polar) coord syst with colour col
void pm_plotScan(PMScan *ls, char *col,double diameter)
{
  float x,y,xw,yw,last_xw,last_yw;
  float si_th = sin(ls->th);
  float co_th = cos(ls->th);
  char *color;

  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);
//  dr_line(0,-100.0,180,-100.0,"black");
  for(int i=0;i<PM_L_POINTS;i++)
  {
    x  = ls->r[i]*pm_co[i];
    y  = ls->r[i]*pm_si[i]  + PM_LASER_Y;
    xw = x*co_th - y*si_th  + ls->rx;
    yw = x*si_th + y*co_th  + ls->ry;    
    if(ls->bad[i]==0)
      color = col;
    else //bad points are drawn green
      color = col;//"green";
//    dr_circle(pm_fi[i]*PM_R2D,ls->r[i]/10.0-100.0,diameter,color);    
    dr_circle(xw,yw,diameter,color);
    if(i!=0)
    {
      dr_line(xw,yw,last_xw,last_yw,color);
//      dr_line(pm_fi[i]*PM_R2D,ls->r[i]/10.0-100.0,
//              pm_fi[i-1]*PM_R2D,ls->r[i-1]/10.0-100.0,color);
    }
    last_xw = xw;
    last_yw = yw;
  }
//  char str[1000];
//  sprintf(str,"%.3lf",ls->t);
//  dr_text(ls->rx,ls->ry,1,1,str,col);
}



//calculates how scan ls would look like from dx,dy
//di is the rotation in index which has to be done first
//resulting scan is returned in act
//function returns the number of valid points
// this is a hack! think about the bad points.
int pm_translate(PMScan *ls,PM_TYPE dx, PM_TYPE dy,int di,PMScan *act)
{
  int i,j,n=0,min_i,max_i;
  PM_TYPE x,y,r,fi,last_fi=-1;

  if(di>0)
    min_i=di;
  else
    min_i=0;

  if(di<0)
    max_i = PM_L_POINTS + di;
  else
    max_i = PM_L_POINTS;

  //reseting all to bad
  for(i=0;i<PM_L_POINTS;i++)
  {
    act->bad[i] = 1;
    act->r[i] = 100000;
  }
    
  for(i=min_i;i<max_i;i++)
  {
    if(ls->bad[i]==0)
    {
      j = i - di;
      x=ls->r[j]*pm_co[j];
      y=ls->r[j]*pm_si[j];

      r=sqrt( (dx-x)*(dx-x) +(dy-y)*(dy-y)   );
      fi = atan2(y-dy,x-dx);

      int int_fi = (int)floor(fi*PM_R2D+0.5);//conv to deg and round
      if(int_fi>=0 && int_fi<PM_L_POINTS && act->r[int_fi]>r) //is it
      {
        act->r[int_fi] = r;
      }
    }
  }

}


//-------------------------------------------------------------------------
//filters the ranges with a median filter. x,y points are not upadted
//ls - laser scan
// seems like the median filter is a good thing!
//if window is 5, then 3 points are needed in a bunch to surrive!
//don't use this function with line fitting!
void pm_median_filter(PMScan *ls)
{
  const int HALF_WINDOW  = 2;//2 to left 2 to right
  const int WINDOW = 2*HALF_WINDOW+1;
  PM_TYPE   r[WINDOW];
  PM_TYPE   w;
  
  int i,j,k,l;

  for(i=0;i<PM_L_POINTS;i++)
  {
    k=0;
    for(j=i-HALF_WINDOW;j<=i+HALF_WINDOW;j++)
    {
      l = ((j>=0)?j:0);
      r[k]=ls->r[(( l < PM_L_POINTS)?l:(PM_L_POINTS-1))];
      k++;
    }
    //bubble sort r
    for(j=(WINDOW-1);j>0;j--)
      for(k=0;k<j;k++)
        if(r[k]>r[k+1]) // wrong order? - swap them
        {
           w=r[k];
           r[k]=r[k+1];
           r[k+1] = w;           
        }           
    ls->r[i] = r[HALF_WINDOW];//choose the middle point
  }
}

//-------------------------------------------------------------------------
// segments scanpoints into groups based on range discontinuities
// number 0 is reserved to segments with size 1
// assumptions: too far points PM_MAX_RANGE - divide segments
//              - bad points are only due to far points and mixed pixels
// seems all right, except a far point can be the beginning of a new segment
// if the next point is good and close -> it shouldn't make a difference
void pm_segment_scan(PMScan *ls)
{
  const PM_TYPE   MAX_DIST = 20.0;//max range diff between conseq. points in a seg
  const PM_TYPE   D_PHI    = M_PI/(PM_L_POINTS-1);//angle increments
        PM_TYPE   r,dr;
        int       seg_cnt = 0;
        int       i,j,k,cnt;
        bool      break_seg;
          
  seg_cnt = 1;

  //init:
  if(fabs(ls->r[0]-ls->r[1])<MAX_DIST) //are they in the same segment?
  {
    ls->seg[0] = seg_cnt;
    ls->seg[1] = seg_cnt;
    cnt        = 2;    //2 points in the segment
  }
  else
  {
    ls->seg[0] = 0; //point is a segment in itself
    ls->seg[1] = seg_cnt;
    cnt        = 1;
  }
  
  for(i=2;i<PM_L_POINTS;i++)
  { //segment breaking conditions: - bad point;
    break_seg = false;
    if(ls->bad[i])
    {
      break_seg = true;
      ls->seg[i] = 0;
    }
    else
    {    
      dr = ls->r[i]-(2.0*ls->r[i-1]-ls->r[i-2]);//extrapolate & calc difference
      if( fabs(ls->r[i]-ls->r[i-1])<MAX_DIST || ((ls->seg[i-1]==ls->seg[i-2])
              && fabs(dr)<MAX_DIST))
      {//not breaking the segment
        cnt++;
        ls->seg[i] = seg_cnt;  
      }
      else
        break_seg = true;
    }//if ls
    if(break_seg) // breaking the segment?
    {      
      if(cnt==1)
      {        
        //check first if the last three are not on a line by coincidence
        dr = ls->r[i]-(2.0*ls->r[i-1]-ls->r[i-2]);
        if(ls->seg[i-2] == 0 && ls->bad[i] == 0 && ls->bad[i-1] == 0
           && ls->bad[i-2] == 0 && fabs(dr)<MAX_DIST)
        {
          ls->seg[i]   = seg_cnt;
          ls->seg[i-1] = seg_cnt;
          ls->seg[i-2] = seg_cnt;
          cnt = 3;
        }//if ls->
        else
        {
          ls->seg[i-1] = 0;
          //what if ls[i] is a bad point? - it could be the start of a new
          //segment if the next point is a good point and is close enough!
          //in that case it doesn't really matters
          ls->seg[i] = seg_cnt;//the current point is a new segment
          cnt = 1;
        }
      }//if cnt ==1
      else
      {
        seg_cnt++;
        ls->seg[i] = seg_cnt;
        cnt = 1;
      }//else if cnt
    }//if break seg    
  }//for    
}//pm_segment_scan



//-------------------------------------------------------------------------
// marks point further than a given distance PM_MAX_RANGE as PM_RANGE
void pm_find_far_points(PMScan *ls)
{
  for(int i=0;i<PM_L_POINTS;i++)
  {
     if(ls->r[i]>PM_MAX_RANGE)
       ls->bad[i] |= PM_RANGE;
  }//
}

//-------------------------------------------------------------------------
//shows in different colours the different laser segments
void pm_show_segmentation(PMScan *ls)
{
  float x,y,xw,yw,last_xw,last_yw;
  float si_th = sin(ls->th);
  float co_th = cos(ls->th);
  char *color;
  char *col = "red";

  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);
  dr_line(0,-100,180,-100,"black");
  for(int i=0;i<PM_L_POINTS;i++)
  { //cout <<i<<endl;
    x = ls->r[i]*pm_co[i];
    y = ls->r[i]*pm_si[i]+PM_LASER_Y;
    xw = x*co_th -y*si_th + ls->rx;
    yw = x*si_th +y*co_th + ls->ry;
    color = dr_COLORS[abs(ls->seg[i])%(dr_COLORS_CNT-1)];
    if(ls->seg[i]==0)
      color = dr_COLORS[dr_COLORS_CNT-1];    
    dr_circle(pm_fi[i]*PM_R2D,ls->r[i]/10.0-100.0,1,color);
    dr_circle(xw,yw,2,color);
    if(i!=0 && ls->seg[i]!=0 && ls->seg[i]==ls->seg[i-1] )
    {
      dr_line(xw,yw,last_xw,last_yw,color);
      dr_line(pm_fi[i]*PM_R2D,ls->r[i]/10.0-100.0,
        pm_fi[i-1]*PM_R2D,ls->r[i-1]/10.0-100.0,color);
    }
    else
    {
      char str[1000];
      sprintf(str,"%i",ls->seg[i]);
      dr_text(xw,yw,1,1,str,col);
      dr_text(pm_fi[i]*PM_R2D,ls->r[i]/10.0-100.0,1,1,str,col);      
    }    
    last_xw = xw;
    last_yw = yw;