main.cpp

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

/***************************************************************************
                         main.cpp  -  project for scan matching in polar coordinates
                           -------------------
   begin                : mon nov 8 2004
   version              : 0.1   
   copyright            : (C) 2005 by Albert Diosi and Lindsay Kleeman
   email                : [e-mail]
***************************************************************************/
/****************************************************************************
    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 <math.h>
#include "draw.h"
#include "polar_match.h"

using namespace std;

#define SAVE_FIG //uncomment if don't want to wait for lengthy image conversion

double const CPU_FREQ =  896363000.0;//Hz laptop - for time measurements
__inline__ unsigned long long int rdtsc()//reads the clock cycles of the processor
{
   unsigned long long int x;
   __asm__ volatile (".byte 0x0f, 0x31" : "=A" (x));
   return x;
}

//scan matching with simulated scans, algorithm describes which scanmatching variant is to be used.
void simulated_room(int algorithm=PM_PSM);
//read the ground thruth measurements and matches them
void ground_thruth(int algorithm=PM_PSM);
//scan matching form initial positions forming a grid. Convergence paths are drawn into a map.
void convergence_map(int algorithm=PM_PSM);
// testing the angle histogram method for discriminating corridors from
// rooms, and also matching the orientation in corridors
void test_angle_histogram(void);

int main(int argc, char *argv[])
{
  int  i,j,k;
  PMScan ls,lsr,lsa;
  FILE *fin;
  double last_time=-1;
  int cnt;
  long long int start_tick,end_tick;

  double table[][2]={{  2.773,133.223},//0
                     { 40.873,143.573},//1
                     { 45.023,147.883},//2
                     {187.733,183.423},//3
                     {334.793,330.683},//4
                     {552.933,555.043},//5
                     {645.283,639.053},//6
                     {670.363,666.213},//7
                     {682.903,676.593},//8
                     { 31.443, 34.763},//9
                     };//laser scan test time table
  const int tcnt = 10;
  PMScan stable[tcnt][2];

  dr_init(320,320,-400, -800, 800, 800);//for other than slam results

  //initialize some global structures...
  pm_init("ground_thruth.log", &fin);
  fclose(fin);

  
  int test = PM_PSM_C;

  #ifdef PM_GENERATE_RESULTS
    cout <<"==================RESULT GENERATION FOR THESIS=============="<<endl;
    cout <<"Please align window and press enter to begin"<<endl;
    dr_zoom();

//    test = PM_PSM;  convergence_map(test);
//    test = PM_PSM_C;convergence_map(test);
//    test = PM_ICP;  convergence_map(test);
    test = PM_PSM;  simulated_room(test);ground_thruth(test);sleep(1);
//    test = PM_PSM_C;simulated_room(test);ground_thruth(test);sleep(1);
//    test = PM_ICP;  simulated_room(test);ground_thruth(test);sleep(1);
  #else
//    simulated_room(PM_PSM);
//    ground_thruth(test);
//    test_angle_histogram();
  #endif
  
  dr_close();
  cout <<"Finished the business"<<endl;
  return EXIT_SUCCESS;
}


//===========================================SIMULATED ROOM=========================
//scan matching with simulated scans
void simulated_room(int algorithm)
{
  long long int start_tick,end_tick;
  PMScan ls,lsr,lsa;
  const int N = 4;
  PM_TYPE a[N]={0,M_PI/2.0,M_PI,-M_PI/2.0};
  PM_TYPE d[N]={300,500,300,300};
  PM_TYPE x,y,th,r,rmin,D,fi;
  int i,j;
  char *alg_str;
  char s[1000];


  if(PM_LASER_Y!= 0)
  {
    cerr <<"simulated_room: ERROR PM_LASER_Y is not 0!!!"<<endl;
    exit(-1);
  }
  
  //ref scan
  x = 0;y=0;th=0*PM_D2R;
  ls.rx=x;ls.ry=y;ls.th=th;
  for(i=0;i<181;i++)
  {
    rmin = 100000;
    fi=pm_fi[i];
    for(j=0;j<N;j++)
    {
      D = cos(th+fi)*cos(a[j])+sin(th+fi)*sin(a[j]);
      if(fabs(D)>0.001)
      {
        r = (d[j]-x*cos(a[j])-y*sin(a[j]))/D;
        if(r>0 && r<rmin)
          rmin = r;
      }
      ls.r[i]=rmin;
    }//for j
    ls.seg[i]=0;
    ls.bad[i]=0;
  }//for i
  pm_plotScan(&ls,"black");
  dr_fit();
  lsr=ls;

  //actual scan
  x = 0;y=0;th=0*PM_D2R;
  ls.rx=x;ls.ry=y;ls.th=th;
  for(i=0;i<181;i++)
  {
    rmin = 100000;
    fi=pm_fi[i];
    for(j=0;j<N;j++)
    {
      D = cos(th+fi)*cos(a[j])+sin(th+fi)*sin(a[j]);
      if(fabs(D)>0.001)
      {
        r = (d[j]-x*cos(a[j])-y*sin(a[j]))/D;
        if(r>0 && r<rmin)
          rmin = r;
      }
      ls.r[i]=rmin;
    }//for j
    ls.seg[i]=0;
    ls.bad[i]=0;
  }//for i

//  cout <<"Simulated room fix on"<<endl;
//  ls.r[90]=200;  ls.r[91]=200;  ls.r[92]=200;  ls.r[93]=200;
//  ls.r[94]=200;  ls.r[95]=200;  ls.r[96]=200;


  lsa= ls;


//  lsa.rx = 50;lsa.ry=50;lsa.th=0*PM_D2R;
  lsa.rx = 100;lsa.ry=-100;lsa.th=15*PM_D2R;
  #ifdef PM_GENERATE_RESULTS
      pm_plotScan4Thesis(&lsr,&lsa);
      sprintf(s,"results/sim_init.png");
      dr_save(s);
//      dr_zoom();
  #else
    pm_plotScan(&lsa,"red");
    dr_zoom();
  #endif

  //matching
    pm_median_filter(&lsr);
    pm_median_filter(&lsa);
    pm_find_far_points(&lsr);
    pm_find_far_points(&lsa);
    pm_segment_scan(&lsr);
    pm_segment_scan(&lsa);
//    pm_save_scan(&lsr,"ref.txt");
//    pm_save_scan(&lsa,"act.txt");    
    try{
     start_tick = rdtsc();
     switch(algorithm)
     {
       case PM_PSM:    //polar scanmatching - matching bearing
           pm_psm(&lsr,&lsa);
           alg_str = (char *)"psm";
         break;
       case PM_PSM_C: //polar scanmatching - using cartesian equations
           pm_cartesian_match(&lsr,&lsa);
           alg_str = (char *)"psm_c";
         break;
       case PM_ICP: //scanmatchign with iterative closest point
           pm_icp(&lsr,&lsa);
           alg_str = (char *)"icp";
         break;
     }
     end_tick = rdtsc();
     cout  <<" et: " <<(double)(end_tick-start_tick)/CPU_FREQ<<endl;
    }catch(int err){};

    cout <<"Match result: "<<lsa.rx<<" "<<lsa.ry<<" "<<lsa.th*PM_R2D<<" "<<endl;
    #ifdef PM_GENERATE_RESULTS
      pm_plotScan4Thesis(&lsr,&lsa);
      sprintf(s,"results/sim_%s.png",alg_str);
//      dr_zoom();
       dr_save(s);
       dr_equal(0);
       pm_plotTime4Thesis(0,0,0);
       sprintf(s,"results/sim_conv_%s.png",alg_str);
       dr_save(s);
//       dr_zoom();
       dr_equal(1);
    #else
      cout <<endl<<i<<" next scan"<<endl;
      cout <<"Match result: "<<lsa.rx<<" "<<lsa.ry<<" "<<lsa.th*PM_R2D<<" "<<endl;
      pm_plotScan(&lsa,"green");
      cout <<"rounding of the corners is due to median filter!"<<endl;
      dr_fit();
      dr_zoom();
      dr_erase();
    #endif
}
//===========================================GROUND THRUTH=========================
//read the ground thruth measurements and matches them
//can output results into png,tex and text files
void ground_thruth(int algorithm)
{
  int  i,j,k,m;
  PMScan ls,lsr,lsa;
  FILE *fin, *ftab,*ffig,*ftxt;

  int cnt;
  long long int start_tick,end_tick;
  char *alg_str, match_nmb[1000],s[1000];

  double table[]={1,5,9,17,21,29,33,37,41,45};
  double correct[][3]={{39.41,2.12,13},{2.02,66.55,-14},{38.84,66.99,12},
                       {-21.94,68.33,-27},{14.04,68.33,-1},{35.62,9.33,26}};
  const int tcnt = 10;
  PMScan stable[4];
  bool bError;


  switch(algorithm)
  {
    case PM_PSM:    //polar scanmatching - matching bearing
        alg_str = (char *)"psm";
      break;
    case PM_PSM_C: //polar scanmatching - using cartesian equations
        alg_str = (char *)"psm_c";
      break;
    case PM_ICP: //scanmatchign with iterative closest point
        alg_str = (char *)"icp";
      break;
  }

  cout <<"Ground thruth test with: "<<alg_str<<endl;

  #ifdef PM_GENERATE_RESULTS
    sprintf(s,"results/tex_tab_%s.tex",alg_str);
    ftab = fopen(s,"w");
    sprintf(s,"results/tex_fig_%s.tex",alg_str);
    ffig = fopen(s,"w");
    sprintf(s,"results/txt_results_%s.txt",alg_str);
    ftxt = fopen(s,"w");
    // initialize table file
    fprintf(ftab,"\\begin{table*}\n\\scriptsize\n\\begin{tabular}{|c|c|c|c|c|c|c|}\n\\hline\n");
  #endif
  pm_init("ground_thruth.log", &fin);

  if(PM_LASER_Y!= 0)
  {
    cerr <<"FOR GROUND TRUTH EXPERIMENT SET PM_LASER_Y TO 0!"<<endl;
    exit(-1);
  }

  while(pm_readScan(fin, &ls)==0)
  {
//    m=3;
    ls.t += PM_TIME_DELAY;//compensating for time registration error compensation
    for(m=0;m<tcnt;m++)
    {
      if(fabs(ls.t-table[m])<0.005)
      {
        stable[0] = ls;        
        for(j=1;j<4;j++)
        {
          pm_readScan(fin, &ls);          
          stable[j] = ls;
        }

        //now 4 scans are stored in stable
//        //show scans
//        for(i=0;i<4;i++)
//        {
//          dr_erase();
//          pm_plotScan(&stable[i],dr_COLORS[i]);
//          dr_zoom();
//        }
        k=0;
        for(i=0;i<3;i++)
          for(j=i+1;j<4;j++)
          {
//            if(!(//m==2 && k==3 || m==3 && k==3 || m==3 && k==5 ||
//                 m==7 && k==0 || m==7 && k==1 || m==7 && k==2 || m==7 && k==4) )
//            if(k!=3) {k++;continue;};  //debug
            lsr = stable[i];
            lsa = stable[j];
            pm_median_filter(&lsr);
            pm_median_filter(&lsa);
            pm_find_far_points(&lsr);
            pm_find_far_points(&lsa);
            pm_segment_scan(&lsr);
            pm_segment_scan(&lsa);

            #ifdef PM_GENERATE_RESULTS
                sprintf(match_nmb,"%i_%i",m,k);
                pm_plotScan4Thesis(&lsr,&lsa);
                sprintf(s,"results/ground_init_%s_%s.png",alg_str,match_nmb);
                #ifdef SAVE_FIG
                  dr_save(s);//don't need to save initial...
                #endif
            #endif

            bError = false;
            PM_TYPE err=1,angle,err_idx;
            bool corridor=false;
            double c11,c12,c22,c33;
            usleep(30000);
            try{
              start_tick = rdtsc();
                switch(algorithm)
                {
                  case PM_PSM:    //polar scanmatching - matching bearing
                      pm_psm(&lsr,&lsa);
                      alg_str = (char *)"psm";
                    break;
                  case PM_PSM_C: //polar scanmatching - using cartesian equations
                      pm_cartesian_match(&lsr,&lsa);
                      alg_str = (char *)"psm_c";
                    break;
                  case PM_ICP: //scanmatchign with iterative closest point
                      pm_icp(&lsr,&lsa);
                      alg_str = (char *)"icp";
                    break;
                }
              end_tick = rdtsc();
              cout  <<" et: " <<(double)(end_tick-start_tick)/CPU_FREQ<<endl;
              err_idx = pm_error_index(&lsr,&lsa);
              corridor = pm_is_corridor(&lsa);

              if(corridor)
              {
                angle = pm_corridor_angle(&lsa);