Landmark-based SLAM

Basics

|image: 12\_Users\_dellaert\_git\_github\_doc\_images\_FactorGraph4.png| Figure 10: Factor graph for landmark-based SLAM

In landmark-based SLAM, we explicitly build a map with the location of observed landmarks, which introduces a second type of variable in the factor graph besides robot poses. An example factor graph for a landmark-based SLAM example is shown in Figure 10, which shows the typical connectivity: poses are connected in an odometry Markov chain, and landmarks are observed from multiple poses, inducing binary factors. In addition, the pose \(x_{1}\) has the usual prior on it.

|image: 13\_Users\_dellaert\_git\_github\_doc\_images\_example2.png| Figure 11: The optimized result along with covariance ellipses for both poses (in green) and landmarks (in blue). Also shown are the trajectory (red) and landmark sightings (cyan).

The factor graph from Figure 10 can be created using the MATLAB code in Listing 5.1. As before, on line 2 we create the factor graph, and Lines 8-18 create the prior/odometry chain we are now familiar with. However, the code on lines 20-25 is new: it creates three measurement factors, in this case “bearing/range” measurements from the pose to the landmark.

% Create graph container and add factors to it
graph = NonlinearFactorGraph;

% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);

% Add prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
% add directly to graph
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));

% Add odometry
odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));

% Add bearing/range measurement factors
degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

Of Keys and Symbols

The only unexplained code is on lines 4-6: here we create integer keys for the poses and landmarks using the *symbol* function. In GTSAM, we address all variables using the *Ke*y type, which is just a typedef to *size_t* a 32 or 64 bit integer, depending on your platform. The keys do not have to be numbered continuously, but they do have to be unique within a given factor graph. For factor graphs with different types of variables, we provide the *symbol* function in MATLAB, and the *Symbol* type in C++, to help you create (large) integer keys that are far apart in the space of possible keys, so you don’t have to think about starting the point numbering at some arbitrary offset. To create a a symbol key you simply provide a character and an integer index. You can use base 0 or 1, or use arbitrary indices: it does not matter. In the code above, we we use ‘x’ for poses, and ‘l’ for landmarks.

The optimized result for the factor graph created by Listing 5.1 is shown in Figure 11, and it is readily apparent that the landmark \(l_{1}\) with two measurements is better localized. In MATLAB we can also examine the actual numerical values, and doing so reveals some more GTSAM magic:

>> result Values with 5 values: l1: (2, 2) l2: (4, 2) x1: (-1.8e-16, 5.1e-17, -1.5e-17) x2: (2, -5.8e-16, -4.6e-16) x3: (4, -3.1e-15, -4.6e-16)

Indeed, the keys generated by symbol are automatically detected by the *print* method in the *Values* class, and rendered in human-readable form “x1”, “l2”, etc, rather than as large, unwieldy integers. This magic extends to most factors and other classes where the Key type is used.

A Larger Example

|image: 14\_Users\_dellaert\_git\_github\_doc\_images\_littleRobot.png| Figure 12: A larger example with about 100 poses and 30 or so landmarks, as produced by gtsam_examples/PlanarSLAMExample_graph.m

GTSAM comes with a slightly larger example that is read from a .graph file by PlanarSLAMExample_graph.m, shown in Figure 12. To not clutter the figure only the marginals are shown, not the lines of sight. This example, with 119 (multivariate) variables and 517 factors optimizes in less than 10 ms.

A Real-World Example

|image: 15\_Users\_dellaert\_git\_github\_doc\_images\_Victoria.png| Figure 13: Small section of optimized trajectory and landmarks (trees detected in a laser range finder scan) from data recorded in Sydney’s Victoria Park (dataset due to Jose Guivant, U. Sydney).

A real-world example is shown in Figure 13, using data from a well known dataset collected in Sydney’s Victoria Park, using a truck equipped with a laser range-finder. The covariance matrices in this figure were computed very efficiently, as explained in detail in (Kaess and Dellaert, 2009). The exact covariances (blue, smaller ellipses) obtained by our fast algorithm coincide with the exact covariances based on full inversion (orange, mostly hidden by blue). The much larger conservative covariance estimates (green, large ellipses) were based on our earlier work in (Kaess et al., 2008).