MbICP.h

Go to the documentation of this file.

/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 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.
  *Neither the name of the University of Aveiro nor the names of its contributors may 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.
***************************************************************************************************/
/*************************************************************************************/
/*                                                                                   */
/*  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"
#include <stdlib.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,int*numIter);
int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1, Tsc *sensorMotion, Tsc *solution);

#ifdef __cplusplus
}
#endif


#endif