Cartographer can be seen as two separate, but related subsystems.
The first one is **local SLAM** (sometimes also called **frontend** or local trajectory builder).
Its job is to build a succession of **submaps**.
Each submap is meant to be locally consistent but we accept that local SLAM drifts over time.
Most of the local SLAM options can be found in `install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua`_ for 2D and `install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua`_ for 3D. (for the rest of this page we will refer to `TRAJECTORY_BUILDER_nD` for the common options)
On a higher abstraction, the job of local SLAM is to generate good submaps and the job of global SLAM is to tie them most consistently together.
Input
-----
Range finding sensors (for example: LIDARs) provide depth information in multiple directions.
However, some of the measurements are irrelevant for SLAM.
If the sensor is partially covered with dust or if it is directed towards a part of the robot, some of the measured distance can be considered as noise for SLAM.
On the other hand, some of the furthest measurements can also come from undesired sources (reflection, sensor noise) and are irrelevant for SLAM as well.
To tackle those issue, Cartographer starts by applying a bandpass filter and only keeps range values between a certain min and max range.
Those min and max values should be chosen according to the specifications of your robot and sensors.
..code-block:: lua
TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range
..note::
In 2D, Cartographer replaces ranges further than max_range by ``TRAJECTORY_BUILDER_2D.missing_data_ray_length``. It also provides a ``max_z`` and ``min_z`` values to filter 3D point clouds into a 2D cut.
..note::
In Cartographer configuration files, every distance is defined in meters
Distances are measured over a certain period of time, while the robot is actually moving.
However, distances are delivered by sensors "in batch" in large ROS messages.
Each of the messages' timestamp can be considered independently by Cartographer to take into account deformations caused by the robot's motion.
The more often Cartographer gets measurements, the better it becomes at unwarping the measurements to assemble a single coherent scan that could have been captured instantly.
It is therefore strongly encouraged to provide as many range data (ROS messages) by scan (a set of range data that can be matched against another scan) as possible.
..code-block:: lua
TRAJECTORY_BUILDER_nD.num_accumulated_range_data
Range data is typically measured from a single point on the robot but in multiple angles.
This means that close surfaces (for instance the road) are very often hit and provide lots of points.
On the opposite, far objects are less often hit and offer less points.
In order to reduce the computational weight of points handling, we usually need to subsample point clouds.
However, a simple random sampling would remove points from areas where we already have a low density of measurements and the high-density areas would still have more points than needed.
To address that density problem, we can use a voxel filter that downsamples raw points into cubes of a constant size and only keeps the centroid of each cube.
A small cube size will result in a more dense data representation, causing more computations.
A large cube size will result in a data loss but will be much quicker.
..code-block:: lua
TRAJECTORY_BUILDER_nD.voxel_filter_size
After having applied a fixed-size voxel filter, Cartographer also applies an **adaptive voxel filter**.
This filter tries to determine the optimal voxel size (under a max length) to achieve a target number of points.
In 3D, two adaptive voxel filters are used to generate a high resolution and a low resolution point clouds, their usage will be clarified in :ref:`local-slam`.
An Inertial Measurement Unit can be an useful source of information for SLAM because it provides an accurate direction of gravity (hence, of the ground) and a noisy but good overall indication of the robot's rotation.
In order to filter the IMU noise, gravity is observed over a certain amount of time.
If you use 2D SLAM, range data can be handled in real-time without an additional source of information so you can choose whether you'd like Cartographer to use an IMU or not.
With 3D SLAM, you need to provide an IMU because it is used as an initial guess for the orientation of the scans, greatly reducing the complexity of scan matching.
..code-block:: lua
TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant
..note::
In Cartographer configuration files, every time value is defined in seconds
.._local-slam:
Local SLAM
----------
Once a scan has been assembled and filtered from multiple range data, it is ready for the local SLAM algorithm.
Local SLAM inserts a new scan into its current submap construction by **scan matching** using an initial guess from the **pose extrapolator**.
The idea behind the pose extrapolator is to use sensor data of other sensors besides the range finder to predict where the next scan should be inserted into the submap.
Two scan matching strategies are available:
- The ``CeresScanMatcher`` takes the initial guess as prior and finds the best spot where the scan match fits the submap.
It does this by interpolating the submap and sub-pixel aligning the scan.
This is fast, but cannot fix errors that are significantly larger than the resolution of the submaps.
If your sensor setup and timing is reasonable, using only the ``CeresScanMatcher`` is usually the best choice to make.
- The ``RealTimeCorrelativeScanMatcher`` can be enabled if you do not have other sensors or you do not trust them.
It uses an approach similar to how scans are matched against submaps in loop closure (described later), but instead it matches against the current submap.
The best match is then used as prior for the ``CeresScanMatcher``.
This scan matcher is very expensive and will essentially override any signal from other sensors but the range finder, but it is robust in feature rich environments.
Either way, the ``CeresScanMatcher`` can be configured to give a certain weight to each of its input.
The weight is a measure of trust into your data, this can be seen as a static covariance.
The unit of weight parameters are dimensionless quantities and can't be compared between each others.
The bigger the weight of a source of data is, the more emphasis Cartographer will put on this source of data when doing scan matching.
Sources of data include occupied space (points from the scan), translation and rotation from the pose extrapolator (or ``RealTimeCorrelativeScanMatcher``)
In 3D, the ``occupied_space_weight_0`` and ``occupied_space_weight_1`` parameters are related, respectively, to the high resolution and low resolution filtered point clouds.
The ``CeresScanMatcher`` gets its name from `Ceres Solver`_, a library developed at Google to solve non-linear least squares problems.
The scan matching problem is modelled as the minimization of such a problem with the **motion** (a transformation matrix) between two scans being a parameter to determine.
Ceres optimizes the motion using a descent algorithm for a given number of iterations.
Ceres can be configured to adapt the convergence speed to your own needs.
Probability grids cut out space into a 2D or 3D table where each cell has a fixed size and contains the odds of being obstructed.
Odds are updated according to "*hits*" (where the range data is measured) and "*misses*" (the free space between the sensor and the measured points).
Both *hits* and *misses* can have a different weight in occupancy probability calculations giving more or less trust to occupied or free space measurements.
In 2D, only one probability grid per submap is stored.
In 3D, for scan matching performance reasons, two *hybrid* probability grids are used.
(the term "hybrid" only refers to an internal tree-like data representation and is abstracted to the user)
- a low resolution hybrid grid for far measurements
- a high resolution hybrid grid for close measurements
Scan matching starts by aligning far points of the low resolution point cloud with the low resolution hybrid grid and then refines the pose by aligning the close high resolution points with the high resolution hybrid grid.
Cartographer ROS provides an RViz plugin to visualize submaps. You can select the submaps you want to see from their number. In 3D, RViz only shows 2D projections of the 3D hybrid probability grids (in grayscale). Options are made available in RViz's left pane to switch between the low and high resolution hybrid grids visualization.
**TODO**: *Documenting TSDF configuration*
Global SLAM
-----------
While the local SLAM generates its succession of submaps, a global optimization (usually refered to as "*the optimization problem*" or "*sparse pose adjustment*") task runs in background.
Its role is to re-arrange submaps between each other so that they form a coherent global map.
For instance, this optimization is in charge of altering the currently built trajectory to properly align submaps with regards to loop closures.
The optimization is run in batches once a certain number of trajectory nodes was inserted. Depending on how frequently you need to run it, you can tune the size of these batches.
..code-block:: lua
POSE_GRAPH.optimize_every_n_nodes
..note::
Setting POSE_GRAPH.optimize_every_n_nodes to 0 is a handy way to disable global SLAM and concentrate on the behavior of local SLAM. This is usually one of the first thing to do to tune Cartographer.
The global SLAM is a kind of "*GraphSLAM*", it is essentially a pose graph optimization which works by building **constraints** between **nodes** and submaps and then optimizing the resulting constraints graph.
Constraints can intuitively be thought of as little ropes tying all nodes together.
The sparse pose adjustement fastens those ropes altogether.
The resulting net is called the "*pose graph*".
..note::
Constraints can be visualized in RViz, it is very handy to tune global SLAM. One can also toggle ``POSE_GRAPH.constraint_builder.log_matches`` to get regular reports of the constraints builder formatted as histograms.
- Non-global constraints (also known as intra submaps constraints) are built automatically between nodes that are closely following each other on a trajectory.
- Global constraints (also referred to as loop closure constraints or inter submaps constraints) are regularly searched between a new submap and previous nodes that are considered "*close enough*" in space (part of a certain **search window**) and a strong fit (a good match when running scan matching).
In practice, global constraints can do more than finding loop closures on a single trajectory. They can also align different trajectories recorded by multiple robots but we will keep this usage and the parameters related to "global localization" out of the scope of this document.
To limit the amount of constraints (and computations), Cartographer only considers a subsampled set of all close nodes for constraints building.
This is controlled by a sampling ratio constant.
Sampling too few nodes could result in missed constraints and ineffective loop closures.
Sampling too many nodes would slow the global SLAM down and prevent real-time loop closures.
..code-block:: lua
POSE_GRAPH.constraint_builder.sampling_ratio
When a node and a submap are considered for constraint building, they go through a first scan matcher called the ``FastCorrelativeScanMatcher``.
This scan matcher has been specifically designed for Cartographer and makes real-time loop closures scan matching possible.
The ``FastCorrelativeScanMatcher`` relies on a "*Branch and bound*" mechanism to work at different grid resolutions and efficiently eliminate incorrect matchings.
This mechanism is extensively presented in the Cartographer paper presented earlier in this document.
It works on an exploration tree whose depth can be controlled.
Once the ``FastCorrelativeScanMatcher`` has a good enough proposal (above a minimum score of matching), it is then fed into a Ceres Scan Matcher to refine the pose.
When Cartographer runs *the optimization problem*, Ceres is used to rearrange submaps according to multiple *residuals*.
Residuals are calculated using weighted cost functions.
The global optimization has cost functions to take into account plenty of data sources: the global (loop closure) constraints, the non-global (matcher) constraints, the IMU acceleration and rotation measurements, the local SLAM rough pose estimations, an odometry source or a fixed frame (such as a GPS system).
The weights and Ceres options can be configured as described in the :ref:`local-slam` section.
One can find useful information about the residuals used in the optimization problem by toggling ``POSE_GRAPH.max_num_final_iterations``
As part of its IMU residual, the optimization problem gives some flexibility to the IMU pose and, by default, Ceres is free to optimize the extrinsic calibration between your IMU and tracking frame.
If you don't trust your IMU pose, the results of Ceres' global optimization can be logged and used to improve your extrinsic calibration.
If Ceres doesn't optimize your IMU pose correctly and you trust your extrinsic calibration enough, you can make this pose constant.
In residuals, the influence of outliers is handled by a **Huber loss** function configured with a certain a Huber scale.
The bigger the Huber scale, `the higher is the impact`_ of (potential) outliers.
.._the higher is the impact: https://github.com/ceres-solver/ceres-solver/blob/0d3a84fce553c9f7aab331f0895fa7b1856ef5ee/include/ceres/loss_function.h#L172
..code-block:: lua
POSE_GRAPH.optimization_problem.huber_scale
Once the trajectory is finished, Cartographer runs a new global optimization with, typically, a lot more iterations than previous global optimizations.
This is done to polish the final result of Cartographer and usually does not need to be real-time so a large number of iterations is often a right choice.
