polar_match.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002              polar_match.cpp  - matching laser scans in polar coord system
00003              designed for use with SICK LMS in cm res./361 meas. mode
00004              only 181 readings (1 deg res) are used (less accuracy but higher
00005              speed) - changes made to accomodate other lasers, but not tested properly yet.
00006                              -------------------
00007 begin           : Tue Nov 9 2004
00008 version         : 0.2
00009 copyright       : (C) 2005,2006,2007,2008,2009 by Albert Diosi and Lindsay Kleeman
00010 email           : albert.diosi@gmail.com
00011 comments        : - range units are cm; angle units are deg
00012                   - don't forget to set the optimization switch!
00013                   - if it works with float->change fabs-fabsf, floor->floorf
00014                   - why not add angle as well to the iterative least squares?
00015                      => bad convergence, error tends to be fixed with rotation
00016                   - try to make the C - coef for sigmoid function smaller with
00017                      with the as scanmatching converges, to increase the precision
00018                      as in dudek
00019 changed:
00020                 05/03/2007- add interpolation to icp
00021                 04/11/2005- bug fixed in pm_is_corridor. Bug pointed out by Alan Zhang
00022                 03/08/2005- simple implementation of IDC added not working yet - remove!
00023                 26/05/2005- projection filter of ICP fixed
00024                 7/12/2004 - dx,dy estimation interleaved with dth estimation
00025                             seems to work OK
00026                 3/12/2004 - iterative range least squares seems to work (for
00027                            dx,dy only), though it needs to be thoroughly tested
00028 
00029                 26/11/2004
00030 
00031 edited: (Ivan Dyanovski, ivan.dryanovski@gmail.com)
00032 
00033                 11/08/2010 - removed everything related to visualisation
00034                            - changed defines to private class members
00035                            - removed methods and variables not needed for basic
00036                              matching
00037 
00038 
00039 TODO: change the time measurement approach; do a proper cleanup. have doxygen comments. 
00040       - Comment colours used when GR is defined.
00041       - Document all magic constansts.(pm_psm: change of C after 10 steps)               
00042 ***************************************************************************/
00043 /****************************************************************************
00044     This file is part of polar_matching.
00045 
00046     polar_matching is free software; you can redistribute it and/or modify
00047     it under the terms of the GNU General Public License as published by
00048     the Free Software Foundation; either version 2 of the License, or
00049     (at your option) any later version.
00050 
00051     polar_matching is distributed in the hope that it will be useful,
00052     but WITHOUT ANY WARRANTY; without even the implied warranty of
00053     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00054     GNU General Public License for more details.
00055 
00056     You should have received a copy of the GNU General Public License
00057     along with polar_matching; if not, write to the Free Software
00058     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00059 ****************************************************************************/
00060 
00061 #include "polar_scan_matcher/polar_match.h"
00062 
00063 using namespace std;
00064 
00065 PolarMatcher::PolarMatcher()
00066 {
00067 
00068 }
00069 
00072 void PolarMatcher::pm_init()
00073 {
00074   pm_fi.resize(PM_L_POINTS);
00075   pm_si.resize(PM_L_POINTS);
00076   pm_co.resize(PM_L_POINTS);
00077 
00078   PM_FI_MIN = M_PI/2.0 - PM_FOV*PM_D2R/2.0; 
00079   PM_FI_MAX = M_PI/2.0 + PM_FOV*PM_D2R/2.0; 
00080   PM_DFI    = PM_FOV*PM_D2R/ ( PM_L_POINTS + 1.0 );
00081 
00082   for ( int i=0;i<PM_L_POINTS;i++ )
00083   {
00084     pm_fi[i] = ( ( float ) i ) *PM_DFI + PM_FI_MIN;
00085     pm_si[i] = sin ( pm_fi[i] );
00086     pm_co[i] = cos ( pm_fi[i] );
00087   }
00088 }//pm_init
00089 
00090 //-------------------------------------------------------------------------
00091 //filters the ranges with a median filter. x,y points are not upadted
00092 //ls - laser scan
00093 // seems like the median filter is a good thing!
00094 //if window is 5, then 3 points are needed in a bunch to surrive!
00095 //don't use this function with line fitting!
00096 void PolarMatcher::pm_median_filter ( PMScan *ls )
00097 {
00098   const int HALF_WINDOW  = 2;//2 to left 2 to right
00099   const int WINDOW = 2*HALF_WINDOW+1;
00100   PM_TYPE   r[WINDOW];
00101   PM_TYPE   w;
00102 
00103   int i,j,k,l;
00104 
00105   for ( i=0;i<PM_L_POINTS;i++ )
00106   {
00107     k=0;
00108     for ( j=i-HALF_WINDOW;j<=i+HALF_WINDOW;j++ )
00109     {
00110       l = ( ( j>=0 ) ?j:0 );
00111       r[k]=ls->r[ ( ( l < PM_L_POINTS ) ?l: ( PM_L_POINTS-1 ) ) ];
00112       k++;
00113     }
00114     //bubble sort r
00115     for ( j= ( WINDOW-1 );j>0;j-- )
00116       for ( k=0;k<j;k++ )
00117         if ( r[k]>r[k+1] ) // wrong order? - swap them
00118         {
00119           w=r[k];
00120           r[k]=r[k+1];
00121           r[k+1] = w;
00122         }
00123     ls->r[i] = r[HALF_WINDOW];//choose the middle point
00124   }
00125 }
00126 
00127 //-------------------------------------------------------------------------
00128 // segments scanpoints into groups based on range discontinuities
00129 // number 0 is reserved to segments with size 1
00130 // assumptions: too far points PM_MAX_RANGE - divide segments
00131 //              - bad points are only due to far points and mixed pixels
00132 // seems all right, except a far point can be the beginning of a new segment
00133 // if the next point is good and close -> it shouldn't make a difference
00134 void PolarMatcher::pm_segment_scan ( PMScan *ls )
00135 {
00136   const PM_TYPE   MAX_DIST = 20.0;//max range diff between conseq. points in a seg
00137   PM_TYPE   dr;
00138   int       seg_cnt = 0;
00139   int       i,cnt;
00140   bool      break_seg;
00141 
00142   seg_cnt = 1;
00143 
00144   //init:
00145   if ( fabs ( ls->r[0]-ls->r[1] ) <MAX_DIST ) //are they in the same segment?
00146   {
00147     ls->seg[0] = seg_cnt;
00148     ls->seg[1] = seg_cnt;
00149     cnt        = 2;    //2 points in the segment
00150   }
00151   else
00152   {
00153     ls->seg[0] = 0; //point is a segment in itself
00154     ls->seg[1] = seg_cnt;
00155     cnt        = 1;
00156   }
00157 
00158   for ( i=2;i<PM_L_POINTS;i++ )
00159   {
00160     //segment breaking conditions: - bad point;
00161     break_seg = false;
00162     if ( ls->bad[i] )
00163     {
00164       break_seg = true;
00165       ls->seg[i] = 0;
00166     }
00167     else
00168     {
00169       dr = ls->r[i]- ( 2.0*ls->r[i-1]-ls->r[i-2] );//extrapolate & calc difference
00170       if ( fabs ( ls->r[i]-ls->r[i-1] ) <MAX_DIST || ( ( ls->seg[i-1]==ls->seg[i-2] )
00171               && fabs ( dr ) <MAX_DIST ) )
00172       {
00173         //not breaking the segment
00174         cnt++;
00175         ls->seg[i] = seg_cnt;
00176       }
00177       else
00178         break_seg = true;
00179     }//if ls
00180     if ( break_seg ) // breaking the segment?
00181     {
00182       if ( cnt==1 )
00183       {
00184         //check first if the last three are not on a line by coincidence
00185         dr = ls->r[i]- ( 2.0*ls->r[i-1]-ls->r[i-2] );
00186         if ( ls->seg[i-2] == 0 && ls->bad[i] == 0 && ls->bad[i-1] == 0
00187                 && ls->bad[i-2] == 0 && fabs ( dr ) <MAX_DIST )
00188         {
00189           ls->seg[i]   = seg_cnt;
00190           ls->seg[i-1] = seg_cnt;
00191           ls->seg[i-2] = seg_cnt;
00192           cnt = 3;
00193         }//if ls->
00194         else
00195         {
00196           ls->seg[i-1] = 0;
00197           //what if ls[i] is a bad point? - it could be the start of a new
00198           //segment if the next point is a good point and is close enough!
00199           //in that case it doesn't really matters
00200           ls->seg[i] = seg_cnt;//the current point is a new segment
00201           cnt = 1;
00202         }
00203       }//if cnt ==1
00204       else
00205       {
00206         seg_cnt++;
00207         ls->seg[i] = seg_cnt;
00208         cnt = 1;
00209       }//else if cnt
00210     }//if break seg
00211   }//for
00212 }//pm_segment_scan
00213 
00214 //-------------------------------------------------------------------------
00215 // marks point further than a given distance PM_MAX_RANGE as PM_RANGE
00216 void PolarMatcher::pm_find_far_points ( PMScan *ls )
00217 {
00218   for ( int i=0;i<PM_L_POINTS;i++ )
00219   {
00220     if ( ls->r[i]>PM_MAX_RANGE )
00221       ls->bad[i] |= PM_RANGE;
00222   }//
00223 }
00224 
00225 //-------------------------------------------------------------------------
00226 //calculates an error index expressing the quality of the match
00227 //of the actual scan to the reference scan
00228 //has to be called after scan matching so the actual scan in expressed
00229 //in the reference scan coordinate system
00230 //return the average minimum Euclidean distance; MAXIMUM RANGE points
00231 //are not considered; number of non maximum range points have to be
00232 //smaller than a threshold
00233 //actual scan is compared to reference scan and vice versa, maximum is
00234 //taken
00235 //I could implement it in polar frame as well, would be O(n)
00236 PM_TYPE PolarMatcher::pm_error_index ( PMScan *lsr,PMScan *lsa )
00237 {
00238   int     i,j;
00239   PM_TYPE rx[PM_L_POINTS],ry[PM_L_POINTS],ax[PM_L_POINTS],ay[PM_L_POINTS];
00240   PM_TYPE x,y;
00241   PM_TYPE d,dmin,dsum,dx,dy;
00242   PM_TYPE dsum1;
00243   int     n,n1,rn=0,an=0;
00244   const   PM_TYPE HUGE_ERROR       = 1000000;
00245   const   int     MIN_POINTS = 100;
00246 
00247   lsa->th = norm_a ( lsa->th );
00248   PM_TYPE co = cos ( lsa->th ),si = sin ( lsa->th );
00249   PM_TYPE c,sig;
00250 
00251   //x axis equation si*x-co*y+c=0
00252   c = - ( lsa->rx*si-lsa->ry*co );//calc for the x axis of the actual frame
00253   //"signum" of a point from the lasers view substituted into the equation
00254   sig = si* ( lsa->rx+cos ( lsa->th+0.1 ) )-co* ( lsa->ry+sin ( lsa->th+0.1 ) ) +c;
00255 
00256   for ( i=0;i<PM_L_POINTS;i++ )
00257   {
00258     x = lsr->r[i]*pm_co[i];
00259     y = lsr->r[i]*pm_si[i];
00260     if ( !lsr->bad[i] && sig* ( si*x-co*y+c ) >0 )
00261     {
00262       rx[rn] = x;
00263       ry[rn++] = y;
00264     }//if
00265     if ( !lsa->bad[i] )
00266     {
00267       x = lsa->r[i]*pm_co[i];
00268       y = lsa->r[i]*pm_si[i];
00269       ax[an] = x*co-y*si+lsa->rx;
00270       ay[an] = x*si+y*co+lsa->ry;
00271       if ( ay[an]>0 )
00272       {
00273         an++;
00274       }
00275     }//if
00276   }//for i
00277 
00278   dsum = 0;n=0;
00279   for ( i=0;i<an;i++ )
00280   {
00281     dmin = 10000;
00282     for ( j=0;j<rn;j++ )
00283     {
00284       dx = rx[j]-ax[i];
00285       dy = ry[j]-ay[i];
00286       d = sqrt ( dx*dx+dy*dy );
00287       if ( d<dmin )
00288         dmin = d;
00289     }//for j
00290     if ( dmin<10000 )
00291     {
00292       n++;
00293       dsum+=dmin;
00294     }
00295   }//for i
00296 
00297   if ( n>0 )
00298   {
00299     dsum1 = dsum/ ( PM_TYPE ) n;
00300     n1    = n;
00301   }
00302   else
00303     return     HUGE_ERROR;
00304 
00305   //now checking the reference scan agains the actual
00306   dsum = 0;n=0;
00307   for ( i=0;i<rn;i++ )
00308   {
00309     dmin = 10000;
00310     for ( j=0;j<an;j++ )
00311     {
00312       dx = rx[i]-ax[j];
00313       dy = ry[i]-ay[j];
00314       d = sqrt ( dx*dx+dy*dy );
00315       if ( d<dmin )
00316         dmin = d;
00317     }//for j
00318     if ( dmin<10000 )
00319     {
00320       n++;
00321       dsum+=dmin;
00322     }
00323   }//for i
00324 
00325   if ( n>0 )
00326   {
00327     dsum = dsum/ ( PM_TYPE ) n;
00328   }
00329   else
00330     return     HUGE_ERROR;
00331 
00332   cout<<"pm_error_index: "<<n1<<" "<<dsum1<<" "<<n<<" "<<dsum<<endl;
00333 
00334   if ( n1>MIN_POINTS && n>MIN_POINTS )
00335   {
00336     if ( dsum1>dsum )
00337       return dsum1; //return the larger one
00338     else
00339       return dsum;
00340   }
00341   return     HUGE_ERROR;
00342 }
00343 
00344 //-------------------------------------------------------------------------
00345 //estimates the covariance matrix(c11,c12,c22,c33) (x,y,th) of
00346 //a scan match based on an error index (err-depends on how good the
00347 //match is), and the angle of the corridor if it is a corridor
00348 // for non corridors cov matrix is diagonal
00349 void PolarMatcher::pm_cov_est ( PM_TYPE err, double *c11,double *c12, double *c22, double *c33,
00350                   bool corridor, PM_TYPE corr_angle )
00351 {
00352 #define SQ(x) ((x)*(x))
00353   const double cov_x  = SQ ( 20 ); //10       // cm
00354   const double cov_y  = SQ ( 20 );//10      // cm
00355   const double cov_th = SQ ( 4.0*M_PI/180.0 ); // 2 deg
00356   //for corridors
00357   const double cov_along   = SQ ( 400 );   // cm
00358   const double cov_across  = SQ ( 30 ); // cm
00359 
00360   err = err-5;
00361   if ( err<1 )
00362     err = 1;
00363   if ( corridor ) //was the actual scan taken of a corridor?
00364   {
00365     double co = cos ( corr_angle );
00366     double si = sin ( corr_angle );
00367     *c11 = err* ( SQ ( co ) *cov_along+SQ ( si ) *cov_across );
00368     *c12 = err* ( -co*si* ( -cov_along+cov_across ) );
00369     *c22 = err* ( SQ ( si ) *cov_along+SQ ( co ) *cov_across );
00370     *c33 = err*cov_th;
00371   }//if
00372   else
00373   {
00374     *c12 = 0;
00375     *c11 = err*cov_x;
00376     *c22 = err*cov_y;
00377     *c33 = err*cov_th;
00378   }//else
00379 }//pm_cov_est
00380 
00381 //-------------------------------------------------------------------------
00382 // does scan matching using the equations for translation and orietation
00383 //estimation as in Lu & milios, however our matching bearing association rule
00384 //is used together with our association filter.
00385 //have to do an angle search othervise doesn't converge to 0!
00386 //weights implemented!
00387 PM_TYPE PolarMatcher::pm_psm_c ( PMScan *lsr,PMScan *lsa )
00388 {
00389 //   #define GR //comment out if no graphics necessary
00390   PMScan  act(PM_L_POINTS),  ref(PM_L_POINTS);//copies of actual and reference scans
00391   PM_TYPE rx,ry,rth,ax,ay,ath;//robot pos at ref and actual scans
00392   PM_TYPE t13,t23;
00393   PM_TYPE new_r[PM_L_POINTS];//interpolated r at measurement bearings
00394   int     new_bad[PM_L_POINTS];//bad flags of the interpolated range readings
00395   PM_TYPE C = 70*70;//weighting factor; see dudek00
00396   int     n = 0;//number of valid points
00397   int       iter,i,small_corr_cnt=0;
00398   PM_TYPE   abs_err=0,dx=0,dy=0,dth=0;//match error, actual scan corrections
00399 
00400   act = *lsa;
00401   ref = *lsr;
00402 
00403   rx =  ref.rx; ry = ref.ry; rth = ref.th;
00404   ax =  act.rx; ay = act.ry; ath = act.th;
00405 
00406   //transformation of actual scan laser scanner coordinates into reference
00407   //laser scanner coordinates
00408   t13 =  cos(rth) * ax + sin(rth) * ay - sin (rth) * ry - rx * cos(rth);
00409   t23 = -sin(rth) * ax + cos(rth) * ay - cos (rth) * ry + rx * sin(rth);
00410 
00411   ref.rx = 0;   ref.ry = 0;   ref.th = 0;
00412   act.rx = t13; act.ry = t23; act.th = ath-rth;
00413 
00414   ax = act.rx; ay = act.ry; ath = act.th;
00415   //from now on act.rx,.. express the lasers position in the ref frame
00416 
00417   iter = -1;
00418   while ( ++iter<PM_MAX_ITER && small_corr_cnt<3 ) //has to be 5 small corrections before stop
00419   {
00420     if ( ( fabs ( dx ) +fabs ( dy ) +fabs ( dth ) ) <PM_STOP_COND )
00421       small_corr_cnt++;
00422     else
00423       small_corr_cnt=0;
00424 
00425     act.rx = ax;act.ry = ay;act.th = ath;
00426     // convert range readings into ref frame
00427     pm_scan_project(&act,  new_r, new_bad);
00428 
00429     //---------------ORIENTATION SEARCH-----------------------------------
00430     //search for angle correction using crosscorrelation
00431     if ( iter%4 ==3 ) //(iter%2==1)
00432     {
00433        dth = pm_orientation_search(&ref, new_r, new_bad);
00434        ath += dth;
00435        continue;
00436     }
00437 
00438     //------------------------------------------translation-------------
00439     if ( iter>10 )
00440       C = 100;
00441 
00442     // do the weighted linear regression on the linearized ...
00443     // include angle as well
00444     PM_TYPE dr;
00445 
00446     //computation of the new dx1,dy1,dtheta1
00447     PM_TYPE X=0,Y=0,Xp=0,Yp=0,W=0,w;
00448     PM_TYPE sxx=0,sxy=0,syx=0, syy=0;
00449     PM_TYPE meanpx,meanpy,meanppx,meanppy;
00450     meanpx = 0;meanpy = 0;
00451     meanppx= 0;meanppy= 0;
00452 
00453     abs_err=0;
00454     n=0;
00455     for ( i=0;i<PM_L_POINTS;i++ )
00456     {
00457       dr = ref.r[i]-new_r[i];
00458       abs_err += fabs ( dr );
00459       //weight calculation
00460       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 )
00461       {
00462         // do the cartesian calculations....
00463 
00464         PM_TYPE x,y,xp,yp;
00465         x = new_r[i]*pm_co[i];//actual
00466         y = new_r[i]*pm_si[i];
00467         xp = ref.r[i]*pm_co[i]; //could speed up this..
00468         yp = ref.r[i]*pm_si[i];//reference
00469 
00470         w = C/ ( dr*dr+C );
00471 
00472         W += w;
00473         X += w*x;
00474         Y += w*y;
00475         Xp+= w*xp;
00476         Yp+= w*yp;
00477 
00478         sxx += w*xp*x;
00479         sxy += w*xp*y;
00480         syx += w*yp*x;
00481         syy += w*yp*y;
00482 
00483         n++;
00484       }
00485     }//for
00486     if (n < PM_MIN_VALID_POINTS || W < 0.01 ) //are there enough points?
00487     {
00488       cerr <<"pm_linearized_match: ERROR not enough points " << n << " " << W << endl;
00489       throw 1;//not enough points
00490     }
00491 
00492     dth = atan2 ( - ( X*Yp - Y*Xp + W* ( sxy-syx ) ),- ( X*Xp+Y*Yp-W* ( sxx+syy ) ) );
00493     dx  = ( Xp-X*cos ( dth ) +Y*sin ( dth ) ) /W;
00494     dy  = ( Yp-X*sin ( dth )-Y*cos ( dth ) ) /W;
00495 
00496     //correction thanks to Alan Zhang:
00497     PM_TYPE ax_old,ay_old;
00498     ax_old = ax;
00499     ay_old = ay;
00500 
00501     ax = cos ( dth ) *ax_old - sin ( dth ) *ay_old + dx;
00502     ay = sin ( dth ) *ax_old + cos ( dth ) *ay_old + dy;
00503     ath+= dth;
00504 
00505     dth *=PM_R2D;
00506   }//for iter
00507 
00508   lsa->rx =ax;lsa->ry=ay;lsa->th=ath;
00509   return ( abs_err/n );
00510 }//pm_psm_c
00511 
00512 //-------------------------------------------------------------------------
00513 // minimizes least square error through changing lsa->rx, lsa->ry,lsa->th
00514 // this looks for angle too, like pm_linearized_match_proper,execept it
00515 // fits a parabola to the error when searching for the angle and interpolates.
00516 PM_TYPE PolarMatcher::pm_psm ( PMScan *lsr,PMScan *lsa )
00517 {
00518   PMScan    act(PM_L_POINTS),  ref(PM_L_POINTS);//copies of actual and reference scans
00519   PM_TYPE   rx,ry,rth,ax,ay,ath;//robot pos at ref and actual scans
00520   PM_TYPE   t13,t23;
00521   PM_TYPE   new_r[PM_L_POINTS];//interpolated r at measurement bearings
00522   int       new_bad[PM_L_POINTS];//bad flags of the interpolated range readings
00523   PM_TYPE   C = 70*70;//weighting factor; see dudek00
00524   int       iter,small_corr_cnt=0;
00525   PM_TYPE   dx=0,dy=0,dth=0;//match error, actual scan corrections
00526   PM_TYPE   avg_err = 100000000.0;
00527 
00528   act = *lsa;
00529   ref = *lsr;
00530 
00531   rx =  ref.rx; ry = ref.ry; rth = ref.th;
00532   ax =  act.rx; ay = act.ry; ath = act.th;
00533 
00534   //transformation of actual scan laser scanner coordinates into reference
00535   //laser scanner coordinates
00536   t13 =  cos(rth) * ax + sin(rth) * ay-sin (rth) * ry - rx * cos (rth);
00537   t23 = -sin(rth) * ax + cos(rth) * ay-cos (rth) * ry + rx * sin (rth);
00538 
00539   ref.rx = 0;   ref.ry = 0;   ref.th = 0;
00540   act.rx = t13; act.ry = t23; act.th = ath-rth;
00541 
00542   ax = act.rx; ay = act.ry; ath = act.th;
00543   //from now on act.rx,.. express the lasers position in the ref frame
00544 
00545   iter = -1;
00546   while ( ++iter<PM_MAX_ITER && small_corr_cnt<3 ) //has to be 5 small corrections before stop
00547   {
00548     if ( ( fabs ( dx ) +fabs ( dy ) +fabs ( dth ) ) <PM_STOP_COND )
00549       small_corr_cnt++;
00550     else
00551       small_corr_cnt=0;
00552 
00553     act.rx = ax;act.ry = ay;act.th = ath;
00554     pm_scan_project(&act,  new_r, new_bad);
00555 
00556     //---------------ORIENTATION SEARCH-----------------------------------
00557     //search for angle correction using crosscorrelation
00558     if ( iter%2 ==1 )
00559     {
00560        dth = pm_orientation_search(&ref, new_r, new_bad);
00561        ath += dth;
00562        continue;
00563     }
00564 
00565     //------------------------------------------translation-------------
00566     //reduce C with time to consider only the best matches
00567     if ( iter>10 )
00568       C = 100;
00569 
00570     avg_err = pm_translation_estimation(&ref, new_r, new_bad, C, &dx, &dy);
00571 
00572     ax += dx;
00573     ay += dy;
00574 
00575 //    //for SIMULATION iteration results..
00576 
00577   }//while iter
00578 
00579   lsa->rx =ax;lsa->ry=ay;lsa->th=ath;
00580   return ( avg_err);
00581 }//pm_linearized_match_int_angle
00582 
00583 //-------------------------------------------------------------------------
00584 //calculate distance of the point (x3,y3) from a line (defined by (x1,y1)
00585 //and (x2,y2)). Returns the distance to the line or -1 if the
00586 //projection of (x3,y3) falls outside the line segment defined by (x1,y1)
00587 //and (x2,y2).
00588 //The projection of (x3,y3) onto the line is also returned in x,y
00589 PM_TYPE PolarMatcher::point_line_distance ( PM_TYPE x1, PM_TYPE y1, PM_TYPE x2, PM_TYPE y2,
00590                               PM_TYPE x3, PM_TYPE y3,PM_TYPE *x, PM_TYPE *y )
00591 {
00592   PM_TYPE ax,ay,t1,D;
00593   ax = x2-x1;
00594   ay = y2-y1;
00595   D =  sqrt ( ax*ax+ay*ay );
00596   if ( D <0.0001 )
00597   {
00598     cerr <<"point_line_distance: unexpected D:" <<D<<endl;
00599     return -1;
00600   }
00601   t1 =  - ( -ax*x3 + ay*y1 + ax*x1 - ay*y3 ) / ( ax*ax+ay*ay );
00602   if ( t1<0 || t1>1 )   // projection falls outside the line segment
00603   {
00604     return -1;
00605   }
00606   *x = x1+t1*ax;
00607   *y = y1+t1*ay;
00608   return ( sqrt ( ( x3-*x ) * ( x3-*x ) + ( y3-*y ) * ( y3-*y ) ) );//distance of line to p.
00609 }//  point_line_distance
00610 
00611 //-------------------------------------------------------------------------
00615 void PolarMatcher::pm_scan_project(const PMScan *act,  PM_TYPE   *new_r,  int *new_bad)
00616 {
00617     PM_TYPE   r[PM_L_POINTS],fi[PM_L_POINTS];//actual scan in ref. coord. syst.
00618     PM_TYPE   x,y;
00619     int       i;
00620     PM_TYPE   delta;
00621 
00622     // convert range readings into ref frame
00623     // this can be speeded up, by connecting it with the interpolation
00624     for ( i=0;i<PM_L_POINTS;i++ )
00625     {
00626       delta   = act->th + pm_fi[i];
00627       x       = act->r[i]*cos ( delta ) + act->rx;
00628       y       = act->r[i]*sin ( delta ) + act->ry;
00629       r[i]    = sqrt ( x*x+y*y );
00630       fi[i]   = atan2 ( y,x );
00631       //handle discontinuity at pi
00632       if(x<0 && y<0)
00633         fi[i] += 2.0*M_PI;
00634 
00635       new_r[i]  = 10000;//initialize big interpolated r;
00636       new_bad[i]= PM_EMPTY;//for interpolated r;
00637 
00638     }//for i
00639 
00640     //------------------------INTERPOLATION------------------------
00641     //calculate/interpolate the associations to the ref scan points
00642     //algorithm ignores crosings at 0 and 180 -> to make it faster
00643     for ( i=1;i<PM_L_POINTS;i++ )
00644     {
00645       //i points to the angles in the actual scan
00646 
00647       // i and i-1 has to be in the same segment, both shouldn't be bad
00648       // and they should be larger than the minimum angle
00649       if ( act->seg[i] != 0 && act->seg[i] == act->seg[i-1] && !act->bad[i] &&
00650               !act->bad[i-1] /* && fi[i]>PM_FI_MIN && fi[i-1]>PM_FI_MIN*/ )
00651       {
00652         //calculation of the "whole" parts of the angles
00653         int j0,j1;
00654         PM_TYPE r0,r1,a0,a1;
00655         bool occluded;
00656         if ( fi[i]>fi[i-1] ) //are the points visible?
00657         {
00658           //visible
00659           occluded = false;
00660           a0  = fi[i-1];
00661           a1  = fi[i];
00662 
00663           j0  =  (int) ceil ( ( fi[i-1] - PM_FI_MIN ) /PM_DFI );
00664           j1  =  (int) floor ( ( fi[i] - PM_FI_MIN ) /PM_DFI );
00665           r0  = r[i-1];
00666           r1  = r[i];
00667         }
00668         else
00669         {
00670           //invisible - still have to calculate to filter out points which
00671           occluded = true; //are covered up by these!
00672           //flip the points-> easier to program
00673           a0  = fi[i];
00674           a1  = fi[i-1];
00675 
00676           j0  =  (int) ceil ( ( fi[i] - PM_FI_MIN ) /PM_DFI );
00677           j1  =  (int) floor ( ( fi[i-1] - PM_FI_MIN ) /PM_DFI );
00678           r0  = r[i];
00679           r1  = r[i-1];
00680         }
00681         //here fi0 is always smaller than fi1!
00682 
00683         //interpolate for all the measurement bearings beween fi0 and fi1
00684         while ( j0<=j1 ) //if at least one measurement point difference, then ...
00685         {
00686          // cout <<( ( PM_TYPE ) j0*PM_DFI )<<" "<<( ( ( PM_TYPE ) j0*PM_DFI +PM_FI_MIN )-a0 )<<endl;
00687           PM_TYPE ri = ( r1-r0 ) / ( a1-a0 ) * ( ( ( PM_TYPE ) j0*PM_DFI+PM_FI_MIN )-a0 ) +r0;
00688 
00689           //if fi0 -> falls into the measurement range and ri is shorter
00690           //than the current range then overwrite it
00691           if ( j0>=0 && j0<PM_L_POINTS && new_r[j0]>ri )
00692           {
00693             new_r[j0]    = ri;//overwrite the previous reading
00694             new_bad[j0] &=~PM_EMPTY;//clear the empty flag
00695             if ( occluded ) //check if it was occluded
00696               new_bad[j0] = new_bad[j0]|PM_OCCLUDED;//set the occluded flag
00697             else
00698               new_bad[j0] = new_bad[j0]&~PM_OCCLUDED;
00699             //the new range reading also it has to inherit the other flags
00700             new_bad[j0] |= act->bad[i];//superfluos - since act.bad[i] was checked for 0
00701             new_bad[j0] |= act->bad[i-1];//superfluos - since act.bad[i-1] was checked for 0
00702 
00703           }
00704           j0++;//check the next measurement angle!
00705         }//while
00706       }//if act
00707     }//for i
00708 }//pm_scan_project
00709 
00710 //-------------------------------------------------------------------------
00718 PM_TYPE PolarMatcher::pm_orientation_search(const PMScan *ref, const PM_TYPE *new_r, const int *new_bad)
00719 {
00720       int i;
00721       int       window       = PM_SEARCH_WINDOW;//20;//+- width of search for correct orientation
00722       PM_TYPE   dth=0;//\actual scan corrections
00723 
00724       //pm_fi,ref.r - reference points
00725       PM_TYPE e,err[PM_L_POINTS];       // the error rating
00726       PM_TYPE beta[PM_L_POINTS];// angle for the corresponding error
00727       PM_TYPE n;
00728       int k=0;
00729 
00730       for ( int di=-window;di<=window;di++ )
00731       {
00732         n=0;e=0;
00733 
00734         int min_i,max_i;
00735         if ( di<=0 )
00736           {min_i = -di;max_i=PM_L_POINTS;}
00737         else
00738           {min_i = 0;max_i=PM_L_POINTS-di;}
00739 
00740         for ( i=min_i;i<max_i;i++ ) //searching through the actual points
00741         {
00742           //if fi[i],r[i] isn't viewed from the back, isn't moving
00743           // and isn't a solitary point, then try to associate it ..
00744           //also fi[i] is within the angle range ...
00745 
00746           if ( !new_bad[i] && !ref->bad[i+di] )
00747           {
00748             e += fabs ( new_r[i]-ref->r[i+di] );
00749             n++;
00750           }
00751 
00752         }//for i
00753 
00754         if ( n>0 )
00755           err[k]  = e/n;//don't forget to correct with n!
00756         else
00757           err[k]  = 10000;//don't forget to correct with n!
00758         beta[k] = di;
00759         k++;
00760       }//for dfi
00761 
00762       //now search for the global minimum
00763       //later I can make it more robust
00764       //assumption: monomodal error function!
00765       PM_TYPE emin = 1000000;
00766       int   imin;
00767       for ( i=0;i<k;i++ )
00768         if ( err[i]<emin )
00769         {
00770           emin = err[i];
00771           imin = i;
00772         }
00773       if ( err[imin]>=10000 )
00774       {
00775         cerr <<"Polar Match: orientation search failed" <<err[imin]<<endl;
00776         throw 1;
00777       }
00778       dth = beta[imin]*PM_DFI;
00779 
00780       //interpolation
00781       if ( imin>=1 && imin< ( k-1 ) ) //is it not on the extreme?
00782       {
00783         //lets try interpolation
00784         PM_TYPE D = err[imin-1]+err[imin+1]-2.0*err[imin];
00785         PM_TYPE d=1000;
00786         if ( fabs ( D ) >0.01 && err[imin-1]>err[imin] && err[imin+1]>err[imin] )
00787           d= ( err[imin-1]-err[imin+1] ) /D/2.0;
00788 
00789         if ( fabs ( d ) <1 )
00790           dth+=d*PM_DFI;
00791       }//if
00792 
00793      return(dth);
00794 }//pm_orientation_search
00795 
00796 PM_TYPE PolarMatcher::pm_translation_estimation(const PMScan *ref, const PM_TYPE *new_r, const int *new_bad, PM_TYPE C, PM_TYPE *dx, PM_TYPE *dy)
00797 {
00798   // do the weighted linear regression on the linearized ...
00799   // include angle as well
00800   int i;
00801   PM_TYPE hi1, hi2,hwi1,hwi2, hw1=0,hw2=0,hwh11=0;
00802   PM_TYPE hwh12=0,hwh21=0,hwh22=0,w;
00803   PM_TYPE dr;
00804   PM_TYPE abs_err = 0;
00805   int     n = 0;
00806   for ( i=0;i<PM_L_POINTS;i++ )
00807   {
00808     dr = ref->r[i]-new_r[i];
00809     abs_err += fabs ( dr );
00810 
00811     //weight calculation
00812     if (ref->bad[i] == 0            && 
00813         new_bad[i]  == 0            && 
00814         new_r[i]    <  PM_MAX_RANGE && 
00815         new_r[i]    > 10.0          && 
00816         fabs(dr)    < PM_MAX_ERROR  )
00817     {
00818       //weighting according to DUDEK00
00819 
00820       w = C / (dr*dr + C);
00821       n++;
00822 
00823       //proper calculations of the jacobian
00824       hi1 = pm_co[i];//xx/new_r[i];//this the correct
00825       hi2 = pm_si[i];//yy/new_r[i];
00826 
00827       hwi1 = hi1*w;
00828       hwi2 = hi2*w;
00829 
00830       //par = (H^t*W*H)^-1*H^t*W*dr
00831       hw1 += hwi1*dr;//H^t*W*dr
00832       hw2 += hwi2*dr;
00833 
00834       //H^t*W*H
00835       hwh11 += hwi1*hi1;
00836       hwh12 += hwi1*hi2;
00837       hwh21 += hwi2*hi1; //should take adv. of symmetricity!!
00838       hwh22 += hwi2*hi2;
00839     }
00840     else
00841     {
00842 /*
00843       if (ref->bad[i] != 0) printf("rb ");
00844       if (new_bad[i]  != 0) printf("nb ");
00845       if (new_r[i]    <  PM_MAX_RANGE) printf("MR ");
00846       if (fabs(dr)    < PM_MAX_ERROR) printf("ME ");
00847       printf(".\n");
00848   */
00849     }
00850   }//for i
00851   if ( n<PM_MIN_VALID_POINTS ) //are there enough points?
00852   {
00853     cerr <<"(i) pm_translation_estimation: ERROR not enough points" << n <<endl;
00854     throw 1;//not enough points
00855   }
00856 
00857   //calculation of inverse
00858   PM_TYPE D;//determinant
00859   PM_TYPE inv11,inv21,inv12,inv22;//inverse matrix
00860 
00861   D = hwh11*hwh22-hwh12*hwh21;
00862   if ( D<0.001 )
00863   {
00864     cerr <<"pm_linearized_match: ERROR determinant to small! "<<D<<endl;
00865     throw 1;
00866   }
00867   inv11 =  hwh22/D;
00868   inv12 = -hwh12/D;
00869   inv21 = -hwh12/D;
00870   inv22 =  hwh11/D;
00871 
00872   *dx = inv11*hw1+inv12*hw2;
00873   *dy = inv21*hw1+inv22*hw2;
00874   return(abs_err/n);
00875 }//pm_translation_estimation
00876 


polar_scan_matcher
Author(s): Ivan Dryanovski
autogenerated on Thu Mar 23 2017 03:34:38