polar_match.h

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

/***************************************************************************
                          polar_match.h  - 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
                           the laser is on the robots y axis
                           - in scan projections, occluded ref scanpoints aren't removed!
    changed:
                          14/03/2005 removing of unnecessary code
 ***************************************************************************/
 /****************************************************************************
    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
****************************************************************************/


#ifndef _POLAR_MATCH_
#define _POLAR_MATCH_

#include <stdio.h>

#define PM_GENERATE_RESULTS //if left uncommented scanmatching results are
                //are saved into "results"directory, to be included into thesis.
#define PM_PSM   1 //polar scanmatching - matching bearing
#define PM_PSM_C 2 //polar scanmatching - using cartesian equations
#define PM_ICP   3 //scanmatchign with iterative closest point

#define PM_TIME_FILE "results/iterations.txt" //when generating results, timing is data is saved
                  //into this file from the matching algorithms
                  // i|time|ax|ay|ath[deg] is saved
#define PM_L_POINTS   181 //maximum nu
#define PM_TIME_DELAY 0.02 //time delay in the laser measurements
#define PM_TYPE       float // change it to double for higher accuracy
#define PM_MAX_RANGE  1000 // max valid laser range
#define PM_MAX_ITER   30 // maximum number of iterations for polar scan matching
#define PM_MIN_VALID_POINTS 40 //minimum number of valid points for scanmatching
#define PM_LASER_Y    0//31.3 // y coord of the laser on the robot
                       //for ground thruth & simulation, has to be set to 0
#define PM_MAX_ERROR  100 //[cm] max distance between associated points used in pose est.
                       
// range reading errors
#define PM_RANGE     1  // range reading is longer than PM_MAX_RANGE
#define PM_MOVING    2  // range reading corresponds to a moving point
#define PM_MIXED     4  // range reading is a mixed pixel
#define PM_OCCLUDED  8  // range reading is occluded
#define PM_EMPTY     16 // at that bearing there is no measurment (between 2
                        //    segments there is no interpolation!)


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


//might want to consider doubles for rx,ry -> in a large environment?
struct PMScan
{
  double   t;    //time when scan was taken
  PM_TYPE  rx;   //robot odometry pos
  PM_TYPE  ry;   //robot odometry pos
  PM_TYPE  th;   //robot orientation 
  PM_TYPE  r[PM_L_POINTS];//[cm] - 0 or negative ranges denote invalid readings.
  PM_TYPE  x[PM_L_POINTS];//[cm]
  PM_TYPE  y[PM_L_POINTS];//[cm]
  int      bad[PM_L_POINTS];// 0 if OK
                            //sources of invalidity - too big range;
                            //moving object; occluded;mixed pixel
  int      seg[PM_L_POINTS];//nuber describing into which segment the point belongs to
};


//opens the scanfile and reads out the first line
//the file pointer will point to the file
void pm_init(char* filename, FILE **fin);

//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);

// draws current scan at x,y,th
void pm_plotScanAt(PMScan *ls, PM_TYPE x,PM_TYPE y,PM_TYPE th,char *col,double diameter = 2.0);


// draws current scan in world and robot(polar) coord syst with colour col
void pm_plotScan(PMScan *ls, char *col,double diameter = 2.0);

////match scan ls agains ref
//void pm_match(PMScan *ref,PMScan *ls);

//filters the ranges with a median filter. x,y points are not upadted
//ls - laser scan; the job of the median filter is to remove chair and table
//legs wich would be moving anyway;
void pm_median_filter(PMScan *ls);

// marks point further than a given distance PM_MAX_RANGE as PM_RANGE
void pm_find_far_points(PMScan *ls);

//segments scanpoints into groups based on range discontinuities
void pm_segment_scan(PMScan *ls);

//shows in different colours the different laser segments
void pm_show_segmentation(PMScan *ls);

// minimizes least square error through changing lsa->rx, lsa->ry,lsa->th
// this looks for angle too, like pm_linearized_match_proper,execept it
// fits a parabola to the error when searching for the angle and interpolates.
PM_TYPE pm_psm(PMScan *lsr,PMScan *lsa);

// does scan matching using the equations for translation and orietation
//estimation as in Lu & milios, however our matching bearing association rule
//is used together with our association filter.
PM_TYPE pm_cartesian_match(PMScan *lsr,PMScan *lsa);


// minimizes least square error of points through changing lsa->rx, lsa->ry,lsa->th
// by using ICP. Only measurements furher than 32m are ignored. Interpolation is
// not implemented. Only the best 80% of points are used.
// scan projection is done in each step.
PM_TYPE pm_icp(PMScan *lsr,PMScan *lsa);



//guesses if the scan was taken of a corridor or not
//using a 180 deg angle histogram - returns true if corridor
bool pm_is_corridor(PMScan *act);

//mathcing the orienation of corridors -> both scans are assumed
//to be corridors
//tries to match the ange histogram of ref scan to that of actual
//scan. the actual scans orientation with respect to ref scan
//is returned in lsa.th
//throws an exception upon error.
void pm_angle_histogram_match(PMScan *lsr,PMScan *lsa);


//calculates an error index expressing the quality of the match
//of the actual scan to the reference scan
//has to be called after scan matching so the actual scan in expressed
//in the reference scan coordinate system
//return the average minimum Euclidean distance; MAXIMUM RANGE points
//are not considered; number of non maximum range points have to be
//smaller than a threshold
PM_TYPE pm_error_index(PMScan *lsr,PMScan *lsa);


//assuming the scan was taken of a corridor, determines the
//orientation of the corridor by finding the maximum of a
// 180 deg angle histogram 
PM_TYPE pm_corridor_angle(PMScan *act);


//estimates the covariance matrix(c11,c12,c22,c33) (x,y,th) of
//a scan match based on an error index (err-depends on how good the
//match is), and the angle of the corridor if it is a corridor
void pm_cov_est(PM_TYPE err, double *c11,double *c12, double *c22, double *c33,
                    bool corridor=false, PM_TYPE corr_angle=0);
                    
inline PM_TYPE norm_a(PM_TYPE a);

//to interface with matlab
void pm_save_scan(PMScan *act,char *filename);

// plots actual and reference scan in a way how it should appear in the thesis
void pm_plotScan4Thesis(PMScan *lsr,PMScan *lsa);

// plots contents of timing file - for convergence measurements
// x,y,th are the true results; tht is in degrees!
void pm_plotTime4Thesis(PM_TYPE xt, PM_TYPE yt, PM_TYPE tht,int *iter=NULL,double *time=NULL);

#endif