polar_match.h

Go to the documentation of this file.

/*                                                                                                                         **************************************************************************
                          polar_match.h  - Matching laser scans in polar coord system
                             -------------------
    begin                : Tue Nov 9 2004
    version              : 0.3    
    copyright            : (C) 2004-2010 by Albert Diosi and Lindsay Kleeman
    email                : albert.diosi@gmail.com
    comments             : - range units are cm; angle units are radians or degrees
                           - the laser is on the robot's Y axis
                           - in scan projections, occluded ref scanpoints are not removed!
                           - TODO: Investigate why is checking the range difference necessary for 
                            the Hokuyo UTM and not for the rest.
 ***************************************************************************/
/****************************************************************************
Copyright (c) 2004-2010, Albert Diosi and Lindsay Kleeman
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
    * The name of the copyright holders may not be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
****************************************************************************/

/*
First of all you need to select the laser range finder you have by
setting "#define PM_LASER "
to PM_SICK_LMS200, PM_HOKUYO_URG_04LX. 
*/

#ifndef _POLAR_MATCH_
#define _POLAR_MATCH_

#include <stdio.h>

//TODO: get the result generation to work again:
//#define PM_GENERATE_RESULTS //If left uncommented scanmatching results are
                     //saved into the "results"directory if two required programs are present.

//----------------- L A S E R    S P E C I F I C    P A R A M E T E R S------------

// STEP 1) Define a name for your laser range finder here (if it hasn't been defined yet):
#define PM_PSD_SCANNER      0
#define PM_HOKUYO_URG_04LX  1
#define PM_SICK_LMS200      2
#define PM_HOKUYO_UTM_30LX  3
#define PM_VSCAN  4

// STEP 2) Set the type your laser range finder here:
//  #define PM_LASER PM_SICK_LMS200
//  #define PM_LASER PM_HOKUYO_URG_04LX
//  #define PM_LASER PM_PSD_SCANNER
   #define PM_LASER PM_VSCAN

// STEP 3) Add your laser range finder's parameters here if it is a different model (use centimeters)

  #define PM_LASER_NAME       "VirtualScan" 
  #define PM_L_POINTS         720  
  #define PM_FOV              360  
  #define PM_MAX_RANGE        100000  
  #define PM_MIN_VALID_POINTS 80   
  #define PM_SEARCH_WINDOW    10   
  #define PM_CORRIDOR_THRESHOLD 25.0 

// STEP 4) Set the time registration delay (the time between your time stamps and the 
// time the scan was taken) and the distance of your laser from the odometry center 
// in the forward direction.
#define PM_TIME_DELAY       0 
#define PM_LASER_Y          0 
#define PM_MIN_RANGE        10.0f 
#define PM_SEG_MAX_DIST     80.0 
#define PM_WEIGHTING_FACTOR 30*30 
#define PM_CHANGE_WEIGHT_ITER 10 

#define PM_TYPE             double 

#define PM_MAX_ERROR        100  
//#define PM_STOP_COND        0.001  
#define PM_STOP_COND        0.1  
#define PM_MAX_ITER         500   
#define PM_MAX_ITER_ICP     500   
#define PM_STOP_COND_ICP    0.1  

#define PM_MIN_STD_XY          5.0 
#define PM_MIN_STD_ORIENTATION 4.0  
#define PM_MATCH_ERROR_OFFSET  5.0  

#define PM_ODO  -1 
#define PM_PSM   1 
#define PM_ICP   3 

#define PM_TIME_FILE "results/iterations.txt" 

// Description of range reading errors. Each range measurement may be tagged with one of these:
#define PM_RANGE     1  
#define PM_MOVING    2  
#define PM_MIXED     4  
#define PM_OCCLUDED  8  
#define PM_EMPTY     16 

extern PM_TYPE   pm_fi[PM_L_POINTS];
extern PM_TYPE   pm_si[PM_L_POINTS];
extern PM_TYPE   pm_co[PM_L_POINTS];
extern const PM_TYPE   PM_D2R; 
extern const PM_TYPE   PM_R2D; 

struct PMScan
{
  double   t;    
  PM_TYPE  rx;   
  PM_TYPE  ry;   
  PM_TYPE  th;   
  PM_TYPE  r[PM_L_POINTS];
  PM_TYPE  x[PM_L_POINTS];
  PM_TYPE  y[PM_L_POINTS];
  int      bad[PM_L_POINTS];
  int      seg[PM_L_POINTS];
};

void pm_init(const  char* filename=NULL, FILE **fin=NULL);
int  pm_readScan(FILE *fin, PMScan *ls);
void pm_save_scan(PMScan *act,const char *filename);

void pm_preprocessScan(PMScan *ls);

PM_TYPE pm_psm( const PMScan *lsr,PMScan *lsa);
PM_TYPE pm_icp( const PMScan *lsr,PMScan *lsa);

void pm_plotScanAt(const PMScan *ls, PM_TYPE x,PM_TYPE y,PM_TYPE th,const char *col, double diameter = 2.0, bool connect_lines = false);
void pm_plotScan(PMScan *ls, const char *col,double diameter = 2.0, bool connect_lines = false);
void pm_show_segmentation(const PMScan *ls);
void pm_plotScan4Thesis(PMScan *lsr,PMScan *lsa);
void pm_plotTime4Thesis(PM_TYPE xt, PM_TYPE yt, PM_TYPE tht,int *iter=NULL,double *time=NULL);

bool    pm_is_corridor(PMScan *act);
PM_TYPE pm_error_index(PMScan *lsr,PMScan *lsa);
PM_TYPE pm_error_index2 ( PMScan *ref,PMScan *cur, int* associatedPoints=NULL );
PM_TYPE pm_corridor_angle(PMScan *act);
void    pm_cov_est(PM_TYPE err, double *c11,double *c12, double *c22, double *c33,
                   bool corridor=false, PM_TYPE corr_angle=0);

void  pm_unit_test(int matching_alg = PM_PSM, bool interactive=true);
#endif