Description

graphslam-engine constructs and optimizes robot pose-graphs from rawlog datasets. This app performs 2D graph-slam: the robot localizes itself in the environment while, at the same time, builds a map of that environment consisting of robot poses and links (relative poses). The MRPT rawlog files should contain (some of) the following observation types:

• CObservationOdometry
• CObservation2DRangeScan
• CObservation3DRangeScan
Currently working with 3DRangeScan is currently in an experimental phase.

An example of using the graphslam-engine application is given below:

Command line options

The available command line arguments are listed below. Alternatively run graphslam-engine -h for the full arguments list.

1. Description

icp-slam-live is a front-end application to the MRPT C++ library class mrpt::slam::CMetricMapBuilderICP, using as data source a LIDAR in real-time. See icp-slam for an equivalent application for offline operation.

2. Demo

To run one of the demos included in MRPT, execute:

See the directory icp-slam-live for example config files.

Paper: Subjective Local Maps for Hybrid Metric-Topological SLAM

Subjective Local Maps for Hybrid Metric-Topological SLAMRobotics and Autonomous Systems, 2009 – (PDF).

Abstract: Hybrid maps where local metric sub-maps are kept in the nodes of a graph-based topological structure are gaining relevance as the focus of robot Simultaneous Localization and Mapping (SLAM) shifts towards spatial scalability and long-term operation. In this paper we examine the applicability of spectral graph partitioning techniques to automatically generate metric sub-maps by establishing groups in the sequence of observations gathered by the robot. One of the main aims of this work is to provide a probabilistically grounded interpretation of such a partitioning technique in the context of generating these local maps. We also discuss how to apply it to different kinds of sensory data (stereo images and laser range scans) and how to consider them simultaneously. An important feature of our approach is that it implicitly takes into account the intrinsic characteristics of the sensors, such as the sensor field of view, to perform the partitioning instead of applying heuristics supplied by a human as in other works, and thus the robot builds “subjective” local maps. The ideas presented here are supported by experimental results from a real mobile robot as well as simulations for statistical analysis. We discuss the effects of considering different combinations of sensors in the resulting clustering of the environment.

Paper: RO-SLAM with SOG

“Efficient Probabilistic Range-Only SLAM”, IROS 2008 – PDF – Slides PPT
Abstract: This work addresses Range-Only SLAM (RO-SLAM) as the Bayesian inference problem of sequentially tracking a vehicle while estimating the location of a set of beacons without any prior information. The only assumptions are the availability of odometry and a range sensor able of identifying the different beacons. We propose exploiting the conditional independence between each beacon distribution within a Rao-Blackwellized Particle Filter (RBPF) for maintaining independent Sum of Gaussians (SOGs) for each map element. It is shown then that a proper probabilistic observation model can be derived for online operation with no need for delayed initializations. We provide a rigorous statistical comparison of this proposal with previous work of the authors where a Monte-Carlo approximation was employed instead for the conditional densities. As verified experimentally, this new proposal represents a significant improvement in accuracy, computation time, and robustness against outliers.

Paper: Stereo Vision Models for RBPF SLAM

“Stereo vision-specific models for Particle Filter-based SLAM”Robotic and Autonomous Systems – PDF.

Abstract:This work addresses the SLAM problem for stereo vision systems under the unified formulation of particle filter methods. In contrast to most existing approaches to visual SLAM, the present method does not rely on restrictive smooth camera motion models, but on computing incremental 6D pose differences from the image flow through a probabilistic visual odometry method. Moreover, our observation model, which considers both the 3D positions and the SIFT descriptors of the landmarks, avoids explicit data association between the observations and the map by marginalizing the observation likelihood over all the possible associations. We have experimentally validated our research with two experiments in indoor scenarios.

Optimal Filtering for Non-Parametric Observation Models: Applications to Localization and SLAM

“An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization”ICRA 2008, Pasadena (California, USA), May 19-23, 2008. (PDF) – (Slides)

Abstract: The lack of a parameterized observation model in robot localization using occupancy grids requires the application of sampling-based methods, or particle filters. This work addresses the problem of optimal Bayesian filtering for dynamic systems with observation models that cannot be approximated properly as any parameterized distribution, which includes localization and SLAM with occupancy grids. By integrating ideas from previous works on adaptive sample size, auxiliary particle filters, and rejection sampling, we derive a new particle filter algorithm that enables the usage of the optimal proposal distribution to estimate the true posterior density of a non-parametric dynamic system. Our solution avoids approximations adopted in previous approaches at the cost of a higher computational burden. We present simulations and experimental results for a real robot showing the suitability of the method for localization.

6 DoF SLAM using a ToF Camera: The challenge of a continuously growing number of landmarks

“6 DoF SLAM using a ToF Camera: The challenge of a continuously growing number of landmarks”Siegfried Hochdorfer, Christian Schlegel, IROS 2010 – (Link).

Authors are with the Collaborative Center for Applied Research (http://www.zafh-servicerobotik.de/), Ulm, Germany.

Online video with results from this paper:

SRBA-SLAM

Section: Mobile Robot Programming Toolkit – MRPT (1)

NAME

srba-slam – A demo program for Relative Bundle Adjustment (RBA)

SYNOPSIS

srba-slam –help

srba-slam –list-problems

srba-slam {–se2|–se3} {–lm-2d|–lm-3d} –obs [StereoCamera|…] -d DATASET.txt [–sensor-params-cfg-file SENSOR_CONFIG.cfg] [–noise NOISE_SIGMA] [–verbose {0|1|2|3}] [–step-by-step]

Application: rbpf-slam

It is actually a front-end to the class mrpt::slam::CMetricMapBuilderRBPF. All the parameters to the algorithm are passed through a configuration file in the command line. The filter processes actions and observations from a rawlog file and optionally generates a number of files describing the evolution of the filter and the maps.

1. Rao-Blackwellized Particle Filter SLAM (rbpf-slam)

It is actually a front-end to the class mrpt::slam::CMetricMapBuilderRBPF. All the parameters to the algorithm are passed through a configuration file in the command line. The filter processes actions and observations from a rawlog file and optionally generates a number of files describing the evolution of the filter and the maps.
The mathematical background of RBPF-based SLAM and to see an updated list of the implemented RBPF-SLAM solutions can be found in this tutorial page.

1. Description

This is a very simple program written in 2 hours just to illustrate the capabilities of Xbox Kinect to perform Visual SLAM with the MRPT libraries. It is not supposed to be used for even medium-sized maps.

Usage:
– Point to some static, near object.
– Press ‘r’ to reset the map.
– Start moving the Kinect slowly. Take care that at least 3-5 features exist at all time.

Working principle: The program creates a “key-frame” for each 10cm or 10deg the camera moves, then establish the 6D relative pose of the camera to the set of tracked features at the last “key-frame”.

Application: kf-slam

This application implements a simple Kalman Filter for solving the SLAM problem for generic 3D range-bearing sensors and a full 6D robot state space. This program is a front-end for the classmrpt::slam::CRangeBearingKFSLAM . All the parameters to the algorithm are passed through a configuration file in the command line. The filter processes actions and observations from a rawlog file and generates a number of files describing the evolution of the filter.

kf-slam can also optionally employ a 2D version of the same range-bearing problem (since mrpt 0.9.4), a more efficient approach for SLAM problems really happening in planar environments. See this configuration file as an example of how to use the kf_implementation parameter.

1. Description

icp-slam is a front-end application to the MRPT C++ library class mrpt::slam::CMetricMapBuilderICP. This SLAM algorithm builds a map incrementally by aligning new range scans to either a point point or an occupancy grid map using the ICP algorithm. The ICP implementation itself can be found in the class mrpt::slam::CICP. See this tutorial describing the algorithm.

2. Demo

To run one of the demos included in MRPT, execute:

or to use that same configuration with your own data set:

Check out the comments in the configuration files provided in MRPT/share/mrpt/config_files/icp-slam/ for further reference. Note that both ICP can be configured to use either a points or occupancy grid map to perform the scan alignment. And even using one of them, the other map type can be also built simultaneously.

NAME

graph-slam – Command-line Graph-SLAM hub application

USAGE EXAMPLES

View Dijkstra-based spanning tree coordinates estimation of a 2D graph file

graph-slam –2d –dijkstra –view -i in.graph

Visualization of a 2D (or 3D) graph file

graph-slam –2d [or –3d] –view -i in.graph

Levenberg-Marquartd optimization of a 3D graph and visualize result

graph-slam –3d –levmarq –view -i in.graph

DESCRIPTION

graph-slam is a command-line application to visualize pose constraint graphs
and execute Graph-SLAM methods on them.

1. Description

This GUI application is an extension to a similar Matlab program developed by J. Neira and J. D. Tardós (University of Zaragoza). It allows extensive experimentation with data-association and the behavior of Kalman Filter-based 2D SLAM, in a didactic way.

At the core of this program is just one MRPT class: mrpt::slam::CRangeBearingKFSLAM2D. Check out its Doxygen documentation for more details.

Paper explaining the Matching Likelihood criteron for data association (one of the two choices offered in this program):

Example: graph_slam_demo

Output data from graph-slam

1. Description

This example, found in the path MRPT/samples/graph_slam_demo, demonstrates:

• the basic construction of a graph of pose constraints,
• the execution of a graph-slam optimization algorithm on it (in order to optimize the global node poses given the information in all the edges and one fixed root node), and
• how to render graphs as MRPT’s OpenGL primitives.
• RBPF-SLAM algorithms (C++ library mrpt-slam)

Example of gridmapping with a RBPF (rbpf-slam app).

1. Mathematical background

• “Rao-Blackwellised particle filtering for dynamic Bayesian networks”, Doucet, A. and De Freitas, N. and Murphy, K. and Russell, S. (2000) – PDF
• Some slides, by Boris Lipchin.

2. RBPF-based SLAM solutions implemented in MRPT

A RBPF is a special instance of a particle filter, thus in principle any of the four generic filtering algorithms declared in the virtual C++ base classes can be used to sequentially estimate the robot path. Just a quick summary of the algorithms:

• Sequential Importance Resampling – SIR with the “standard” proposal distribution (enum value: pfStandardProposal)
• Auxiliary Particle Filter – APF with the “standard” proposal distribution (enum value: pfAuxiliaryPFStandard)
• Optimal proposal distribution (enum value: pfOptimalProposal)
• Approximate Optimal Sampling – A rejection sampling-based approximation to the optimal proposal when a closed-form expression is not available (enum value: pfAuxiliaryPFOptimal)

Note: So far (Aug 2011), there is no RBPF-SLAM implementation with the APF algorithm (which is in turn useful for Monte Carlo localization).

2.1. pfStandardProposal: SIR with “standard proposal” and any kind of metric map

Description: The pfStandardProposal algorithm can be used with any metric map or combination of several of them simultaneously, provided that an observation likelihood function is implemented for the sensor observations in each map. This algorithm however is not recommendable since the “standard” proposal distribution is the motion model (the “actions“), and most of the particles will end up in areas incompatible with the observations, and thus, resampling will occur quite often. Only use this algorithm if the motion model is known to be very precise. Example config files:

HMT-SLAM

This page overviews the theoretical foundations of the Hybrid Metric Topological (HMT) SLAM framework. For details about the implementation in the MRPT C++ libraries, see mrpt-hmtslam.

1 Overview

HMT-SLAM stands for Hybrid Metrical-Topological SLAM. It is a complete, consistent bayesian formulation of the SLAM (Simultaneous Localization and Mapping) problem that copes both with metrical and topological maps. Up to the first published paper of HMT-SLAM (see [1] below), all the intents to include topological information into classical metrical SLAM dealt with topologies as a separate component from metrics, and therefore use different techniques for processing both of them. In HMT-SLAM, however, both parts are dealt with in a unified probabilistic framework, which facilitates the integration of low-level, subsymbolic (metrical) algorithms with high-level, symbolic (topological) reasoning.

Derivation and Implementation of a Full 6D EKF-based Solution to Range-Bearing SLAM

Technical report: “Derivation and Implementation of a Full 6D EKF-based Solution to Range-Bearing SLAM”Jose-Luis Blanco, Perception and Mobile Robots Research Group, University of Malaga, Spain. (new version with corrected Jacobians, soon!, old version: PDF)
This document describes the theory behind the application kf-slam.

Bibtex info:

1. Description

Short version:

The module mrpt-slam provides a generic C++ implementation of RBPF-SLAM for different map types, among which there is one solution to Range-Only SLAM with maps represented as Sum of Gaussians, which are dynamically adapted to represent well the uncertainty of all mapped beacons. There exists a stand-alone executable (rbpf-slam) and demonstration configuration files and datasets.

Long version:

This solution to Range-Only SLAM (RO-SLAM) addresses the Bayesian inference problem of sequentially tracking a vehicle while estimating the location of a set of beacons without any prior information. The only assumptions are the availability of odometry and a range sensor able of identifying the different beacons (i.e. no need to solve data association).

SLAM (Simultaneous Localization and Mapping) for beginners: the basics

For those who are new into mobile robotics and want some introductions, I recommend these taped seminars by  Cyrill Stachniss:

Also checkout this course by Dr. Jürgen Sturm (LUM):

More technical introductions from the scientific literature:

• About EKF-based SLAM with landmark maps:
• Hugh Durrant-Whyte & Tim Bailey. “Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms“, (2006) (PDF)
• Tim Bailey & Hugh Durrant-Whyte and . “Simultaneous Localisation and Mapping (SLAM): Part II State of the Art“, (2006) (PDF)
• Graph-SLAM maps

View of mismatches between edge information and actual keyframes.

1. Text .graph file format

This file format was (to the best of my knowledge) first used in public software with TORO, and since then has been employed by other libraries and programs published in OpenSLAM.org.

Graphs in both 2D (x,y,phi) or 3D (x,y,z,yaw,pitch,roll) are supported. Files consist of plain text which is interpreted line by line. The first word in each line determines the type of that entry.
The following entry types exist:

• VERTEX2: A 2D node
id is a unique identifier for each node. phi is in radians.
• VERTEX3: A 3D node
id is a unique identifier for each node. yaw,pitch and roll are in radians.
• EDGE2: A 2D pose constraint
• EDGE3: A 3D pose constraint
• EQUIV: A “topological loop closure”, forcing two nodes to actually become the same.
• FIX: Defines “fixed” keyframes, that works as “anchors” to help the optimization problem to have one single solution. Fixes poses do not change during optimization.

2. Binary .graphbin file format

This format may be used if I/O disk performance is an issue. It consists of direct binary serializations of the corresponding objects, that is:

SLAM Book (2012)

Title: Simultaneous Localization and Mapping for Mobile Robots: Introduction and Methods
Authors: Fernández-Madrigal, J.A. and Blanco, J.L.
Release date: 2012
ISBN: 978-1466621046
Pages: 497

Available in:

Description As mobile robots become more common in general knowledge and practices, as opposed to simply in research labs, there is an increased need for the introduction and methods to Simultaneous Localization and Mapping (SLAM) and its techniques and concepts related to robotics. =&2=& investigates the complexities of the theory of probabilistic localization and mapping of mobile robots as well as providing the most current and concrete developments. This reference source aims to be useful for practitioners, graduate and postgraduate students, and active researchers alike.

Erratum (last update: Feb 2017)

 Errata location Says… Should say… Page 200, eq. (20) Remove all four minus signs. Page 405 Equations fpi(a,p) and its Jacobians Corrected versions are in section 4.2 of this techical report.

Summary of contents

Part I: The Foundations of Mobile Robot Localization and Mapping

CHAPTER 1: Introduction

CHAPTER 2: Robotic Bases

CHAPTER 3: Probabilistic Bases

CHAPTER 4: Statistical Bases

Part II: Mobile Robot Localization

CHAPTER 5: Robot Motion Models

CHAPTER 6: Sensor Models

CHAPTER 7: Mobile Robot Localization with Recursive Bayesian Filters

Tutorials: SLAM algorithms

[subpages depth=”5″]

SLAM: Map types vs. algorithms

Not all SLAM algorithms fit any kind of observation (sensor data) and produce any map type. The following table summarizes what algorithms (of those implemented in MRPT) fit what situation.

 ↓Observations \ Maps → Occupancy grid (Class COccupancy GridMap2D) Point maps (Class CPointsMap) Landmark map (class depends on SLAM method) Graph of pose constraints 2D laser scanner (CObservation 2DRangeScan) X X 1D sonar range readings (CObservation Range) X X X Features / Landmarks (SLAM method-dependent) X X X Range-only sensors (CObservation BeaconRanges) X X X Relative poses X X X

Maps (for localization, SLAM, map building)

[subpages depth=”5″]

Paper: Malaga dataset 2009 with 6D ground truth

A Collection of Outdoor Robotic Datasets with centimeter-accuracy Ground TruthJose-Luis Blanco, Francisco-Angel Moreno, Javier Gonzalez, Autonomous Robots , 2009 – (Draft PDF) – (Official PDF )

Abstract: The lack of publicly accessible datasets with a reliable ground truth has prevented in the past a fair and coherent comparison of different methods proposed in the mobile robot Simultaneous Localization and Mapping (SLAM) literature. Providing such a ground truth renders specially challenging in the case of visual SLAM, where the world model is 3-dimensional and the robot path is 6-dimensional. This work addresses both the practical and theoretical issues found while building a collection of six outdoor datasets. It is discussed how to estimate the 6-d vehicle path from readings of a set of three Real Time Kinematics (RTK) GPS receivers, as well as the associated uncertainty bounds that can be employed to evaluate the performance of Visual SLAM methods. The vehicle was also equipped with several laser scanners, from which reference point clouds are built as a test-bed for other algorithms such as segmentation or surface fitting. All the datasets, calibration information and associated software tools are available for download.

Paper: Occupancy Grid Matching

Paper: J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, “A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps”, Robotica, 2013 (DOI, draft PDF).

Abstract:This article presents a new approach to matching occupancy grid maps by means of finding correspondences between a set of sparse features detected in the maps. The problem is stated here as a special instance of generic image registration. To cope with the uncertainty and ambiguity that arise from matching grid maps, we introduce a modified RANSAC algorithm which searches for a dynamic number of internally consistent subsets of feature pairings from which to compute hypotheses about the translation and rotation between the maps. By providing a (possibly multi-modal) probability distribution of the relative pose of the maps, our method can be seamlessly integrated into large-scale mapping frameworks for mobile robots. This article provides a benchmarking of different detectors and descriptors, along extensive experimental results that illustrate the robustness of the algorithm with a 97% success ratio in loop-closure detection for ~1700 matchings between local maps obtained from four publicly available datasets.

An alternative to the Mahalanobis distance for determining optimal correspondences in data association

“An alternative to the Mahalanobis distance for determining optimal correspondences in data association”. J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal. IEEE Transactions on Robotics (T-RO), vol. 28, no.4, 980-986, 2012. (BibtexDraft PDF)

Abstract: The most common criteria for determining data association rely on minimizing the squared Mahalanobis distance (SMD) between observations and predictions. We hold that the SMD is just a heuristic, while the alternative matching likelihood (ML) is the optimal statistic to be maximized. Thorough experiments undoubtedly conﬁrm this idea, with false positive reductions of up to 16%.

1. Description

The program track-video-features demonstrates live detection and tracking of features in a video stream.

To invoke it, just type from the console:

track-video-features and a GUI dialog will ask you to pick the video source (among video files, rawlogs, live firewire or USB cameras, a Kinect sensor, etc.). Alternatively, use: track−video−features <DATASET.rawlog> track-video-features <video_file.{avi/mpg/mp4/flv}> to directly run it with a robotic dataset or a video file. 2. Source code The program is located at <MRPT>/apps/track-video-features/: https://raw.github.com/MRPT/mrpt/master/apps/track-video-features/track-video-feats_main.cpp See this tutorial on the related data structures and classes. 3. Demo videos 1. A short sequence in a typical office scenario: Test with a sample video sequence published by Andrew Davison among his MonoSLAM works. Application: simul-landmarks 1. Overview The program simulates range-bearing observations of a robot following a 2D or 3D(6D) path. Note that this simulator can be used for generating synthetic datasets for both 2D “classic” range-bearing SLAM as well as for 3D stereo slam-like SLAM problems. The location and number of landmarks are configurable via the input configuration file. Also, the robot path can be fixed or a kind of “random walk”. The random seed for both the path and the sensor noise can be set, so the generated datasets can be exactly reproduced by different researchers from the same configuration file. The generated rawlogs will contain observations of typemrpt::obs::CObservationBearingRange. Application: ReactiveNav3D-Demo See also: ROS package mrpt_reactivenav2d 1. Description This GUI application is designed to test a 3D reactive navigator. It simulates a “2.5D world” where obstacles are modeled through several occupancy grid maps at different heights. The robot shape can be customized and 2D or 3D range sensors can be “added” to the robot at any 3D relative pose. During its execution, the user sets different target locations for running reactive navigation simulations. Note that navigation logs generated by this simulator (as well as from real robots using these classes) can be inspected with the program navlog-viewer. Application: ReactiveNavigationDemo See also: ROS package mrpt_reactivenav2d This GUI application displays a flat world where obstacles are described through an occupancy grid map and where the user can choose target locations for running reactive navigation simulations. The map can be changed to anyone supplied by the user, and all the options that determine the behavior of the navigation system can be as well modified by the user to experiment by changing values. 1. Description This GUI application displays a flat world where obstacles are described through an occupancy grid map and where the user can choose target locations for running reactive navigation simulations. The map can be changed to anyone supplied by the user, and all the options that determine the behavior of the navigation system can be as well modified by the user to experiment by changing values. Application: pf-localization =&0=& ROS package mrpt_localization The application pf-localization implements a particle filter for localization (aka Markov Localization) of a mobile robot given odometry, a map of the environment and any number and combination of sensor observations such as a likelihood can be computed given the map. This generic implementation is possible through the generic design of metric maps in the MRPT C++ libraries. 1. Overview The application pf-localization implements a particle filter for localization (aka Markov Localization) of a mobile robot given odometry, a map of the environment and any number and combination of sensor observations such as a likelihood can be computed given the map. This generic implementation is possible through the generic design of metric maps in the MRPT C++ libraries. One typical situation is 2D laser scans used against an occupancy grid map. Note that the program can handles both action-observation and observations-only datasets (since MRPT 0.9.3), but the action-observation is preferred. The application processes all data from a rawlog: it is not intended for real-time operation on a robot, though the source code requires little modification to do so. Application: observations2map This program takes a simplemap as input (e.g. from a SLAM app), and generates one or several metric maps from its observations and poses, dumping the map representations to individual files. A typical usage is to rebuild occupancy gridmaps of different resolutions after building a map with icp-slam or rbpf-slam. Usage: For example: About the program arguments: <config_file.ini>: A configuration file. Its minimum content must be a section with the description of a CMultiMetricMap: how many points maps, grid maps, etc. it should have and all their parameters. The default section name ([MappingApplication]) is such that configuration files for the applications icp-slam or rbpf-slam can be used without changes. See examples of those config files in the icp-slam and rbpf-slam sample config file directories. <observations.simplemap>: A file containing the serialization of a mrpt::maps::CSimpleMap Application: map-partition A front-end to the class mrpt::slam::CIncrementalMapPartitioner. The map to be processed, a “.simplemap” file, is passed in the command line, and the program generates the sub-maps and the original and rearranged weight matrices. 1. Spectral graph bisection-based map partitioning (map-partition) Example: kinect-to-2d-laser-demo 1. Introduction The Kinect sensor provides an inexpensive alternative to traditional laser scanners for working in small workspaces. Transforming Kinect’s 3D range images into 2D scans allows us to exploit the large body of methods and techniques existing for 2D laser scans (e.g. in SLAM or localization). MRPT provides this conversion as the method CObservation3DRangeScan::convertTo2DScan(), which returns a 2D laser scan with more “rays” (N) than columns has the 3D observation (W), exactly: N = W * oversampling_ratio. This oversampling is required since laser scans sample the space at evenly-separated angles, while a range camera follows a tangent-like distribution. By oversampling we make sure we don’t leave “gaps” unseen by the virtual “2D laser”. Sparser Relative Bundle Adjustment (SRBA) NOTICE: Since MRPT 1.3.2 (Oct 2015), SRBA is an independent project outside of the MRPT source tree. See its GitHub repo and page. 1. Theory Bundle adjustment is the name given to one solution to visual SLAM based on maximum-likelihood estimation (MLE) over the space of map features and camera poses. However, it is by no way limited to visual maps, since the same technique is also applicable to maps of pose constraints (graph-SLAM) or any other kind of feature maps not relying on visual information. Unit testing in MRPT Each of MRPT’s libraries (see list) have its own benchmark of tests to verify and assure that each class of function behaves as expected. For doing so, MRPT uses Google’s gtest unit testing library (read more on the concept of unit testing in this article on Wikipedia). The mechanism to add new tests is fairly simple, since the CMake scripts worry of recognizing all those source files that implement tests (as explained here) and put them out of the normal MRPT libraries into other special programs which are automatically executed upon testing. As a result, under Visual Studio users can test the correct behavior of MRPT by issuing a “build” command on the target “test”. On Unix/Linux/MacOS the same is achieved issuing a make test command. Generating 3D point clouds from RGB+D observations (CObservation3DRangeScan objects) 1. Calibration parameters Firstly, you must have very clear the calibration parameters involved in the process. Each “RGB+D observation” is stored in MRPT as one object of type mrpt::slam::CObservation3DRangeScan (click to see its full description), which for this tutorial will be assumed that contains: These observations can be captured from a sensor (e.g. Kinect) in real-time, or loaded from a dataset, as explained here. In any case, the calibration parameters of both cameras will come already filled in with their correct values (or, at least, those the user provided in the moment of capturing/grabbing the dataset!). Kinect and MRPT 1. Overview MRPT implements a common C++ interface to Xbox Kinect, a new RGB+D sensor with an immense potential in mobile robotics. The implementation offers the user only one unified interface. Behind the scenes, MRPT uses one of these libraries to actually access the sensor: Intensity (RGB) images are automatically converted to the OpenCV’s format (mrpt::utils::CImage), 10bit or 11bit raw depth is converted to meters, and methods are provided to handle the two matrices of camera parameters (one for the RGB camera and one for the “range image”). Rectifying stereo images 1. Theory See the documentation from Willow Garage’s OpenCV for The following video (view on YouTube) illustrates the effects of each of these parameters: 2. In practice: step by step procedure with MRPT See docs for the class: mrpt::vision::CStereoRectifyMap 3. Example C++ code 4. A complete example See the example: /samples/stereoRectify Probabilistic Motion Models 1. Introduction Within a particle filter, the samples are propagated at each time step using some given proposal distribution. A common approach for mobile robots is taking the probabilistic motion model directly as this proposal. In the MRPT there are two models for probabilistic 2D motion, implemented in mrpt::slam::CActionRobotMovement2D. To use them just fill out the option structure “motionModelConfiguration” and select the method in “CActionRobotMovement2D::TMotionModelOptions::modelSelection”. Iterative Closest Point (ICP) and other matching algorithms 1. Iterative Closest Point (ICP) Algorithms Originally introduced in [1] , the ICP algorithm aims to find the transformation between a point cloud and some reference surface (or another point cloud), by minimizing the square errors between the corresponding entities. The ”iterative” of ICP comes from the fact that the correspondences are reconsidered as the solution comes closer to the error local minimum. As any gradient descent method, the ICP is applicable when we have a relatively good starting point in advance. Otherwise, it will be trapped into the first local minimum and the solution will be useless. 3D-ICP Example Results The main method used in this example is CICP::Align3DPDF. This example first simulates a pair of 3D point clouds by performing ray-tracing from two different poses: Next, a small error in all six dimensions is introduced in the pose of one point cloud, leading to unaligned point clouds: After applying the ICP-3D algorithm to these unaligned maps, the algorithm successfully recovers the correct alignment, as can be seen in the figure: The code The up-to-date example can be found in: https://github.com/MRPT/mrpt/tree/master/samples/slam_icp3d_simple_example The hierarchical model of metric maps 1. About maps in the MRPT All metric maps have a common interface to ease polymorphism and generic programming. To see these common methods, refer to the C++ class CMetricMap. All the map classes are within the namespace mrpt::slam, which is omitted here for readability. See also the documentation of the library mrpt-maps. To ease even more the implementation of generic algorithms, there exists one very important kind of map, the multi-metric map. This class offers the interface of a normal metric map, but it holds internally an arbitrary number of other metric maps. Occupancy grids See the C++ API documentation for: mrpt::slam::COccupancyGridMap2D (part of the library mrpt-maps) 1. Theoretical Bases Log-odds method. (write me!) 2. An efficient implementation 2.1. Map representation The log-odds theoretical method for Bayesian integration is implemented using a discretization to 8 bits per cell: 2.2. A benchmark The next graphs summarize the performance of the most common operations on grid maps. The results have been generated by the program “samples/benchmarkGridmaps”, on a Intel Core 2 Duo 2.2Ghz, MRPT version 0.5.5, averaging over thousands of repetitions. Paper: J.L. Blanco’s Phd Thesis Contributions to Localization, Mapping and Navigation in Mobile RoboticsPhD Thesis, Jose-Luis Blanco-Claraco, November 13th, 2009. Downloads: PDF (12.6Mb) – Citation (Bibtex) – Slides (17.5 Mb) – Slides+videos (196 Mb) Abstract: This thesis focuses on the problem of enabling mobile robots to autonomously build world models of their environments and to employ them as a reference to self–localization and navigation. For mobile robots to become truly autonomous and useful, they must be able of reliably moving towards the locations required by their tasks. This simple requirement gives raise to a myriad of problems that has populated research in the mobile robotics community for decades. Among these issues, two of the most relevant are: (i) secure autonomous navigation while avoiding collisions and (ii) the employment of an adequate world model for robot self-referencing within the environment and also for locating places of interest. In spite of presenting contributions regarding mobile robot navigation, the main focus of this thesis is on the latter problem, usually referred to as Simultaneous Localization and Mapping (SLAM). One of the most interesting contributions of this thesis is a novel approach to extend SLAM to large-scale scenarios by means of a seamless integration of geometric and topological map building in a probabilistic framework that estimates the hybrid metric-topological (HMT) state space of the robot path. The proposed framework unifies in an elegant manner the research areas of topological mapping, reasoning on topological maps and metric SLAM, providing also a natural integration of SLAM and the “robot awakening” problem. Other contributions presented in this thesis cover a wide variety of topics, such as optimal estimation in particle filters, a new probabilistic observation model for laser scanners based on consensus theory, a novel measure of the uncertainty in grid mapping, an efficient method for range-only SLAM, a grounded method for partitioning large maps into submaps, a multi-hypotheses approach to grid map matching, and a mathematical framework for extending simple obstacle avoidance methods to realistic robots. PhD Awards: Particle Filter Algorithms This page describes the theory behinds the particle filter algorithms implemented in the C++ libraries of MRPT. See also the different resampling schemes. For the list of corresponding C++ classes see Particle Filters. 1. Sequential Importance Resampling – SIR (pfStandardProposal) Standard proposal distribution + weights according to likelihood function. 2. Auxiliary Particle Filter – APF (pfAuxiliaryPFStandard) This method was introduced by Pitt and Shephard in 1999 [1] Let’s assume the filtered posterior is described by the followingM\$ weighted samples:

Then, each step in the algorithm consists of first drawing a sample of the particle index $$k$$ which will be propragated from $$t-1$$ into the new step $$t$$. These indexes are auxiliary variables only used as an intermediary step, hence the name of the algorithm. The indexes are drawn according to the likelihood of some reference point $$\mu^{(i)}_t$$ which in some way is related to the transition model $$x_t|x_{t-1}$$ (for example, the mean, a sample, etc.):

Particle Filters

The following C++ classes are the base for different PF implementations all across MRPT:

• mrpt::bayes::CParticleFilterCapable: This virtual class defines the interface that any particles based PDF class must implement in order to be executed by a CParticleFilter.
• mrpt::bayes::CParticleFilter: The class that executes iterations on a CParticleFilterCapable object.
• The container model mrpt::bayes::CParticleFilterData is used by some classes which are not really intended to be processed with a PF. It just represent a C++ template for sets of weighted samples a any given type, and common memory-keeping tasks.

Both the specific particle filter algorithm to run and the resampling scheme to use can be independently selected in the options structuremrpt::bayes::CParticleFilter::TParticleFilterOptions:

=&0=&

Kalman Filters

Kalman Filter algorithms (EKF,IEKF,UKF) are centralized in one single virtual class, mrpt::bayes::CKalmanFilterCapable. This class contains the system state vector and the system covariance matrix, as well as a generic method to execute one complete iteration of the selected algorithm. In practice, solving a specific problem requires deriving a new class from this virtual class and implementing a few methods such as transforming the state vector through the transition model, or computing the Jacobian of the observation model linearized at some given value of the state space.