main.cpp

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

                PMScan ls;
                ls = lsa;
                ls.rx =ls.ry = ls.th = 0;
                #ifndef PM_GENERATE_RESULTS
                  pm_plotScan(&ls,"green",1);
                #endif
                cout <<"corridor"<<endl;
              }
              else
                angle = 0;
              pm_cov_est(err_idx, &c11,&c12, &c22, &c33,corridor, angle);
              cout <<"Error index: "<<err_idx<<" angle "<<angle*PM_R2D<<endl;
              cout <<"COV: "<<sqrt(c11)<<" "<<c12/sqrt(c11)/sqrt(c22)<<" "<<sqrt(c22)<<" "<<sqrt(c33)*PM_R2D<<endl;
              #ifndef PM_GENERATE_RESULTS
                dr_cov_ellipse(0,0,c11,c12,c22,"black");
              #endif
            }catch(int err)
            {
              cout <<"error_caught"<<endl;
              bError = true;
            };

            cout <<endl<<k<<" =============location: "<<m<<endl;
            cout <<"Match result: "<<err<<" "<<lsa.rx<<" "<<lsa.ry<<" "<<lsa.th*PM_R2D<<" "<<endl;
            cout <<"match "<<k<<" Error: "<<lsa.rx-correct[k][0]<<" "<<lsa.ry-correct[k][1]
                    <<" "<<lsa.th*PM_R2D-correct[k][2]<<endl;


           #ifdef PM_GENERATE_RESULTS
             pm_plotScan4Thesis(&lsr,&lsa);
             if(bError)
             {
                dr_text(0,0,1,1,"ERROR","black");
                dr_replot();
             }
             char strg[1000];
             sprintf(strg,"SCENE %i",m);
             dr_text_pix(250,40,strg,"black");

             sprintf(s,"results/ground_%s_%s.png",alg_str,match_nmb);
       //      dr_zoom();
              #ifdef SAVE_FIG
                dr_save(s);
              #endif
              dr_equal(0);
              int iter;
              double time;
              pm_plotTime4Thesis(correct[k][0],correct[k][1],correct[k][2],&iter,&time);
              sprintf(s,"results/ground_conv_%s_%s.png",alg_str,match_nmb);
              dr_text_pix(250,40,strg,"black");
              #ifdef SAVE_FIG
                dr_save(s);
              #endif
//              dr_zoom();
              dr_equal(1);

             double adx,ady,adth;
             adx=fabs(lsa.rx-correct[k][0]);
             ady=fabs(lsa.ry-correct[k][1]);
             adth=fabs(lsa.th*PM_R2D-correct[k][2]);

             //print into table file
             if(k==0)
               fprintf(ftab,"%i ",m);
             if(bError)
               fprintf(ftab," & ERROR ");
             else{
               fprintf(ftab," & $\\begin{array}{c} (%.1lf,~%.1lf,~ %.1lf) \\\\ %.1i,~%.1lf~ \\end{array}$\n ",adx,ady,adth,iter,time);
             }

             //print into txt file table file
             fprintf(ftxt,"%i %i %i %0.1lf %.1lf %.1lf %.1lf\n ",m,k,iter,time,adx,ady,adth);

             //print into fig. file
//             char *m_name[]={(char*)"",(char*)"2",(char*)"2"};

             if(k%3==0)
             {
               if(k!=0)
               {
                fprintf(ffig,"\\caption{Experiment with ground thruth. Scene %i, location %i-%i with %s.}\n",m,k-3,k-1,alg_str);
                fprintf(ffig,"\\label{fig:gt%i_%i%i_%s}\n\\end{center}\n\\end{figure*}\n",m,k-3,k-1,alg_str);
               }
               if(k!=9)
               fprintf(ffig,"\\begin{figure*}\n\\begin{center}\n");
             }
             sprintf(s,"results/ground_init_%s_%s",alg_str,match_nmb);
             fprintf(ffig,"\t\\includegraphics[width=0.3\\columnwidth]{fig_matching/%s}\n",s);
             sprintf(s,"results/ground_%s_%s",alg_str,match_nmb);
             fprintf(ffig,"\t\\includegraphics[width=0.3\\columnwidth]{fig_matching/%s}\n",s);
             sprintf(s,"results/ground_conv_%s_%s",alg_str,match_nmb);
             fprintf(ffig,"\t\\includegraphics[width=0.3\\columnwidth]{fig_matching/%s}\n",s);
           #else
              pm_plotScan(&lsr,"black",2);
              pm_plotScan(&lsa,"red",1);
              dr_zoom();
              dr_erase();
            #endif
            k++;
          }
          #ifdef PM_GENERATE_RESULTS
            fprintf(ftab,"\\\\\n");
            fprintf(ftab,"\\hline \n");
            fprintf(ffig,"\\caption{Experiment with ground thruth. Scene %i, location %i-%i with %s.}\n",m,k-3,k-1,alg_str);
            fprintf(ffig,"\\label{fig:gt%i_%i%i_%s}\n\\end{center}\n\\end{figure*}\n",m,k-3,k-1,alg_str);
          #endif
        }//if
    }//for


  }//while
  #ifdef PM_GENERATE_RESULTS
    fprintf(ftab,"\\end{tabular}\n\\caption{Absolute errors in x[cm],y[cm],$\\theta [^\\circ]$,");
    fprintf(ftab,"number of iterations and runtime [ms] of the $%s$ algorithm in the experiments with ground truth.}\n",
      alg_str);
    fprintf(ftab,"\\label{tab:ground_%s}\n\\end{table*}\n",alg_str);

    fclose(ftab);
    fclose(ffig);
    fclose(ftxt);
  #endif

}//groud_thruth

// testing the angle histogram method for discriminating corridors from
// rooms, and also matching the orientation in corridors
void test_angle_histogram(void)
{
   int  i,j,k,m;
  PMScan ls,lsr,lsa;
  FILE *fin;
  int cnt;
  long long int start_tick,end_tick;

  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];

  pm_init("ground_thruth.log", &fin);

  while(pm_readScan(fin, &ls)==0)
  {
//     m=2;
    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(k!=0) {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);
            try{
              start_tick = rdtsc();
              pm_is_corridor(&lsr);

              
              end_tick = rdtsc();
              cout  <<" et: " <<(double)(end_tick-start_tick)/CPU_FREQ<<endl;
            }catch(int err)
            {
              cout <<"error_caught"<<endl;
            };
            cout <<endl<<i<<" =============location: "<<m<<endl;
            cout <<"Match result: "<<lsa.rx<<" "<<lsa.ry<<" "<<lsa.th*PM_R2D<<" "<<endl;
            cout <<"match "<<k<<" Error: "<<lsa.rx-correct[k][0]<<" "<<lsa.ry-correct[k][1]
                    <<" "<<lsa.th*PM_R2D-correct[k][2]<<endl;
//            dr_zoom();
            dr_erase();
            k++;
          }
      }//if
    }//for
  }//while
}//void test_angle_histogram(void);

void convergence_map(int algorithm)
{
  #define SQ(x) (x)*(x)

  int  i,j,k,m;
  PMScan ls,lsr,lsa;
  FILE *fin,*fout;

  int cnt;
  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;
  int inside=0,total=0,g_inside=0,g_total=0;
  const int MAX_RES = 10000;
  PM_TYPE res_x[MAX_RES],res_y[MAX_RES];//storing results for later display
  int res_cnt;
  
  dr_erase();
  
  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 <<"Convergence map test with: "<<alg_str<<endl;

  pm_init("ground_thruth.log", &fin);

  if(PM_LASER_Y!= 0)
  {
    cerr <<"FOR CONVERGENCE MAP EXPERIMENT SET PM_LASER_Y TO 0!"<<endl;
    exit(-1);
  }
  //open file to write results
  sprintf(s,"results/conv_map_%s_res.txt",alg_str);
  fout = fopen(s,"w");

  
  while(pm_readScan(fin, &ls)==0)
  {
    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;
        }

        k=0;
        for(i=0;i<3;i++)
          for(j=i+1;j<4;j++)
          {
//            if(!(m==9 && k==2)) {k++;continue;};  //interested only in scene 9 match 2
            if(!(k==2)) {k++;continue;};  //interested only in match 2 for all scenes
            res_cnt=0;
            inside = 0;
            total  = 0;
            dr_erase();
            
            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);


            PM_TYPE x,y,th;
            PM_TYPE dx = 10.0, dy = 10.0;
            for (x = -250;x<250;x+=dx)
            {
              for (y = -250;y<250;y+=dy)
              {
                 lsa.rx=x;lsa.ry=y;lsa.th=0;
                 bError = false;
                 PM_TYPE err=1,angle,err_idx;
                 bool corridor=false;
                 try{
                     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;
                     }
                   //read the file containing the result and plot it
                   FILE *f;
                   int cnt =5;
                   int iter=-1;
                   double t=-1,xx,yy,th,lx,ly;

                   f=fopen(PM_TIME_FILE,"r");
                   if(f==NULL)
                   {
                     cout <<"could not open file"<<endl;
                     continue;
                   }
                   while(cnt==5)
                   {
                     cnt=fscanf(f,"%i %lf %lf %lf %lf\n",&iter,&t,&xx,&yy,&th);
                     if(cnt==5)
                     {
                       if(iter!=0)
                         dr_line(lx,ly,xx,yy,"black");
                       lx = xx;
                       ly = yy;
                     }
                   }//whi
                   fclose(f);                   
                   dr_circle(lsa.rx,lsa.ry,3.0,"red");
                   if(sqrt(SQ(lsa.rx-correct[k][0]) +SQ(lsa.ry-correct[k][1]))<10
                      && fabs(correct[k][2] -lsa.th*PM_R2D)<2)
                     inside++;
                   res_x[res_cnt] = lsa.rx;
                   res_y[res_cnt] = lsa.ry;
                   res_cnt++;
                   
                 }catch(int err)
                 {
                   cout <<"error_caught"<<endl;
                   bError = true;
                   dr_circle(x,y,3.0,"black");
                 };
                 total++;
              }//for y
            }//for x
            dr_line(correct[k][0]-20,correct[k][1]+1,correct[k][0]+20,correct[k][1]+1 ,"gray");
            dr_line(correct[k][0]-20,correct[k][1]  ,correct[k][0]+20,correct[k][1]   ,"gray");
            dr_line(correct[k][0]-20,correct[k][1]-1,correct[k][0]+20,correct[k][1]-1 ,"gray");
            dr_line(correct[k][0]+1,correct[k][1]-20,correct[k][0]+1 ,correct[k][1]+20,"gray");
            dr_line(correct[k][0]  ,correct[k][1]-20,correct[k][0]   ,correct[k][1]+20,"gray");
            dr_line(correct[k][0]-1,correct[k][1]-20,correct[k][0]-1 ,correct[k][1]+20,"gray");
            for(int re=0;re<res_cnt;re++)
              dr_circle(res_x[re],res_y[re],3,"yellow");
//            dr_circle(correct[k][0],correct[k][1],10,"green");
            g_inside += inside;
            g_total  += total;
            fprintf(fout,"%i %i %.2f\n",m,k,((float)inside)/((float)total)*25.0);
            dr_fit();
            #ifdef SAVE_FIG
              sprintf(s,"results/conv_map_%s_%i_%i.png",alg_str,m,k);
              dr_save(s);
            #endif
            cout <<alg_str<<": inside: "<<" "<<inside<<" total: "<<total<<endl;
            //dr_zoom();
            k++;
          }
        }//if
    }//for
  }//while
  fprintf(fout,"average %.2f",((float)g_inside)/((float)g_total)*25.0);
  fclose(fout);
}