Range SLAM with iSAM

# pylint: disable=invalid-name, E1101

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

# load the odometry
# DR: Odometry Input (delta distance traveled and delta heading change)
#    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.")

# load the ranges from TD
#    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.")

# parameters
minK = 150  # minimum number of range measurements to process initially
incK = 25  # minimum number of range measurements to process after
robust = True

# Set Noise parameters
priorSigmas = gtsam.Point3(1, 1, math.pi)
odoSigmas = gtsam.Point3(0.05, 0.01, 0.1)
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

# Initialize iSAM
isam = gtsam.ISAM2()
print(isam)

# Add prior on 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)

# 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 odometry factor
    newFactors.add(gtsam.BetweenFactorPose2(i - 1, i, relative_pose,
                                            odoNoise))

    # predict pose and add as initial estimate
    predictedPose = lastPose.compose(relative_pose)
    lastPose = predictedPose
    initial.insert(i, predictedPose)

    # Check if there are range factors to be added
    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))
        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

finalResult = isam.calculateEstimate()

# 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}")

# 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)

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