ISAM with Range Measurements
Contents
2.5. ISAM with Range Measurements¶
2.5.1. Introduction¶
This example illustrates how ISAM2 (Incremental Smoothing and Mapping), whose implementation is available with GTSAM, can be used to solve a 2D SLAM problem from noisy range measurements to some landmarks. ISAM provides a framework to solve the full SLAM problem (i.e inferring simultaneously the entire robot trajectory and a map from a set of data) using an incremental approach for updating the smoothing information matrix. The incremental nature of the approach results in computationally efficient updates on the trajectory and map estimates when new observations are available, and allows for a resolution of the full SLAM problem in real-time.
A presentation of the first version of ISAM is available here, while ISAM2, which exploits a bayes’ tree data structure for further efficiency, is introduced here.
In the following example, ISAM2 is run on data from the second UltraWide Band (UWB) ranging dataset, B2 or “plaza 2”. This dataset contains both odometry data (i.e traveled distance and delta rotation between successive steps) as well as range measurements to some fixed landmarks. The data has been acquired using radio-based ranging systems, and a lawn mower robot moving in an outdoor environment. Fixed radio nodes are used as landmarks. As they communicate their unique ID in addition to the measured distance to the radio node mounted on the robot, it is not necessary to adress a data association problem.
“Navigating with Ranging Radios: Five Data Sets with Ground Truth”, by Joseph Djugash, Bradley Hamner, and Stephan Roth, available at https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf
# pylint: disable=invalid-name, E1101
# Install the pre-requisites
%pip -q install gtbook
# Import useful python libraries
from gtsam import Point2, Pose2
import plotly.express as px
import numpy as np
import gtsam
import math
import matplotlib.pyplot as plt
from gtsam.utils import plot
from numpy.random import default_rng
rng = default_rng()
NM = gtsam.noiseModel # This library contains distribution models of interest, especially Gaussian
Note: you may need to restart the kernel to use updated packages.
# pylint: disable=invalid-name, E1101
# Install the pre-requisites
%pip -q install gtbook
# Import useful python libraries
from gtsam import Point2, Pose2
import plotly.express as px
import numpy as np
import gtsam
import math
import matplotlib.pyplot as plt
from gtsam.utils import plot
from numpy.random import default_rng
rng = default_rng()
NM = gtsam.noiseModel # This library contains distribution models of interest, especially Gaussian
Note: you may need to restart the kernel to use updated packages.
2.5.2. Loading the dataset¶
We start by loading the dataset in a variable with a dictionary type.
The odometry data is first loaded along with the corresponding time stamps.
# Loads the odometry data
# The dead-reckoning odometry inputs (delta distance traveled and delta heading change) have the form:
# Time (sec) ; Delta Distance Traveled (m) ; Delta Heading (rad)
odometry = {}
data_file = gtsam.findExampleDataFile("Plaza2_DR.txt")
for row in np.loadtxt(data_file):
t, distance_traveled, delta_heading = row
odometry[t] = Pose2(distance_traveled, 0, delta_heading)
M = len(odometry)
print(f"Read {M} odometry entries.")
Read 4090 odometry entries.
Next, we load the range measurements. One may note that these may not have been received at the same time as the odometry inputs, so their respective time stamp is also carefully recorded. The number of range measurements may also differ from that of the odometry inputs. Besides, in practice, the RF nodes are placed on the robot and on fixed landmarks. The robot node sends a signal, and receives the range measurements to each responding nodes (landmarks) along with their unique ID.
# Loads the range measurements which have the form:
# Time (sec) ; Sender / Antenna ID ; Receiver Node ID ; Range (m)
triples = []
data_file = gtsam.findExampleDataFile("Plaza2_TD.txt")
for row in np.loadtxt(data_file):
t, sender, receiver, _range = row
triples.append((t, int(receiver), _range))
K = len(triples)
print(f"Read {K} range triples.")
Read 1816 range triples.
2.5.3. Setting the parameters of our SLAM problem¶
An ISAM update integrates two important steps, which are variable re-ordering and relinearization. Yet, these steps can be quite demanding in terms of computational effort. To limit the computational load, an ISAM update is not performed at every measurement step, but when the new set of collected measurements is considered large enough, as defined below.
# parameters
minK = 150 # minimum number of range measurements to process initially
incK = 25 # minimum number of new range measurements to process for one ISAM update
robust = True
We also set noise parameters that define the standard deviations of Gaussian models. We define prior distributions for the initial poses and specify the noise model of the odometry inputs.
# Set noise parameters
priorSigmas = gtsam.Point3(1, 1, math.pi) # In the form [sigma_x, sigma_y, sigma_thetha] for the pose
odoSigmas = gtsam.Point3(0.05, 0.01, 0.1) # In the form [sigma_x, sigma_y, sigma_thetha] for the odometry
sigmaR = 100 # range standard deviation
priorNoise = NM.Diagonal.Sigmas(priorSigmas) # prior
looseNoise = NM.Isotropic.Sigma(2, 1000) # loose LM prior
odoNoise = NM.Diagonal.Sigmas(odoSigmas) # odometry
gaussian = NM.Isotropic.Sigma(1, sigmaR) # non-robust
tukey = NM.Robust.Create(NM.mEstimator.Tukey.Create(15), gaussian) # robust
rangeNoise = tukey if robust else gaussian
2.5.4. Solving the SLAM problem¶
We now instantiate an ISAM2 object that will hold all the specifications of our SLAM problem.
# Initialize iSAM
isam = gtsam.ISAM2()
print(isam)
: cliques: 0, variables: 0
In our implementation of ISAM, we keep track of the new factors, related the realization of either a range or an odometry measurement, between the ISAM updates. We add, to the initial graph, a prior on the first pose.
pose0 = Pose2(-34.2086489999201, 45.3007639991120, math.pi - 2.021089)
newFactors = gtsam.NonlinearFactorGraph()
newFactors.addPriorPose2(0, pose0, priorNoise)
initial = gtsam.Values()
initial.insert(0, pose0)
print(newFactors, initial)
NonlinearFactorGraph: size: 1
Factor 0: PriorFactor on 0
prior mean: (-34.2086, 45.3008, 1.1205)
noise model: diagonal sigmas [1; 1; 3.14159265];
Values with 1 values:
Value 0: (gtsam::Pose2)
(-34.208649, 45.300764, 1.12050365)
We execute the main loop for the optimization. The loop iterates over the odometry measurements to iteratively add odometry factors to the graph of the SLAM problem. The loop also keeps track of initial values for the new pose variables created between two ISAM updates. These initial values are obtained by forwarding the odometry information from the previous pose. Range factors that relate the current pose to the past and non-associated measurements are added to the graph. Besides, a new landmark is initialized whenever a receiver ID is observed for the first time. Finally, ISAM updates are carried out when the new range measurements are in sufficient number.
# set some loop variables
i = 1 # step counter
k = 0 # range measurement counter
initialized = False
lastPose = pose0
countK = 0
initializedLandmarks = set()
# Loop over odometry
for t, relative_pose in odometry.items():
# Add an odometry factor
newFactors.add(gtsam.BetweenFactorPose2(i - 1, i, relative_pose,
odoNoise))
# Predict pose and add it as an initial estimate
predictedPose = lastPose.compose(relative_pose)
lastPose = predictedPose
initial.insert(i, predictedPose)
# Check if there are range factors to be added. The strategy for adding them is to associate each
# range measurement, received between two odometry observation times, to the pose variable observed during
# the later odometry observation time of the two.
while (k < K) and (triples[k][0] <= t):
j = triples[k][1]
landmark_key = gtsam.symbol('L', j)
_range = triples[k][2]
newFactors.add(gtsam.RangeFactor2D(
i, landmark_key, _range, rangeNoise))
# Initialize a new landmark if the received ID has never been
# seen in the past.
if landmark_key not in initializedLandmarks:
p = rng.normal(loc=0, scale=100, size=(2,))
initial.insert(landmark_key, p)
print(f"Adding landmark L{j}")
initializedLandmarks.add(landmark_key)
# We also add a very loose prior on the landmark in case there is only
# one sighting, which cannot fully determine the landmark.
newFactors.add(gtsam.PriorFactorPoint2(
landmark_key, Point2(0, 0), looseNoise))
k = k + 1
countK = countK + 1
# Check whether to update iSAM 2
if (k > minK) and (countK > incK):
if not initialized: # Do a full optimize for first minK ranges
print(f"Initializing at time {k}")
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
newFactors, initial)
initial = batchOptimizer.optimize()
initialized = True
isam.update(newFactors, initial)
result = isam.calculateEstimate()
lastPose = result.atPose2(i)
newFactors = gtsam.NonlinearFactorGraph()
initial = gtsam.Values()
countK = 0
i += 1
# Perform a final optimization to take into account all the measurements (especially the last ones)
finalResult = isam.calculateEstimate()
Adding landmark L1
Adding landmark L6
Adding landmark L0
Adding landmark L5
Initializing at time 151
2.5.5. Visualization of the SLAM results¶
We now visualize the landmarks and trajectory inferred by ISAM.
# Print optimized landmarks:
for j in [0,1,5,6]:
landmark_key = gtsam.symbol('L', j)
p = finalResult.atPoint2(landmark_key)
print(f"{landmark_key}: {p}")
5476377146882523136: [-35.97427664 26.3162765 ]
5476377146882523137: [-75.10179088 21.01401519]
5476377146882523141: [ -1.03850677 -12.13769491]
5476377146882523142: [-36.08617621 72.34978651]
We extract the inferred landmarks and the trajectory from the ISAM results.
# Plot positions
poses = gtsam.utilities.allPose2s(finalResult)
landmarks = gtsam.utilities.extractPoint2(finalResult)
positions = np.array([poses.atPose2(key).translation()
for key in poses.keys()])
print(positions.shape)
(4090, 2)
We create a figure showing all the estimated positions along with the recovered landmarks. One may observe that the SLAM results are quite conform to the ground truth (as depicted in “Navigating with Ranging Radios: Five Data Sets with Ground Truth”, by Joseph Djugash, Bradley Hamner, and Stephan Roth).
fig = px.scatter(x=positions[:,0],y=positions[:,1])
fig.add_scatter(x=landmarks[:,0], y=landmarks[:,1], mode="markers", showlegend= False)
fig.update_layout(margin=dict(l=0, r=0, t=0, b=0))
fig.update_yaxes(scaleanchor = "x", scaleratio = 1)
fig.show()
landmarks
array([[-35.97427664, 26.3162765 ],
[-75.10179088, 21.01401519],
[ -1.03850677, -12.13769491],
[-36.08617621, 72.34978651]])