Cleaned up, corrected bearings
Frank Dellaert
parent
650b65f1fb
commit
f75c2f8427
|
|
@ -1,84 +1,65 @@
|
|||
import gtsam
|
||||
"""
|
||||
GTSAM Copyright 2010, 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)
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
from gtsam import (BearingRangeFactor2D, BetweenFactorPose2,
|
||||
LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
|
||||
Marginals, NonlinearFactorGraph, Point2, Pose2,
|
||||
PriorFactorPose2, Rot2, Values, noiseModel_Diagonal, symbol)
|
||||
|
||||
# Create noise models
|
||||
PRIOR_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
ODOMETRY_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
MEASUREMENT_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
|
||||
|
||||
# Create an empty nonlinear factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Create the keys we need for graph
|
||||
# static Symbol x1('x',1), x2('x',2), x3('x',3);
|
||||
# static Symbol l1('l',1), l2('l',2);
|
||||
x1 = gtsam.symbol(ord('x'), 1)
|
||||
x2 = gtsam.symbol(ord('x'), 2)
|
||||
x3 = gtsam.symbol(ord('x'), 3)
|
||||
l1 = gtsam.symbol(ord('l'), 4)
|
||||
l2 = gtsam.symbol(ord('l'), 5)
|
||||
# Create the keys corresponding to unknown variables in the factor graph
|
||||
X1 = symbol(ord('x'), 1)
|
||||
X2 = symbol(ord('x'), 2)
|
||||
X3 = symbol(ord('x'), 3)
|
||||
L1 = symbol(ord('l'), 4)
|
||||
L2 = symbol(ord('l'), 5)
|
||||
|
||||
# Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
# Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
# noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
|
||||
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
# graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
|
||||
graph.add(gtsam.PriorFactorPose2(x1, priorMean, priorNoise))
|
||||
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
|
||||
graph.add(PriorFactorPose2(X1, Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
|
||||
|
||||
# Add odometry factors between X1,X2 and X2,X3, respectively
|
||||
graph.add(BetweenFactorPose2(X1, X2, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
||||
graph.add(BetweenFactorPose2(X2, X3, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
||||
|
||||
# Add two odometry factors between x1,x2 and x2,x3
|
||||
# Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
# noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
|
||||
# For simplicity, we will use the same noise model for each odometry factor
|
||||
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
# Create odometry (Between) factors between consecutive poses
|
||||
# graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
|
||||
graph.add(gtsam.BetweenFactorPose2(x1, x2, odometry, odometryNoise))
|
||||
# graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
|
||||
graph.add(gtsam.BetweenFactorPose2(x2, x3, odometry, odometryNoise))
|
||||
|
||||
# Add Range-Bearing measurements to two different landmarks
|
||||
# create a noise model for the landmark measurements
|
||||
# noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
|
||||
measurementNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
|
||||
# Rot2 bearing11 = Rot2::fromDegrees(45),
|
||||
# bearing21 = Rot2::fromDegrees(90),
|
||||
# bearing32 = Rot2::fromDegrees(90);
|
||||
# double range11 = std::sqrt(4.0+4.0),
|
||||
# range21 = 2.0,
|
||||
# range32 = 2.0;
|
||||
bearing11 = gtsam.Rot2.fromDegrees(np.pi/4)
|
||||
bearing21 = gtsam.Rot2.fromDegrees(np.pi/2)
|
||||
bearing32 = gtsam.Rot2.fromDegrees(np.pi/2)
|
||||
range11 = np.sqrt(4.0+4.0)
|
||||
range21 = 2.0
|
||||
range32 = 2.0
|
||||
|
||||
# Add Bearing-Range factors
|
||||
# graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
|
||||
# graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
|
||||
# graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
|
||||
graph.add(gtsam.BearingRangeFactor2D(x1,l1,bearing11,range11,measurementNoise))
|
||||
graph.add(gtsam.BearingRangeFactor2D(x2,l1,bearing21,range21,measurementNoise))
|
||||
graph.add(gtsam.BearingRangeFactor2D(x3,l2,bearing32,range32,measurementNoise))
|
||||
# Add Range-Bearing measurements to two different landmarks L1 and L2
|
||||
graph.add(BearingRangeFactor2D(
|
||||
X1, L1, Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
|
||||
graph.add(BearingRangeFactor2D(
|
||||
X2, L1, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
||||
graph.add(BearingRangeFactor2D(
|
||||
X3, L2, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
||||
|
||||
# Print graph
|
||||
graph.print_("Factor Graph:\n");
|
||||
graph.print_("Factor Graph:\n")
|
||||
|
||||
# Create (deliberately inaccurate) initial estimate
|
||||
# Values initialEstimate;
|
||||
# initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
|
||||
# initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
|
||||
# initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
|
||||
# initialEstimate.insert(l1, Point2(1.8, 2.1));
|
||||
# initialEstimate.insert(l2, Point2(4.1, 1.8));
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(x1, gtsam.Pose2(-0.25, 0.20, 0.15))
|
||||
initialEstimate.insert(x2, gtsam.Pose2( 2.30, 0.10, -0.20))
|
||||
initialEstimate.insert(x3, gtsam.Pose2( 4.10, 0.10, 0.10))
|
||||
initialEstimate.insert(l1, gtsam.Point2( 1.80, 2.10))
|
||||
initialEstimate.insert(l2, gtsam.Point2( 4.10, 1.80))
|
||||
initial_estimate = Values()
|
||||
initial_estimate.insert(X1, Pose2(-0.25, 0.20, 0.15))
|
||||
initial_estimate.insert(X2, Pose2(2.30, 0.10, -0.20))
|
||||
initial_estimate.insert(X3, Pose2(4.10, 0.10, 0.10))
|
||||
initial_estimate.insert(L1, Point2(1.80, 2.10))
|
||||
initial_estimate.insert(L2, Point2(4.10, 1.80))
|
||||
|
||||
# Print
|
||||
initialEstimate.print_("Initial Estimate:\n");
|
||||
initial_estimate.print_("Initial Estimate:\n")
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization. The optimizer
|
||||
# accepts an optional set of configuration parameters, controlling
|
||||
|
|
@ -86,24 +67,12 @@ initialEstimate.print_("Initial Estimate:\n");
|
|||
# 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.
|
||||
# LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
|
||||
# Values result = optimizer.optimize();
|
||||
# result.print("Final Result:\n");
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate, params)
|
||||
params = LevenbergMarquardtParams()
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
|
||||
result = optimizer.optimize()
|
||||
result.print_("\nFinal Result:\n")
|
||||
|
||||
# Calculate and print marginal covariances for all variables
|
||||
# Marginals marginals(graph, result);
|
||||
# print(marginals.marginalCovariance(x1), "x1 covariance");
|
||||
# print(marginals.marginalCovariance(x2), "x2 covariance");
|
||||
# print(marginals.marginalCovariance(x3), "x3 covariance");
|
||||
# print(marginals.marginalCovariance(l1), "l1 covariance");
|
||||
# print(marginals.marginalCovariance(l2), "l2 covariance");
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
print("x1 covariance:\n", marginals.marginalCovariance(x1))
|
||||
print("x2 covariance:\n", marginals.marginalCovariance(x2))
|
||||
print("x3 covariance:\n", marginals.marginalCovariance(x3))
|
||||
print("x4 covariance:\n", marginals.marginalCovariance(l1))
|
||||
print("x5 covariance:\n", marginals.marginalCovariance(l2))
|
||||
marginals = Marginals(graph, result)
|
||||
for (key, str) in [(X1,"X1"),(X2,"X2"),(X3,"X3"),(L1,"L1"),(L2,"L2")]:
|
||||
print("{} covariance:\n{}\n".format(str,marginals.marginalCovariance(key)))
|
||||
|
|
|
|||
Loading…
Reference in New Issue