98 lines
3.6 KiB
Python
98 lines
3.6 KiB
Python
"""
|
|
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
|
Atlanta, Georgia 30332-0415
|
|
All Rights Reserved
|
|
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
See LICENSE for the license information
|
|
|
|
Simple robotics example using odometry measurements and bearing-range (laser) measurements
|
|
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
|
"""
|
|
# pylint: disable=invalid-name, E1101
|
|
|
|
from __future__ import print_function
|
|
|
|
import gtsam
|
|
import numpy as np
|
|
from gtsam.symbol_shorthand import L, X
|
|
|
|
# Create noise models
|
|
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
|
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
|
MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
|
|
|
|
|
|
def main():
|
|
"""Main runner"""
|
|
|
|
# Create an empty nonlinear factor graph
|
|
graph = gtsam.NonlinearFactorGraph()
|
|
|
|
# Create the keys corresponding to unknown variables in the factor graph
|
|
X1 = X(1)
|
|
X2 = X(2)
|
|
X3 = X(3)
|
|
L1 = L(4)
|
|
L2 = L(5)
|
|
|
|
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
|
|
graph.add(
|
|
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
|
|
|
|
# Add odometry factors between X1,X2 and X2,X3, respectively
|
|
graph.add(
|
|
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
|
|
ODOMETRY_NOISE))
|
|
graph.add(
|
|
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
|
|
ODOMETRY_NOISE))
|
|
|
|
# Add Range-Bearing measurements to two different landmarks L1 and L2
|
|
graph.add(
|
|
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
|
|
np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
|
|
graph.add(
|
|
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
|
|
MEASUREMENT_NOISE))
|
|
graph.add(
|
|
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
|
|
MEASUREMENT_NOISE))
|
|
|
|
# Print graph
|
|
print("Factor Graph:\n{}".format(graph))
|
|
|
|
# Create (deliberately inaccurate) initial estimate
|
|
initial_estimate = gtsam.Values()
|
|
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
|
|
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
|
|
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
|
|
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
|
|
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
|
|
|
|
# Print
|
|
print("Initial Estimate:\n{}".format(initial_estimate))
|
|
|
|
# Optimize using Levenberg-Marquardt optimization. The optimizer
|
|
# accepts an optional set of configuration parameters, controlling
|
|
# things like convergence criteria, the type of linear system solver
|
|
# to use, and the amount of information displayed during optimization.
|
|
# Here we will use the default set of parameters. See the
|
|
# documentation for the full set of parameters.
|
|
params = gtsam.LevenbergMarquardtParams()
|
|
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
|
|
params)
|
|
result = optimizer.optimize()
|
|
print("\nFinal Result:\n{}".format(result))
|
|
|
|
# Calculate and print marginal covariances for all variables
|
|
marginals = gtsam.Marginals(graph, result)
|
|
for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
|
|
(L2, "L2")]:
|
|
print("{} covariance:\n{}\n".format(s,
|
|
marginals.marginalCovariance(key)))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|