MbICP.h
/*************************************************************************************/
/* */
/* File: MbICP.h */
/* Authors: Luis Montesano and Javier Minguez */
/* Modified: 1/3/2006 */
/* */
/* This library implements the: */
/* */
/* */
/* J. Minguez, L. Montesano, and F. Lamiraux, "Metric-based iterative */
/* closest point scan matching for sensor displacement estimation," IEEE */
/* Transactions on Robotics, vol. 22, no. 5, pp. 1047 \u2013 1054, 2006. */
/*************************************************************************************/
/*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*****************************************************************************/
//
// EVERYTHING IN THE INTERNATIONAL SYSTEM (METERS AND RADIANS)
//
/*****************************************************************************/
#ifndef MbICP
#define MbICP
#include "TData.h"
#ifdef __cplusplus
extern "C" {
#endif
// ----------------------------------------------------------------------------
// DEFINES
// ----------------------------------------------------------------------------
/*
#define MAXLASERPOINTS 361
*/
// ----------------------------------------------------------------------------
// GENERIC TYPES
// ----------------------------------------------------------------------------
/*typedef struct {
float x;
float y;
}Tpf;
typedef struct {
float r;
float t;
}Tpfp;
typedef struct {
int x;
int y;
}Tpi;
typedef struct {
float x;
float y;
float tita;
}Tsc;
*/
// ----------------------------------------------------------------------------
// SPECIFIC TYPES
// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
// GLOBAL FUNCTIONS
// ----------------------------------------------------------------------------
// ************************
// Function that initializes the SM parameters
// ************************
/* void InitScanMatching(float Bw, float Br,
float L, int laserStep,float MaxDistInter, float filtrado,
int MaxIter, float error_th, float exo, float eyo, float etitao, int IterSmoothConv); */
// in:::
/* --------------------- */
/* --- Thresold parameters */
/* --------------------- */
/* Bw: maximum angle diference between points of different scans */
/* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */
/* This is a speed up parameter */
//float Bw;
/* Br: maximum distance difference between points of different scans */
/* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */
//float Br;
/* --------------------- */
/* --- Inner parameters */
/* --------------------- */
/* L: value of the metric */
/* When L tends to infinity you are using the standart ICP */
/* When L tends to 0 you use the metric (more importance to rotation) */
//float L;
/* laserStep: selects points of each scan with an step laserStep */
/* When laserStep=1 uses all the points of the scans */
/* When laserStep=2 uses one each two ... */
/* This is an speed up parameter */
//int laserStep;
/* ProjectionFilter: */
/* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */
/* It works well for angles < 45 \circ*/
/* 1 : activates the filter */
/* 0 : desactivates the filter */
// int ProjectionFilter;
/* MaxDistInter: maximum distance to interpolate between points in the ref scan */
/* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */
//float MaxDistInter;
/* filter: in [0,1] sets the % of asociations NOT considered spurious */
/* E.g. if filter=0.9 you use 90% of the associations */
/* The associations are ordered by distance and the (1-filter) with greater distance are not used */
/* This type of filtering is called "trimmed-ICP" */
//float filter;
/* AsocError: in [0,1] */
/* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */
/* When the number of associations is below AsocError, the main function will return error in associations step */
// float AsocError;
/* --------------------- */
/* --- Exit parameters */
/* --------------------- */
/* MaxIter: sets the maximum number of iterations for the algorithm to exit */
/* The more iterations, the more chance you give the algorithm to be more accurate */
//int MaxIter;
/* errorRatio: in [0,1] sets the maximum error ratio between iterations to exit */
/* In iteration K, let be errorK the residual of the minimization */
/* Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is the solution of the scan matching */
//float error_th;
/* errx_out,erry_out, errt_out: minimum error of the asociations to exit */
/* In each iteration, the error is the residual of the minimization in each component */
/* The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out) */
/* When errorK tends to 0 the more precise is the solution of the scan matching */
//float errx_out,erry_out, errt_out;
/* IterSmoothConv: number of consecutive iterations that satisfity the error criteria (the two above criteria) */
/* (error_th) OR (errorx_out && errory_out && errt_out) */
/* With this parameter >1 avoids random solutions and estabilices the algorithm */
//int IterSmoothConv;
void Init_MbICP_ScanMatching(
float max_laser_range,
float Bw,
float Br,
float L,
int laserStep,
float MaxDistInter,
float filter,
int ProjectionFilter,
float AsocError,
int MaxIter,
float errorRatio,
float errx_out,
float erry_out,
float errt_out,
int IterSmoothConv);
// -------------------------------------------------------------
// ************************
// Function that does the scan matching
// ************************
/* int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
Tsc *sensorMotion, Tsc *solution); */
// in:::
// laserK: is the reference scan in polar coordinates (max. num points is MAXLASERPOINTS)
// laserK1: is the new scan in polar coordinates (max. num points is MAXLASERPOINTS)
// sensorMotion: initial SENSOR motion estimation from location K to location K1
// solution: SENSOR motion solution from location K to location K1
// out:::
// 1 : Everything OK in less that the Maximum number of iterations
// 2 : Everything OK but reached the Maximum number of iterations
// -1: Failure in the association step
// -2: Failure in the minimization step
int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
Tsc *sensorMotion, Tsc *solution);
#ifdef __cplusplus
}
#endif
#endif
Tsc
Definition: TData.h:57
Tpfp
Definition: TData.h:47

Last updated 12 September 2005 21:38:45