lodo.h File Reference

The lodo library provides laser-stabilizied odometric pose estimates: the inputs are raw odometry and laser scans, the output is a corrected pose estimate. More...

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

Go to the source code of this file.

Classes
struct	lodo_scan_t
	Data pertaining to an individual scan. More...
struct	lodo_map_point_t
	Working data for a map point. More...
struct	lodo_scan_point_t
	Working data for scan point. More...
struct	lodo_t
	Laser odometry module data. More...
Defines
#define	LODO_MAX_RANGES   401
	Limits.
Functions
lodo_t *	lodo_alloc (int num_ranges, double range_max, double range_res, double range_start, double range_step)
	Allocate object.
void	lodo_free (lodo_t *self)
	Free object.
pose2_t	lodo_add_scan (lodo_t *self, pose2_t odom_pose, int num_ranges, double *ranges)
	Add a scan, compute correction, update map.
void	lodo_project_map (lodo_t *self)
	Project map points into scan polar coordinates.
void	lodo_project_map_free (lodo_t *self, lodo_scan_t *scan_m, matrix33_t Pd, matrix33_t Pm)
	Project map free space into the scan polar frame.
double	lodo_correct (lodo_t *self)
	Compute correction for a scan.
double	lodo_test_offset (lodo_t *self, double offset, double *outliers)
	Test a scan offset.
void	lodo_print_map (lodo_t *self)
	Print projected map points.
void	lodo_print_pef (lodo_t *self)
	Print a polar error functions for scan and map.
void	lodo_print_err (lodo_t *self)
	Print error histograms.
void	lodo_draw_scan (lodo_t *self, lodo_scan_t *scan)
	Draw a scan.
void	lodo_draw_map (lodo_t *self)
	Draw the current map (hits).

---

Detailed Description

The lodo library provides laser-stabilizied odometric pose estimates: the inputs are raw odometry and laser scans, the output is a corrected pose estimate.

The drift in the corrected estimate is much lower than that seen with odometry alone. See, for example, the figure below: this plot shows the trajectory of a robot that has travelled 125m and executed 19 complete rotations before returning to the starting location. The final cumulative orientation error is less than 5 degrees (versus 110 degrees for pure odometry).

Trajectory plot: odometry versus laser-stabilized odometry

Absolute error plot: odometry versus laser-stabilized odometry

How it works

The lodo library uses an incremental SLAM algorithm to correct drift in the robot's orientation. The algorithm has three key data structures:

• The current laser scan (a list of range values).
• The local map (a ring buffer containing recent laser scans).
• The polar error function (a grid generated from the local map).

The algorithm applied to each new laser scan is as follows:

1. Generate the polar error function:
   • For each scan in the local map:
     • For each point on the polygonal boundary of the map scan:
       • Project point into the local coordinate frame of the new scan.
       • Convert to polar coordinates and mark the corresponding cell in the polar error function as "occupied".
   • For each cell in the polar error function, compute the distance to the nearest occupied cell with the same radial value.
2. Find orientation correction:
   • For each possible correction:
     • Apply correction to each point in the new scan and convert into polar coordinates.
     • Look up error value in the polar error function.
     • Add to the todal error for this correction value.
   • Select correction value with the lowest total error.
3. Build map:
   • If the new scan has a signficant number of "outlier" points (large error values), add this scan to the current map. The map is a ring buffer, so adding a new scan will necessarily lead to an old one being discarded.

The intuition behind this algorithm is best explained in diagrams (below).

Figure (a) shows a map scan in global carestian coordinates; the map scan is projected into the coordinate frame of the new scan and converted to polar coordinates (b); using dynamic programming, we compute the error function (c). This polar error function can be used to test possible orientation corrections very efficiently: changes in the orientation of the new scan correspond the translations of the polar error function.

(a) Original map scan (cartesian coordinates).	(b) Map scan in local polar coordinates.
(c) Calculated polar error function.

lodo_caveats

The current version of the library takes about 15ms to process each scan on a 2.8 GHz P4, so expect it to use lots of cycles on your robot. There is still lots of optimization to do, however, so expect future releases to clock in around the 5ms mark.

Todo:

Accuracy, reliability and performance

---

Function Documentation

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

Allocate object. Parameters: num_ranges  Number of range readings in each scan (should be 181). range_max  Maximum useable range value (e.g., 8.00 or 16.00). range_res  Resolution for comparing range values (i.e., range bin width). range_start  Starting angle for range readings (should be -M_PI / 2). range_step  Angular step size for each successive range reading (should be M_PI / 180). Returns: Object handle.

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

Add a scan, compute correction, update map. Parameters: self  Object handle. odom_pose  Raw odometric pose of the robot. num_ranges  Number of range readings. ranges  Array of laser range readings. Returns: Returns the corrected robot pose.

void lodo_project_map (  lodo_t *  self  )

Project map points into scan polar coordinates. For internal use only.

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

Project map free space into the scan polar frame. For internal use only.

double lodo_correct (  lodo_t *  self  )

Compute correction for a scan. For internal use only.

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

Test a scan offset. For internal use only.

---