Switched back to gtsam. pattern, added pylint exceptions
Frank Dellaert
parent
f75c2f8427
commit
025450dcc5
|
|
@ -1,26 +1,40 @@
|
|||
#!/usr/bin/env python
|
||||
"""
|
||||
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 robot motion example, with prior and two odometry measurements
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
# Create noise models
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
|
||||
# Create an empty nonlinear factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add a prior on the first pose, setting it to the origin
|
||||
# A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
|
||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
|
||||
|
||||
# Add odometry factors
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
# 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.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
|
||||
graph.print_("\nFactor Graph:\n")
|
||||
|
||||
# Create the data structure to hold the initialEstimate estimate to the solution
|
||||
|
|
|
|||
|
|
@ -9,54 +9,56 @@ 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 numpy as np
|
||||
|
||||
from gtsam import (BearingRangeFactor2D, BetweenFactorPose2,
|
||||
LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
|
||||
Marginals, NonlinearFactorGraph, Point2, Pose2,
|
||||
PriorFactorPose2, Rot2, Values, noiseModel_Diagonal, symbol)
|
||||
import gtsam
|
||||
|
||||
# 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]))
|
||||
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]))
|
||||
|
||||
# Create an empty nonlinear factor graph
|
||||
graph = NonlinearFactorGraph()
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# 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)
|
||||
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)
|
||||
|
||||
# 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))
|
||||
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(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))
|
||||
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(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))
|
||||
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
|
||||
graph.print_("Factor Graph:\n")
|
||||
|
||||
# Create (deliberately inaccurate) initial estimate
|
||||
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))
|
||||
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
|
||||
initial_estimate.print_("Initial Estimate:\n")
|
||||
|
|
@ -67,12 +69,12 @@ initial_estimate.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.
|
||||
params = LevenbergMarquardtParams()
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
optimizer = gtsam.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)
|
||||
for (key, str) in [(X1,"X1"),(X2,"X2"),(X3,"X3"),(L1,"L1"),(L2,"L2")]:
|
||||
print("{} covariance:\n{}\n".format(str,marginals.marginalCovariance(key)))
|
||||
marginals = gtsam.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)))
|
||||
|
|
|
|||
|
|
@ -1,3 +1,16 @@
|
|||
"""
|
||||
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)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
|
|
@ -7,36 +20,38 @@ import numpy as np
|
|||
import gtsam
|
||||
|
||||
|
||||
def Vector3(x, y, z): return np.array([x, y, z])
|
||||
def Vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
# Create noise models
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1))
|
||||
|
||||
# 1. Create a factor graph container and add factors to it
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# 2a. Add a prior on the first pose, setting it to the origin
|
||||
# A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1))
|
||||
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise))
|
||||
|
||||
# For simplicity, we will use the same noise model for odometry and loop closures
|
||||
model = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1))
|
||||
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
|
||||
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
|
||||
|
||||
# 2b. Add odometry factors
|
||||
# Create odometry (Between) factors between consecutive poses
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model))
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
2, 3, gtsam.Pose2(2, 0, math.pi / 2), model))
|
||||
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
3, 4, gtsam.Pose2(2, 0, math.pi / 2), model))
|
||||
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
4, 5, gtsam.Pose2(2, 0, math.pi / 2), model))
|
||||
4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
|
||||
# 2c. Add the loop closure constraint
|
||||
# This factor encodes the fact that we have returned to the same pose. In real
|
||||
# systems, these constraints may be identified in many ways, such as appearance-based
|
||||
# techniques with camera images. We will use another Between Factor to enforce this constraint:
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
5, 2, gtsam.Pose2(2, 0, math.pi / 2), model))
|
||||
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
graph.print_("\nFactor Graph:\n") # print
|
||||
|
||||
# 3. Create the data structure to hold the initial_estimate estimate to the
|
||||
|
|
@ -68,8 +83,5 @@ result.print_("Final Result:\n")
|
|||
|
||||
# 5. Calculate and print marginal covariances for all variables
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
print("x1 covariance:\n", marginals.marginalCovariance(1))
|
||||
print("x2 covariance:\n", marginals.marginalCovariance(2))
|
||||
print("x3 covariance:\n", marginals.marginalCovariance(3))
|
||||
print("x4 covariance:\n", marginals.marginalCovariance(4))
|
||||
print("x5 covariance:\n", marginals.marginalCovariance(5))
|
||||
for i in range(1, 6):
|
||||
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ A structure-from-motion example with landmarks
|
|||
- The landmarks form a 10 meter cube
|
||||
- The robot rotates around the landmarks, always facing towards the cube
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,15 @@
|
|||
"""An example of running visual SLAM using iSAM2."""
|
||||
# pylint: disable=invalid-name
|
||||
"""
|
||||
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
|
||||
|
||||
An example of running visual SLAM using iSAM2.
|
||||
Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue