polar_match.h
Go to the documentation of this file.
00001 /***************************************************************************
00002                           polar_match.h  - 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)
00006                              -------------------
00007     begin                : Tue Nov 9 2004
00008     version              : 0.2    
00009     copyright            : (C) 2005 by Albert Diosi and Lindsay Kleeman
00010     email                : albert.diosi@gmail.com
00011     comments             : range units are cm; angle units are deg
00012                            the laser is on the robots y axis
00013                            - in scan projections, occluded ref scanpoints aren't removed!
00014     changed:
00015                           14/03/2005 removing of unnecessary code
00016  ***************************************************************************/
00017  /****************************************************************************
00018     This file is part of polar_matching.
00019 
00020     polar_matching is free software; you can redistribute it and/or modify
00021     it under the terms of the GNU General Public License as published by
00022     the Free Software Foundation; either version 2 of the License, or
00023     (at your option) any later version.
00024 
00025     polar_matching is distributed in the hope that it will be useful,
00026     but WITHOUT ANY WARRANTY; without even the implied warranty of
00027     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00028     GNU General Public License for more details.
00029 
00030     You should have received a copy of the GNU General Public License
00031     along with polar_matching; if not, write to the Free Software
00032     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00033 ****************************************************************************/
00034 
00035 #ifndef POLAR_SCAN_MATCHING_POLAR_MATCHER_H
00036 #define POLAR_SCAN_MATCHING_POLAR_MATCHER_H
00037 
00038 #include <stdio.h>
00039 #include <iostream>
00040 #include <unistd.h>
00041 #include <math.h>
00042 #include <stdlib.h>
00043 #include <string.h>
00044 #include <vector>
00045 
00046 #define PM_TYPE             double // change it to double for higher accuracy  and lower speed
00047 
00048 // range reading errors
00049 #define PM_RANGE     1  // range reading is longer than PM_MAX_RANGE
00050 #define PM_MOVING    2  // range reading corresponds to a moving point
00051 #define PM_MIXED     4  // range reading is a mixed pixel
00052 #define PM_OCCLUDED  8  // range reading is occluded
00053 #define PM_EMPTY     16 // at that bearing there is no measurment (between 2
00054                         //    segments there is no interpolation!)
00055 
00056 #define PM_ODO  -1 //show results with odometry only in mapping_with_matching()
00057 #define PM_PSM   1 //polar scanmatching - matching bearing
00058 #define PM_PSM_C 2 //polar scanmatching - using cartesian equations
00059 #define PM_ICP   3 //scanmatchign with iterative closest point
00060 #define PM_IDC   4 //lu's iterative dual correspondence - not implemented,
00061                    //Guttman's external code is used for comparisson
00062 #define PM_MBICP 5 //scanmatchign with metric based ICP
00063 
00064 const double PM_D2R = M_PI/180.0; // degrees to rad
00065 const double PM_R2D = 180.0/M_PI; // rad to degrees
00066 
00067 //const int     PM_L_POINTS = 681;
00068 
00069 //might want to consider doubles for rx,ry -> in a large environment?
00070 struct PMScan
00071 {
00072   PMScan(int nPoints)
00073   {
00074     r.resize(nPoints);
00075     x.resize(nPoints);
00076     y.resize(nPoints);
00077     bad.resize(nPoints);
00078     seg.resize(nPoints);
00079   }
00080 
00081   PM_TYPE  rx;   //robot odometry pos
00082   PM_TYPE  ry;   //robot odometry pos
00083   PM_TYPE  th;   //robot orientation 
00084   std::vector<PM_TYPE>  r;//[cm] - 0 or negative ranges denote invalid readings.
00085   std::vector<PM_TYPE>  x;//[cm]
00086   std::vector<PM_TYPE>  y;//[cm]
00087   std::vector<int>      bad;// 0 if OK
00088                             //sources of invalidity - too big range;
00089                             //moving object; occluded;mixed pixel
00090   std::vector<int>      seg;//nuber describing into which segment the point belongs to
00091 };
00092 
00093 class PolarMatcher
00094 {
00095   private:
00096 
00097     //calculates an error index expressing the quality of the match
00098     //of the actual scan to the reference scan
00099     //has to be called after scan matching so the actual scan in expressed
00100     //in the reference scan coordinate system
00101     //return the average minimum Euclidean distance; MAXIMUM RANGE points
00102     //are not considered; number of non maximum range points have to be
00103     //smaller than a threshold
00104     PM_TYPE pm_error_index(PMScan *lsr,PMScan *lsa);
00105 
00106     //estimates the covariance matrix(c11,c12,c22,c33) (x,y,th) of
00107     //a scan match based on an error index (err-depends on how good the
00108     //match is), and the angle of the corridor if it is a corridor
00109     void pm_cov_est(PM_TYPE err, double *c11,double *c12, double *c22, double *c33,
00110                         bool corridor=false, PM_TYPE corr_angle=0);
00111 
00112     void pm_scan_project(const PMScan *act,  PM_TYPE   *new_r,  int *new_bad);
00113     PM_TYPE pm_orientation_search(const PMScan *ref, const PM_TYPE *new_r, const int *new_bad);
00114     PM_TYPE pm_translation_estimation(const PMScan *ref, const PM_TYPE *new_r, const int *new_bad, PM_TYPE C, PM_TYPE *dx, PM_TYPE *dy);
00115 
00116     PM_TYPE point_line_distance ( PM_TYPE x1, PM_TYPE y1, PM_TYPE x2, PM_TYPE y2,
00117                               PM_TYPE x3, PM_TYPE y3,PM_TYPE *x, PM_TYPE *y);
00118 
00119     inline PM_TYPE norm_a ( PM_TYPE a )
00120     {
00121       int m;
00122       m= ( int ) ( a/ ( 2.0*M_PI ) );
00123       a=a- ( PM_TYPE ) m*M_PI;
00124       if ( a< ( -M_PI ) )
00125         a+=2.0*M_PI;
00126       if ( a>=M_PI )
00127         a-=2.0*M_PI;
00128       return ( a );
00129     }
00130 
00131   public:
00132 
00133     int     PM_L_POINTS;
00134 
00135     PM_TYPE PM_FI_MIN;// = M_PI/2.0 - PM_FOV*PM_D2R/2.0;//[rad] bearing from which laser scans start
00136     PM_TYPE PM_FI_MAX;// = M_PI/2.0 + PM_FOV*PM_D2R/2.0;//[rad] bearing at which laser scans end
00137     PM_TYPE PM_DFI;   // = PM_FOV*PM_D2R/ ( PM_L_POINTS + 1.0 );//[rad] angular resolution of laser scans
00138 
00139     std::vector<PM_TYPE>  pm_fi;//contains precomputed angles
00140     std::vector<PM_TYPE>  pm_si;//contains sinus of angles
00141     std::vector<PM_TYPE>  pm_co;//contains cos of angles
00142 
00143     double  PM_FOV ;             
00144     double  PM_MAX_RANGE ;       
00145     int     PM_MIN_VALID_POINTS; 
00146     int     PM_SEARCH_WINDOW;     
00147 
00148     double  PM_TIME_DELAY;       
00149 
00150     double  PM_MAX_ERROR;        //[cm] max distance between associated points used in pose est.
00151     double  PM_STOP_COND;        
00152     int     PM_MAX_ITER;         
00153     int     PM_MAX_ITER_ICP;     
00154     int     PM_STOP_COND_ICP;    
00155 
00156     PolarMatcher();
00157 
00158     void pm_init();
00159 
00160     //filters the ranges with a median filter. x,y points are not upadted
00161     //ls - laser scan; the job of the median filter is to remove chair and table
00162     //legs wich would be moving anyway;
00163     void pm_median_filter(PMScan *ls);
00164 
00165     // marks point further than a given distance PM_MAX_RANGE as PM_RANGE
00166     void pm_find_far_points(PMScan *ls);
00167 
00168     //segments scanpoints into groups based on range discontinuities
00169     void pm_segment_scan(PMScan *ls);
00170 
00171     // minimizes least square error through changing lsa->rx, lsa->ry,lsa->th
00172     // this looks for angle too, like pm_linearized_match_proper,execept it
00173     // fits a parabola to the error when searching for the angle and interpolates.
00174     PM_TYPE pm_psm(PMScan *lsr,PMScan *lsa);
00175 
00176     // does scan matching using the equations for translation and orietation
00177     //estimation as in Lu & milios, however our matching bearing association rule
00178     //is used together with our association filter.
00179     //this is PSM-C in the tech report
00180     PM_TYPE pm_psm_c(PMScan *lsr,PMScan *lsa);
00181 };
00182 
00183 #endif //POLAR_SCAN_MATCHING_POLAR_MATCH_H


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