12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updated examples and tests with new symbol shorthands

release/4.3a0
Varun Agrawal 2020-06-10 15:44:49 -05:00
parent 8008d69dc6
commit ad08920880
9 changed files with 49 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cython/gtsam/examples/ImuFactorExample.py
View File

@ -12,25 +12,18 @@ from __future__ import print_function
import math
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_B as B
from gtsam import symbol_shorthand_V as V
from gtsam import symbol_shorthand_X as X
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam.utils.plot import plot_pose3
from PreintegrationExample import POSES_FIG, PreintegrationExample
BIAS_KEY = int(gtsam.symbol(ord('b'), 0))
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol(ord('x'), key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol(ord('v'), key)
BIAS_KEY = B(0)
np.set_printoptions(precision=3, suppress=True)

22
cython/gtsam/examples/ImuFactorExample2.py
View File

@ -8,27 +8,19 @@ from __future__ import print_function
import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
import numpy as np
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
PinholeCameraCal3_S2, Point3, Pose3,
PriorFactorConstantBias, PriorFactorPose3,
PriorFactorVector, Rot3, Values)
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol(ord('x'), key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol(ord('v'), key)
from gtsam import symbol_shorthand_B as B
from gtsam import symbol_shorthand_V as V
from gtsam import symbol_shorthand_X as X
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def vector3(x, y, z):
@ -115,7 +107,7 @@ def IMU_example():
newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasKey = B(0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)

15
cython/gtsam/examples/PlanarSLAMExample.py
View File

@ -13,9 +13,10 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
from __future__ import print_function
import numpy as np
import gtsam
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
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)
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))

2
cython/gtsam/examples/README.md
View File

@ -1,7 +1,7 @@
These examples are almost identical to the old handwritten python wrapper
examples. However, there are just some slight name changes, for example
`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))`
Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing.
# Porting Progress

17
cython/gtsam/examples/SFMExample.py
View File

@ -13,6 +13,8 @@ from __future__ import print_function
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
@ -22,11 +24,6 @@ from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
from gtsam.utils import plot
def symbol(name: str, index: int) -> int:
""" helper for creating a symbol without explicitly casting 'name' from str to int """
return gtsam.symbol(ord(name), index)
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
@ -73,7 +70,7 @@ def main():
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
@ -82,14 +79,14 @@ def main():
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, measurement_noise, symbol('x', i), symbol('l', j), K)
measurement, measurement_noise, X(i), L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
@ -98,10 +95,10 @@ def main():
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
initial_estimate.insert(symbol('x', i), transformed_pose)
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
initial_estimate.insert(symbol('l', j), transformed_point)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results

5
cython/gtsam/examples/SimpleRotation.py
View File

@ -10,8 +10,9 @@ This example will perform a relatively trivial optimization on
a single variable with a single factor.
"""
import numpy as np
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
def main():
@ -33,7 +34,7 @@ def main():
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
prior.print_('goal angle')
model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = gtsam.symbol(ord('x'), 1)
key = X(1)
factor = gtsam.PriorFactorRot2(key, prior, model)
"""

19
cython/gtsam/examples/VisualISAM2Example.py
View File

@ -13,23 +13,14 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
from __future__ import print_function
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
from gtsam.examples import SFMdata
def X(i):
"""Create key for pose i."""
return int(gtsam.symbol(ord('x'), i))
def L(j):
"""Create key for landmark j."""
return int(gtsam.symbol(ord('l'), j))
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def visual_ISAM2_plot(result):

17
cython/gtsam/examples/VisualISAMExample.py
View File

@ -19,11 +19,8 @@ from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values)
def symbol(name: str, index: int) -> int:
""" helper for creating a symbol without explicitly casting 'name' from str to int """
return gtsam.symbol(ord(name), index)
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
def main():
@ -58,7 +55,7 @@ def main():
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
@ -66,7 +63,7 @@ def main():
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(symbol('x', i), initial_xi)
initial_estimate.insert(X(i), initial_xi)
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
@ -75,12 +72,12 @@ def main():
if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
@ -88,7 +85,7 @@ def main():
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j].vector() + noise
initial_estimate.insert(symbol('l', j), Point3(initial_lj))
initial_estimate.insert(L(j), Point3(initial_lj))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)

9
cython/gtsam/tests/test_GaussianFactorGraph.py
View File

@ -15,17 +15,18 @@ from __future__ import print_function
import unittest
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
from gtsam.utils.test_case import GtsamTestCase
import numpy as np
def create_graph():
"""Create a basic linear factor graph for testing"""
graph = gtsam.GaussianFactorGraph()
x0 = gtsam.symbol(ord('x'), 0)
x1 = gtsam.symbol(ord('x'), 1)
x2 = gtsam.symbol(ord('x'), 2)
x0 = X(0)
x1 = X(1)
x2 = X(2)
BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))