lodo.h

Go to the documentation of this file.

/*
  LODO library: laser-stabilized odometry
  Copyright (C) 2004 Andrew Howard  ahoward@usc.edu

  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.
*/

#ifndef LODO_H
#define LODO_H

#ifdef __cplusplus
extern "C"
{
#endif

#include <gsl/gsl_min.h>
#include "slap.h"


#define LODO_MAX_RANGES 401


typedef struct
{
  pose2_t opose;

  pose2_t cpose;
  
  double ranges[LODO_MAX_RANGES];
  
} lodo_scan_t;


typedef struct
{
  vector3_t local;

  vector3_t polar;
  
} lodo_map_point_t;


typedef struct
{
  int ni;
  
} lodo_scan_point_t;


typedef struct
{  
  int num_ranges;

  double range_max;

  double range_start;

  double range_step;

  pose2_t laser_pose;

  double max_error;

  int pef_num_ranges, pef_num_bearings;

  double pef_range_start, pef_range_step;

  double pef_bearing_start, pef_bearing_step;

  int pef_size;
  int *pef;

  double fit_interval;
  
  double fit_err_thresh;

  double fit_outlier_dist;

  double fit_outlier_frac;

  double odom_dist, odom_turn;

  double map_dist_interval, map_turn_interval;

  double map_last_dist, map_last_turn;

  int map_scan_count, max_map_scans;
  lodo_scan_t *map_scans;

  int scan_count;
  lodo_scan_t scan;

  lodo_scan_point_t scan_points[LODO_MAX_RANGES];

  int num_map_points, max_map_points;
  lodo_map_point_t *map_points;

  gsl_min_fminimizer *mini;

  double fit_correct;

  int fit_valid;

  int fit_add;
  
} lodo_t;


lodo_t *lodo_alloc(int num_ranges, double range_max, double range_res,
                   double range_start, double range_step);

void lodo_free(lodo_t *self);

pose2_t lodo_add_scan(lodo_t *self, pose2_t odom_pose, int num_ranges, double *ranges);

void lodo_project_map(lodo_t *self);

void lodo_project_map_free(lodo_t *self, lodo_scan_t *scan_m,
                           matrix33_t Pd, matrix33_t Pm);

double lodo_correct(lodo_t *self);

double lodo_test_offset(lodo_t *self, double offset, double *outliers);

void lodo_print_map(lodo_t *self);

void lodo_print_pef(lodo_t *self);

void lodo_print_err(lodo_t *self);

void lodo_draw_scan(lodo_t *self, lodo_scan_t *scan);

void lodo_draw_map(lodo_t *self);

#ifdef __cplusplus
}
#endif

#endif

---