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

Replace with new python tests

release/4.3a0
Fan Jiang 2020-07-27 09:32:31 -04:00
parent 7873c36088
commit 9216934ca8
50 changed files with 408 additions and 520 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
python/gtsam_py/python/__init__.py
View File

@ -1,26 +1,9 @@
from .gtsam import *
try:
import gtsam_unstable
def _deprecated_wrapper(item, name):
def wrapper(*args, **kwargs):
from warnings import warn
message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
'Please import it from gtsam_unstable.')
warn(message)
return item(*args, **kwargs)
return wrapper
for name in dir(gtsam_unstable):
if not name.startswith('__'):
item = getattr(gtsam_unstable, name)
if callable(item):
item = _deprecated_wrapper(item, name)
globals()[name] = item
except ImportError:
pass
def Point2(x=0, y=0):
import numpy as np
return np.array([x, y], dtype=float)
def Point3(x=0, y=0, z=0):
import numpy as np
return np.array([x, y, z], dtype=float)

6
python/gtsam_py/python/examples/DogLegOptimizerExample.py
View File

@ -35,17 +35,17 @@ def run(args):
graph = gtsam.NonlinearFactorGraph()
# Priors
prior = gtsam.noiseModel_Isotropic.Sigma(3, 1)
prior = gtsam.noiseModel.Isotropic.Sigma(3, 1)
graph.add(gtsam.PriorFactorPose2(11, T11, prior))
graph.add(gtsam.PriorFactorPose2(21, T21, prior))
# Odometry
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
# Range
model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01)
model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)
graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
params = gtsam.DoglegParams()

4
python/gtsam_py/python/examples/GPSFactorExample.py
View File

@ -26,8 +26,8 @@ lon0 = -84.30626
h0 = 274
# Create noise models
GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25)
GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25)
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()

50
python/gtsam_py/python/examples/ImuFactorExample.py
View File

@ -5,27 +5,32 @@ All Rights Reserved
See LICENSE for the license information
A script validating and demonstrating the ImuFactor inference.
Author: Frank Dellaert, Varun Agrawal
A script validating the ImuFactor inference.
"""
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 = B(0)
BIAS_KEY = int(gtsam.symbol('b', 0))
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol('x', key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol('v', key)
np.set_printoptions(precision=3, suppress=True)
@ -35,8 +40,8 @@ class ImuFactorExample(PreintegrationExample):
def __init__(self):
self.velocity = np.array([2, 0, 0])
self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
# Choose one of these twists to change scenario:
zero_twist = (np.zeros(3), np.zeros(3))
@ -47,7 +52,7 @@ class ImuFactorExample(PreintegrationExample):
accBias = np.array([-0.3, 0.1, 0.2])
gyroBias = np.array([0.1, 0.3, -0.1])
bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
dt = 1e-2
super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
@ -71,14 +76,8 @@ class ImuFactorExample(PreintegrationExample):
initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
pose = state_i.pose().compose(poseNoise)
velocity = state_i.velocity() + np.random.randn(3)*0.1
initial.insert(X(i), pose)
initial.insert(V(i), velocity)
initial.insert(X(i), state_i.pose())
initial.insert(V(i), state_i.velocity())
# simulate the loop
i = 0 # state index
@ -89,12 +88,6 @@ class ImuFactorExample(PreintegrationExample):
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
actual_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
@ -140,10 +133,7 @@ class ImuFactorExample(PreintegrationExample):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print(result.atimuBias_ConstantBias(BIAS_KEY))
print(result.atimuBias.ConstantBias(BIAS_KEY))
plt.ioff()
plt.show()

77
python/gtsam_py/python/examples/ImuFactorExample2.py
View File

@ -8,20 +8,23 @@ from __future__ import print_function
import math
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)
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
import gtsam
import gtsam.utils.plot as gtsam_plot
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol('x', key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol('v', key)
def vector3(x, y, z):
"""Create 3d double numpy array."""
@ -40,7 +43,7 @@ def ISAM2_plot(values, fignum=0):
while values.exists(X(i)):
pose_i = values.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
position = pose_i.translation().vector()
position = pose_i.translation()
min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
# max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
@ -65,20 +68,21 @@ PARAMS.setIntegrationCovariance(I * 0.1)
PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
Point3(0.05, -0.10, 0.20))
BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20))
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
ZERO_BIAS = gtsam.imuBias.ConstantBias()
# Start with a camera on x-axis looking at origin
radius = 30
up = Point3(0, 0, 1)
target = Point3(0, 0, 0)
position = Point3(radius, 0, 0)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
@ -87,37 +91,37 @@ def IMU_example():
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = ConstantTwistScenario(
scenario = gtsam.ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = NonlinearFactorGraph()
newgraph = gtsam.NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = ISAM2()
isam = gtsam.ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = Values()
initialEstimate = gtsam.Values()
# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel_Diagonal.Sigmas(
noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = B(0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)
biasKey = gtsam.symbol('b', 0)
biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, ZERO_BIAS,
biasnoise)
newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
initialEstimate.insert(biasKey, ZERO_BIAS)
velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
@ -138,10 +142,10 @@ def IMU_example():
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
factor = gtsam.BetweenFactorConstantBias(
biasKey - 1, biasKey, ZERO_BIAS, BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
initialEstimate.insert(biasKey, ZERO_BIAS)
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
@ -151,7 +155,8 @@ def IMU_example():
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
@ -164,7 +169,7 @@ def IMU_example():
ISAM2_plot(result)
# reset
newgraph = NonlinearFactorGraph()
newgraph = gtsam.NonlinearFactorGraph()
initialEstimate.clear()

4
python/gtsam_py/python/examples/OdometryExample.py
View File

@ -21,8 +21,8 @@ import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot
# 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]))
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()

6
python/gtsam_py/python/examples/PlanarManipulatorExample.py
View File

@ -167,13 +167,11 @@ class ThreeLinkArm(object):
axes = fig.gca()
sXl1 = Pose2(0, 0, math.radians(90))
t = sXl1.translation()
p1 = np.array([t.x(), t.y()])
p1 = sXl1.translation()
gtsam_plot.plot_pose2_on_axes(axes, sXl1)
def plot_line(p, g, color):
t = g.translation()
q = np.array([t.x(), t.y()])
q = g.translation()
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], color)
return q

21
python/gtsam_py/python/examples/PlanarSLAMExample.py
View File

@ -13,25 +13,24 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
from __future__ import print_function
import gtsam
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
import gtsam
# 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]))
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 = 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)
X1 = gtsam.symbol('x', 1)
X2 = gtsam.symbol('x', 2)
X3 = gtsam.symbol('x', 3)
L1 = gtsam.symbol('l', 4)
L2 = gtsam.symbol('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))

4
python/gtsam_py/python/examples/Pose2SLAMExample.py
View File

@ -29,8 +29,8 @@ def vector3(x, y, z):
# 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))
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()

9
python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py
View File

@ -53,15 +53,16 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
assert args.kernel == "none", "Supplied kernel type is not yet implemented"
# Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
graphWithPrior = graph
priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination")
params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
# ... and optimize
result = optimizer.optimize()
@ -82,7 +83,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities_extractPose2(result)
resultPoses = gtsam.extractPose2(result)
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()

13
python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py
View File

@ -39,21 +39,22 @@ is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add Prior on the first key
priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
@ -65,7 +66,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities_allPose3s(result)
resultPoses = gtsam.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()

2
python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py
View File

@ -24,7 +24,7 @@ is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add prior on the first key. TODO: assumes first key ios z
priorModel = gtsam.noiseModel_Diagonal.Variances(
priorModel = gtsam.noiseModel.Diagonal.Variances(
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
firstKey = initial.keys().at(0)
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))

4
python/gtsam_py/python/examples/PreintegrationExample.py
View File

@ -68,7 +68,7 @@ class PreintegrationExample(object):
else:
accBias = np.array([0, 0.1, 0])
gyroBias = np.array([0, 0, 0])
self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
self.runner = gtsam.ScenarioRunner(
self.scenario, self.params, self.dt, self.actualBias)
@ -111,7 +111,7 @@ class PreintegrationExample(object):
actualPose = self.scenario.pose(t)
plot_pose3(POSES_FIG, actualPose, 0.3)
t = actualPose.translation()
self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
self.maxDim = max([max(t), self.maxDim])
ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)

9
python/gtsam_py/python/examples/README.md
View File

@ -1,7 +1,8 @@
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, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing.
# THIS FILE IS OUTDATED!
~~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('b', 0))`~~
# Porting Progress

33
python/gtsam_py/python/examples/SFMExample.py
View File

@ -10,20 +10,24 @@ A structure-from-motion problem on a simulated dataset
"""
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
import gtsam
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
NonlinearFactorGraph, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values)
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(name, index)
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
@ -56,7 +60,7 @@ def main():
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
@ -69,24 +73,24 @@ 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(X(0), poses[0], pose_noise)
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)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
camera = SimpleCamera(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, measurement_noise, X(i), L(j), K)
measurement, measurement_noise, symbol('x', i), symbol('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(L(0), points[0], point_noise)
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
@ -95,10 +99,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(X(i), transformed_pose)
initial_estimate.insert(symbol('x', i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
initial_estimate.insert(L(j), transformed_point)
initial_estimate.insert(symbol('l', j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
@ -117,5 +121,6 @@ def main():
plot.set_axes_equal(1)
plt.show()
if __name__ == '__main__':
main()

3
python/gtsam_py/python/examples/SFMdata.py
View File

@ -33,7 +33,8 @@ def createPoses(K):
poses = []
for theta in angles:
position = gtsam.Point3(radius*np.cos(theta),
radius*np.sin(theta), height)
radius*np.sin(theta),
height)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses

7
python/gtsam_py/python/examples/SimpleRotation.py
View File

@ -10,9 +10,8 @@ This example will perform a relatively trivial optimization on
a single variable with a single factor.
"""
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
import gtsam
def main():
@ -33,8 +32,8 @@ 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 = X(1)
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = gtsam.symbol('x', 1)
factor = gtsam.PriorFactorRot2(key, prior, model)
"""

27
python/gtsam_py/python/examples/VisualISAM2Example.py
View File

@ -13,15 +13,24 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
from __future__ import print_function
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
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
from gtsam.examples import SFMdata
def X(i):
"""Create key for pose i."""
return int(gtsam.symbol('x', i))
def L(j):
"""Create key for landmark j."""
return int(gtsam.symbol('l', j))
def visual_ISAM2_plot(result):
"""
@ -64,7 +73,7 @@ def visual_ISAM2_example():
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
@ -110,12 +119,12 @@ def visual_ISAM2_example():
# at least twice before adding it to iSAM.
if i == 0:
# Add a prior on pose x0
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array(
[0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
graph.push_back(gtsam.PriorFactorPoint3(
L(0), points[0], point_noise)) # add directly to graph
@ -123,7 +132,7 @@ def visual_ISAM2_example():
# Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points):
initial_estimate.insert(L(j), gtsam.Point3(
point.x()-0.25, point.y()+0.20, point.z()+0.15))
point[0]-0.25, point[1]+0.20, point[2]+0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)

33
python/gtsam_py/python/examples/VisualISAMExample.py
View File

@ -15,12 +15,15 @@ from __future__ import print_function
import numpy as np
import gtsam
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values)
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
SimpleCamera, Values, Point3)
def symbol(name: str, index: int) -> int:
""" helper for creating a symbol without explicitly casting 'name' from str to int """
return gtsam.symbol(name, index)
def main():
@ -34,7 +37,7 @@ def main():
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
camera_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v
camera_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
@ -51,11 +54,11 @@ def main():
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
camera = SimpleCamera(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
@ -63,7 +66,7 @@ def main():
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(X(i), initial_xi)
initial_estimate.insert(symbol('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
@ -71,21 +74,21 @@ def main():
# adding it to iSAM.
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(X(0), poses[0], pose_noise)
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)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j].vector() + noise
initial_estimate.insert(L(j), Point3(initial_lj))
initial_lj = points[j] + noise
initial_estimate.insert(symbol('l', j), initial_lj)
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)

0
python/gtsam_py/python/tests/__init__.py
View File

2
python/gtsam_py/python/tests/testScenarioRunner.py
View File

@ -32,7 +32,7 @@ class TestScenarioRunner(GtsamTestCase):
dt = 0.1
params = gtsam.PreintegrationParams.MakeSharedU(self.g)
bias = gtsam.imuBias_ConstantBias()
bias = gtsam.imuBias.ConstantBias()
runner = gtsam.ScenarioRunner(
scenario, params, dt, bias)

13
python/gtsam_py/python/tests/test_GaussianFactorGraph.py
View File

@ -15,21 +15,20 @@ 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 = X(0)
x1 = X(1)
x2 = X(2)
x0 = gtsam.symbol('x', 0)
x1 = gtsam.symbol('x', 1)
x2 = gtsam.symbol('x', 2)
BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)

6
python/gtsam_py/python/tests/test_JacobianFactor.py
View File

@ -48,7 +48,7 @@ class TestJacobianFactor(GtsamTestCase):
# the RHS
b2 = np.array([-1., 1.5, 2., -1.])
sigmas = np.array([1., 1., 1., 1.])
model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas)
model4 = gtsam.noiseModel.Diagonal.Sigmas(sigmas)
combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
# eliminate the first variable (x2) in the combined factor, destructive
@ -66,7 +66,7 @@ class TestJacobianFactor(GtsamTestCase):
[+0.00, -8.94427]])
d = np.array([2.23607, -1.56525])
expectedCG = gtsam.GaussianConditional(
x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel.Unit.Create(2))
# check if the result matches
self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
@ -82,7 +82,7 @@ class TestJacobianFactor(GtsamTestCase):
# the RHS
b1 = np.array([0.0, 0.894427])
model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
model2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([1., 1.]))
expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
# check if the result matches the combined (reduced) factor

4
python/gtsam_py/python/tests/test_KalmanFilter.py
View File

@ -22,13 +22,13 @@ class TestKalmanFilter(GtsamTestCase):
F = np.eye(2)
B = np.eye(2)
u = np.array([1.0, 0.0])
modelQ = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
modelQ = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
Q = 0.01 * np.eye(2)
H = np.eye(2)
z1 = np.array([1.0, 0.0])
z2 = np.array([2.0, 0.0])
z3 = np.array([3.0, 0.0])
modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
modelR = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
R = 0.01 * np.eye(2)
# Create the set of expected output TestValues

6
python/gtsam_py/python/tests/test_KarcherMeanFactor.py
View File

@ -18,7 +18,7 @@ import numpy as np
from gtsam.utils.test_case import GtsamTestCase
KEY = 0
MODEL = gtsam.noiseModel_Unit.Create(3)
MODEL = gtsam.noiseModel.Unit.Create(3)
def find_Karcher_mean_Rot3(rotations):
@ -59,8 +59,8 @@ class TestKarcherMean(GtsamTestCase):
R12 = R.compose(R.compose(R))
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
keys = gtsam.KeyVector()
keys.push_back(1)
keys.push_back(2)
keys.append(1)
keys.append(2)
graph.add(gtsam.KarcherMeanFactorRot3(keys))
initial = gtsam.Values()

4
python/gtsam_py/python/tests/test_LocalizationExample.py
View File

@ -26,7 +26,7 @@ class TestLocalizationExample(GtsamTestCase):
# Add two odometry factors
# create a measurement for both factors (the same in this case)
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta
graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
@ -37,7 +37,7 @@ class TestLocalizationExample(GtsamTestCase):
groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))
groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0))
groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0))
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
for i in range(3):
graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model))

15
python/gtsam_py/python/tests/test_NonlinearOptimizer.py
View File

@ -17,8 +17,7 @@ import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
GaussNewtonParams, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, PCGSolverParameters,
DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
Point2, PriorFactorPoint2, Values)
from gtsam.utils.test_case import GtsamTestCase
@ -30,7 +29,7 @@ class TestScenario(GtsamTestCase):
def test_optimize(self):
"""Do trivial test with three optimizer variants."""
fg = NonlinearFactorGraph()
model = gtsam.noiseModel_Unit.Create(2)
model = gtsam.noiseModel.Unit.Create(2)
fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
@ -62,16 +61,6 @@ class TestScenario(GtsamTestCase):
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual2 = LevenbergMarquardtOptimizer(
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)

4
python/gtsam_py/python/tests/test_OdometryExample.py
View File

@ -25,7 +25,7 @@ class TestOdometryExample(GtsamTestCase):
# Add a Gaussian prior on pose x_1
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta
# add directly to graph
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
@ -33,7 +33,7 @@ class TestOdometryExample(GtsamTestCase):
# Add two odometry factors
# create a measurement for both factors (the same in this case)
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))

6
python/gtsam_py/python/tests/test_PlanarSLAMExample.py
View File

@ -32,13 +32,13 @@ class TestPlanarSLAM(GtsamTestCase):
# Add prior
# gaussian for prior
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]))
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
# add directly to graph
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add odometry
# general noisemodel for odometry
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(gtsam.BetweenFactorPose2(
1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
graph.add(gtsam.BetweenFactorPose2(
@ -49,7 +49,7 @@ class TestPlanarSLAM(GtsamTestCase):
4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
# Add pose constraint
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
# Initialize to noisy points

6
python/gtsam_py/python/tests/test_Pose2SLAMExample.py
View File

@ -32,13 +32,13 @@ class TestPose2SLAMExample(GtsamTestCase):
# Add prior
# gaussian for prior
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]))
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
# add directly to graph
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add odometry
# general noisemodel for odometry
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(gtsam.BetweenFactorPose2(
1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
graph.add(gtsam.BetweenFactorPose2(
@ -49,7 +49,7 @@ class TestPose2SLAMExample(GtsamTestCase):
4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
# Add pose constraint
model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
# Initialize to noisy points

2
python/gtsam_py/python/tests/test_Pose3SLAMExample.py
View File

@ -30,7 +30,7 @@ class TestPose3SLAMExample(GtsamTestCase):
fg = gtsam.NonlinearFactorGraph()
fg.add(gtsam.NonlinearEqualityPose3(0, p0))
delta = p0.between(p1)
covariance = gtsam.noiseModel_Diagonal.Sigmas(
covariance = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180]))
fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))

26
python/gtsam_py/python/tests/test_PriorFactor.py
View File

@ -23,39 +23,17 @@ class TestPriorFactor(GtsamTestCase):
key = 5
priorPose3 = gtsam.Pose3()
model = gtsam.noiseModel_Unit.Create(6)
model = gtsam.noiseModel.Unit.Create(6)
factor = gtsam.PriorFactorPose3(key, priorPose3, model)
values.insert(key, priorPose3)
self.assertEqual(factor.error(values), 0)
key = 3
priorVector = np.array([0., 0., 0.])
model = gtsam.noiseModel_Unit.Create(3)
model = gtsam.noiseModel.Unit.Create(3)
factor = gtsam.PriorFactorVector(key, priorVector, model)
values.insert(key, priorVector)
self.assertEqual(factor.error(values), 0)
def test_AddPrior(self):
"""
Test adding prior factors directly to factor graph via the .addPrior method.
"""
# define factor graph
graph = gtsam.NonlinearFactorGraph()
# define and add Pose3 prior
key = 5
priorPose3 = gtsam.Pose3()
model = gtsam.noiseModel_Unit.Create(6)
graph.addPriorPose3(key, priorPose3, model)
self.assertEqual(graph.size(), 1)
# define and add Vector prior
key = 3
priorVector = np.array([0., 0., 0.])
model = gtsam.noiseModel_Unit.Create(3)
graph.addPriorVector(key, priorVector, model)
self.assertEqual(graph.size(), 2)
if __name__ == "__main__":
unittest.main()

24
python/gtsam_py/python/tests/test_SFMExample.py
View File

@ -34,29 +34,29 @@ class TestSFMExample(GtsamTestCase):
graph = gtsam.NonlinearFactorGraph()
# Add factors for all measurements
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma)
measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, measurementNoiseSigma)
for i in range(len(data.Z)):
for k in range(len(data.Z[i])):
j = data.J[i][k]
graph.add(gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], measurementNoise,
symbol(ord('x'), i), symbol(ord('p'), j), data.K))
symbol('x', i), symbol('p', j), data.K))
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0),
posePriorNoise = gtsam.noiseModel.Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(symbol('x', 0),
truth.cameras[0].pose(), posePriorNoise))
pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0),
pointPriorNoise = gtsam.noiseModel.Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(symbol('p', 0),
truth.points[0], pointPriorNoise))
# Initial estimate
initialEstimate = gtsam.Values()
for i in range(len(truth.cameras)):
pose_i = truth.cameras[i].pose()
initialEstimate.insert(symbol(ord('x'), i), pose_i)
initialEstimate.insert(symbol('x', i), pose_i)
for j in range(len(truth.points)):
point_j = truth.points[j]
initialEstimate.insert(symbol(ord('p'), j), point_j)
initialEstimate.insert(symbol('p', j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
@ -66,16 +66,16 @@ class TestSFMExample(GtsamTestCase):
# Marginalization
marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(symbol(ord('p'), 0))
marginals.marginalCovariance(symbol(ord('x'), 0))
marginals.marginalCovariance(symbol('p', 0))
marginals.marginalCovariance(symbol('x', 0))
# Check optimized results, should be equal to ground truth
for i in range(len(truth.cameras)):
pose_i = result.atPose3(symbol(ord('x'), i))
pose_i = result.atPose3(symbol('x', i))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
for j in range(len(truth.points)):
point_j = result.atPoint3(symbol(ord('p'), j))
point_j = result.atPoint3(symbol('p', j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
if __name__ == "__main__":

5
python/gtsam_py/python/tests/test_Scenario.py
View File

@ -43,8 +43,11 @@ class TestScenario(GtsamTestCase):
# R = v/w, so test if loop crests at 2*R
R = v / w
T30 = scenario.pose(T)
xyz = T30.rotation().xyz()
if xyz[0] < 0:
xyz = -xyz
np.testing.assert_almost_equal(
np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
np.array([math.pi, 0, math.pi]), xyz)
self.gtsamAssertEquals(gtsam.Point3(
0, 0, 2.0 * R), T30.translation(), 1e-9)

8
python/gtsam_py/python/tests/test_SimpleCamera.py
View File

@ -5,7 +5,7 @@ All Rights Reserved
See LICENSE for the license information
PinholeCameraCal3_S2 unit tests.
SimpleCamera unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import math
@ -14,7 +14,7 @@ import unittest
import numpy as np
import gtsam
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
from gtsam.utils.test_case import GtsamTestCase
K = Cal3_S2(625, 625, 0, 0, 0)
@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase):
def test_constructor(self):
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
camera = PinholeCameraCal3_S2(pose1, K)
camera = SimpleCamera(pose1, K)
self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
def test_level2(self):
# Create a level camera, looking in Y-direction
pose2 = Pose2(0.4,0.3,math.pi/2.0)
camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1)
camera = SimpleCamera.Level(K, pose2, 0.1)
# expected
x = Point3(1,0,0)

12
python/gtsam_py/python/tests/test_StereoVOExample.py
View File

@ -28,11 +28,11 @@ class TestStereoVOExample(GtsamTestCase):
# - No noise on measurements
## Create keys for variables
x1 = symbol(ord('x'),1)
x2 = symbol(ord('x'),2)
l1 = symbol(ord('l'),1)
l2 = symbol(ord('l'),2)
l3 = symbol(ord('l'),3)
x1 = symbol('x',1)
x2 = symbol('x',2)
l1 = symbol('l',1)
l2 = symbol('l',2)
l3 = symbol('l',3)
## Create graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
@ -44,7 +44,7 @@ class TestStereoVOExample(GtsamTestCase):
## Create realistic calibration and measurement noise model
# format: fx fy skew cx cy baseline
K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
## Add measurements
# pose 1

80
python/gtsam_py/python/tests/test_Triangulation.py Normal file
View File

@ -0,0 +1,80 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Test Triangulation
Author: Frank Dellaert & Fan Jiang (Python)
"""
import unittest
import numpy as np
import gtsam as g
from gtsam.utils.test_case import GtsamTestCase
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3
class TestVisualISAMExample(GtsamTestCase):
def test_TriangulationExample(self):
# Some common constants
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
# Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
pose1 = Pose3(upright, Point3(0, 0, 1))
camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
# create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark)
z2 = camera2.project(landmark)
# twoPoses
poses = Pose3Vector()
measurements = Point2Vector()
poses.append(pose1)
poses.append(pose2)
measurements.append(z1)
measurements.append(z2)
optimize = True
rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
# 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements = Point2Vector()
measurements.append(z1 - np.array([0.1, 0.5]))
measurements.append(z2 - np.array([-0.2, 0.3]))
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2)
#
# # two Poses with Bundler Calibration
# bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480)
# camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal)
# camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal)
#
# z1 = camera1.project(landmark)
# z2 = camera2.project(landmark)
#
# measurements = Point2Vector()
# measurements.append(z1)
# measurements.append(z2)
#
# triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
# self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
if __name__ == "__main__":
unittest.main()

9
python/gtsam_py/python/tests/test_Values.py
View File

@ -15,8 +15,7 @@ import numpy as np
import gtsam
from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
imuBias_ConstantBias)
Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias)
from gtsam.utils.test_case import GtsamTestCase
@ -37,7 +36,7 @@ class TestValues(GtsamTestCase):
values.insert(7, Cal3DS2())
values.insert(8, Cal3Bundler())
values.insert(9, E)
values.insert(10, imuBias_ConstantBias())
values.insert(10, imuBias.ConstantBias())
# Special cases for Vectors and Matrices
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
@ -70,8 +69,8 @@ class TestValues(GtsamTestCase):
self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
self.gtsamAssertEquals(values.atimuBias_ConstantBias(
10), imuBias_ConstantBias(), tol)
self.gtsamAssertEquals(values.atConstantBias(
10), imuBias.ConstantBias(), tol)
# special cases for Vector and Matrix:
actualVector = values.atVector(11)

4
python/gtsam_py/python/tests/test_VisualISAMExample.py
View File

@ -46,11 +46,11 @@ class TestVisualISAMExample(GtsamTestCase):
isam, result = visual_isam.step(data, isam, result, truth, currentPose)
for i in range(len(truth.cameras)):
pose_i = result.atPose3(symbol(ord('x'), i))
pose_i = result.atPose3(symbol('x', i))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
for j in range(len(truth.points)):
point_j = result.atPoint3(symbol(ord('l'), j))
point_j = result.atPoint3(symbol('l', j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
if __name__ == "__main__":

6
python/gtsam_py/python/tests/test_dataset.py
View File

@ -15,7 +15,7 @@ from __future__ import print_function
import unittest
import gtsam
from gtsam import BetweenFactorPose3, BetweenFactorPose3s
from gtsam import BetweenFactorPose3
from gtsam.utils.test_case import GtsamTestCase
@ -37,8 +37,8 @@ class TestDataset(GtsamTestCase):
def test_parse3Dfactors(self):
"""Test parsing into data structure."""
factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
self.assertEqual(factors.size(), 6)
self.assertIsInstance(factors.at(0), BetweenFactorPose3)
self.assertEqual(len(factors), 6)
self.assertIsInstance(factors[0], BetweenFactorPose3)
if __name__ == '__main__':

7
python/gtsam_py/python/tests/test_initialize_pose3.py
View File

@ -24,7 +24,7 @@ class TestValues(GtsamTestCase):
def setUp(self):
model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
# We consider a small graph:
# symbolic FG
@ -64,9 +64,8 @@ class TestValues(GtsamTestCase):
def test_orientations(self):
pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
# comparison is up to M_PI, that's why we add some multiples of 2*M_PI
self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
@ -77,7 +76,7 @@ class TestValues(GtsamTestCase):
g2oFile = gtsam.findExampleDataFile("pose3example-grid")
is3D = True
inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
priorModel = gtsam.noiseModel_Unit.Create(6)
priorModel = gtsam.noiseModel.Unit.Create(6)
inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
initial = gtsam.InitializePose3.initialize(inputGraph)

2
python/gtsam_py/python/tests/test_logging_optimizer.py
View File

@ -21,7 +21,7 @@ from gtsam.utils.test_case import GtsamTestCase
from gtsam.utils.logging_optimizer import gtsam_optimize
KEY = 0
MODEL = gtsam.noiseModel_Unit.Create(3)
MODEL = gtsam.noiseModel.Unit.Create(3)
class TestOptimizeComet(GtsamTestCase):

6
python/gtsam_py/python/utils/circlePose3.py
View File

@ -3,7 +3,7 @@ import numpy as np
from math import pi, cos, sin
def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'):
"""
circlePose3 generates a set of poses in a circle. This function
returns those poses inside a gtsam.Values object, with sequential
@ -18,10 +18,6 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
Vehicle at p0 is looking towards y axis (X-axis points towards world y)
"""
# Force symbolChar to be a single character
if type(symbolChar) is str:
symbolChar = ord(symbolChar[0])
values = gtsam.Values()
theta = 0.0
dtheta = 2 * pi / numPoses

200
python/gtsam_py/python/utils/plot.py
View File

@ -13,9 +13,8 @@ def set_axes_equal(fignum):
Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Args:
fignum (int): An integer representing the figure number for Matplotlib.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
"""
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
@ -35,21 +34,7 @@ def set_axes_equal(fignum):
def ellipsoid(xc, yc, zc, rx, ry, rz, n):
"""
Numpy equivalent of Matlab's ellipsoid function.
Args:
xc (double): Center of ellipsoid in X-axis.
yc (double): Center of ellipsoid in Y-axis.
zc (double): Center of ellipsoid in Z-axis.
rx (double): Radius of ellipsoid in X-axis.
ry (double): Radius of ellipsoid in Y-axis.
rz (double): Radius of ellipsoid in Z-axis.
n (int): The granularity of the ellipsoid plotted.
Returns:
tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
"""
"""Numpy equivalent of Matlab's ellipsoid function"""
u = np.linspace(0, 2*np.pi, n+1)
v = np.linspace(0, np.pi, n+1)
x = -rx * np.outer(np.cos(u), np.sin(v)).T
@ -62,18 +47,9 @@ def ellipsoid(xc, yc, zc, rx, ry, rz, n):
def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
"""
Plots a Gaussian as an uncertainty ellipse
Based on Maybeck Vol 1, page 366
k=2.296 corresponds to 1 std, 68.26% of all probability
k=11.82 corresponds to 3 std, 99.74% of all probability
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
origin (gtsam.Point3): The origin in the world frame.
P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse.
scale (float): Scaling factor of the radii of the covariance ellipse.
n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses.
alpha (float): Transparency value for the plotted surface in the range [0, 1].
"""
k = 11.82
U, S, _ = np.linalg.svd(P)
@ -97,19 +73,10 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
"""
Plot a 2D pose on given axis `axes` with given `axis_length`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
pose (gtsam.Pose2): The pose to be plotted.
axis_length (float): The length of the camera axes.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
"""Plot a 2D pose on given axis 'axes' with given 'axis_length'."""
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
t = pose.translation()
origin = np.array([t.x(), t.y()])
origin = pose.translation()
# draw the camera axes
x_axis = origin + gRp[:, 0] * axis_length
@ -134,89 +101,34 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
np.rad2deg(angle), fill=False)
axes.add_patch(e1)
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 2D pose on given figure with given `axis_length`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
pose (gtsam.Pose2): The pose to be plotted.
axis_length (float): The length of the camera axes.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
"""
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
"""Plot a 2D pose on given figure with given 'axis_length'."""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca()
plot_pose2_on_axes(axes, pose, axis_length=axis_length,
covariance=covariance)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig
plot_pose2_on_axes(axes, pose, axis_length, covariance)
def plot_point3_on_axes(axes, point, linespec, P=None):
"""
Plot a 3D point on given axis `axes` with given `linespec`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
axes.plot([point.x()], [point.y()], [point.z()], linespec)
"""Plot a 3D point on given axis 'axes' with given 'linespec' and optional covariance 'P'."""
axes.plot([point[0]], [point[1]], [point[2]], linespec)
if P is not None:
plot_covariance_ellipse_3d(axes, point.vector(), P)
def plot_point3(fignum, point, linespec, P=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 3D point on given figure with given `linespec`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
Returns:
fig: The matplotlib figure.
"""
def plot_point3(fignum, point, linespec, P=None):
"""Plot a 3D point on given figure with given 'linespec' and optional covariance 'P'."""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_point3_on_axes(axes, point, linespec, P)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig
def plot_3d_points(fignum, values, linespec="g*", marginals=None,
title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
def plot_3d_points(fignum, values, linespec="g*", marginals=None):
"""
Plots the Point3s in `values`, with optional covariances.
Plots the Point3s in 'values', with optional covariances.
Finds all the Point3 objects in the given Values object and plots them.
If a Marginals object is given, this function will also plot marginal
covariance ellipses for each point.
Args:
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dictionary consisting of points to be plotted.
linespec (string): String representing formatting options for Matplotlib.
marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
keys = values.keys()
@ -227,34 +139,25 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
key = keys.at(i)
point = values.atPoint3(key)
if marginals is not None:
covariance = marginals.marginalCovariance(key)
P = marginals.marginalCovariance(key);
else:
covariance = None
P = None
fig = plot_point3(fignum, point, linespec, covariance,
axis_labels=axis_labels)
plot_point3(fignum, point, linespec, P)
except RuntimeError:
continue
# I guess it's not a Point3
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1):
"""
Plot a 3D pose on given axis `axes` with given `axis_length`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
Plot a 3D pose on given axis 'axes' with given 'axis_length'.
Optional 'scale' the pose and plot the covariance 'P'.
"""
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
origin = pose.translation().vector()
origin = pose.translation()
# draw the camera axes
x_axis = origin + gRp[:, 0] * axis_length
@ -269,7 +172,6 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
# plot the covariance
if P is not None:
# covariance matrix in pose coordinate frame
pPp = P[3:6, 3:6]
@ -278,49 +180,16 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
plot_covariance_ellipse_3d(axes, origin, gPp)
def plot_pose3(fignum, pose, axis_length=0.1, P=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 3D pose on given figure with given `axis_length`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
pose (gtsam.Pose3): 3D pose to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
Returns:
fig: The matplotlib figure.
"""
def plot_pose3(fignum, pose, P, axis_length=0.1):
"""Plot a 3D pose on given figure with given 'axis_length'."""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_pose3_on_axes(axes, pose, P=P,
axis_length=axis_length)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig
plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)
def plot_trajectory(fignum, values, scale=1, marginals=None,
title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a complete 3D trajectory using poses in `values`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dict containing the poses.
scale (float): Value to scale the poses by.
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
Used to plot uncertainty bounds.
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
pose3Values = gtsam.utilities_allPose3s(values)
def plot_trajectory(fignum, values, scale=1, marginals=None):
pose3Values = gtsam.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
lastIndex = None
@ -340,12 +209,11 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
pass
if marginals:
covariance = marginals.marginalCovariance(lastKey)
P = marginals.marginalCovariance(lastKey)
else:
covariance = None
P = None
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
plot_pose3(fignum, lastPose, P, scale)
lastIndex = i
@ -355,15 +223,11 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals:
covariance = marginals.marginalCovariance(lastKey)
P = marginals.marginalCovariance(lastKey)
else:
covariance = None
P = None
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
plot_pose3(fignum, lastPose, P, scale)
except:
pass
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())

6
python/gtsam_py/python/utils/test_case.py
View File

@ -21,7 +21,11 @@ class GtsamTestCase(unittest.TestCase):
Keyword Arguments:
tol {float} -- tolerance passed to 'equals', default 1e-9
"""
equal = actual.equals(expected, tol)
import numpy
if isinstance(expected, numpy.ndarray):
equal = numpy.allclose(actual, expected, atol=tol)
else:
equal = actual.equals(expected, tol)
if not equal:
raise self.failureException(
"Values are not equal:\n{}!={}".format(actual, expected))

53
python/gtsam_py/python/utils/visual_data_generator.py
View File

@ -1,9 +1,8 @@
from __future__ import print_function
import numpy as np
from math import pi, cos, sin
import gtsam
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
class Options:
@ -11,7 +10,7 @@ class Options:
Options to generate test scenario
"""
def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()):
"""
Options to generate test scenario
@param triangle: generate a triangle scene with 3 points if True, otherwise
@ -28,10 +27,10 @@ class GroundTruth:
Object holding generated ground-truth data
"""
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.cameras = [Pose3()] * nrCameras
self.points = [Point3()] * nrPoints
self.cameras = [gtsam.Pose3()] * nrCameras
self.points = [gtsam.Point3(0, 0, 0)] * nrPoints
def print_(self, s=""):
print(s)
@ -53,28 +52,28 @@ class Data:
class NoiseModels:
pass
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
self.odometry = [Pose3()] * nrCameras
self.odometry = [gtsam.Pose3()] * nrCameras
# Set Noise parameters
self.noiseModels = Data.NoiseModels()
self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas(
self.noiseModels.posePrior = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
# noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
# noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
# np.array([0.001,0.001,0.001,0.1,0.1,0.1]))
self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
self.noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)
self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
def generate_data(options):
""" Generate ground-truth and measurement data. """
K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
nrPoints = 3 if options.triangle else 8
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
@ -84,26 +83,26 @@ def generate_data(options):
if options.triangle: # Create a triangle target, just 3 points on a plane
r = 10
for j in range(len(truth.points)):
theta = j * 2 * np.pi / nrPoints
truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
theta = j * 2 * pi / nrPoints
truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),
Point3(-10, -10, 10), Point3(10, -10, 10),
Point3(10, 10, -10), Point3(-10, 10, -10),
Point3(-10, -10, -10), Point3(10, -10, -10)
gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10),
gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10),
gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10),
gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10)
]
# Create camera cameras on a circle around the triangle
height = 10
r = 40
for i in range(options.nrCameras):
theta = i * 2 * np.pi / options.nrCameras
t = Point3(r * np.cos(theta), r * np.sin(theta), height)
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
Point3(),
Point3(0, 0, 1),
truth.K)
theta = i * 2 * pi / options.nrCameras
t = gtsam.Point3(r * cos(theta), r * sin(theta), height)
truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
gtsam.Point3(0, 0, 0),
gtsam.Point3(0, 0, 1),
truth.K)
# Create measurements
for j in range(nrPoints):
# All landmarks seen in every frame

22
python/gtsam_py/python/utils/visual_isam.py
View File

@ -25,7 +25,7 @@ def initialize(data, truth, options):
newFactors = gtsam.NonlinearFactorGraph()
initialEstimates = gtsam.Values()
for i in range(2):
ii = symbol(ord('x'), i)
ii = symbol('x', i)
if i == 0:
if options.hardConstraint: # add hard constraint
newFactors.add(
@ -41,10 +41,10 @@ def initialize(data, truth, options):
# Add visual measurement factors from two first poses and initialize
# observed landmarks
for i in range(2):
ii = symbol(ord('x'), i)
ii = symbol('x', i)
for k in range(len(data.Z[i])):
j = data.J[i][k]
jj = symbol(ord('l'), j)
jj = symbol('l', j)
newFactors.add(
gtsam.GenericProjectionFactorCal3_S2(data.Z[i][
k], data.noiseModels.measurement, ii, jj, data.K))
@ -59,8 +59,8 @@ def initialize(data, truth, options):
# Add odometry between frames 0 and 1
newFactors.add(
gtsam.BetweenFactorPose3(
symbol(ord('x'), 0),
symbol(ord('x'), 1), data.odometry[1], data.noiseModels.odometry))
symbol('x', 0),
symbol('x', 1), data.odometry[1], data.noiseModels.odometry))
# Update ISAM
if options.batchInitialization: # Do a full optimize for first two poses
@ -98,28 +98,28 @@ def step(data, isam, result, truth, currPoseIndex):
odometry = data.odometry[prevPoseIndex]
newFactors.add(
gtsam.BetweenFactorPose3(
symbol(ord('x'), prevPoseIndex),
symbol(ord('x'), currPoseIndex), odometry,
symbol('x', prevPoseIndex),
symbol('x', currPoseIndex), odometry,
data.noiseModels.odometry))
# Add visual measurement factors and initializations as necessary
for k in range(len(data.Z[currPoseIndex])):
zij = data.Z[currPoseIndex][k]
j = data.J[currPoseIndex][k]
jj = symbol(ord('l'), j)
jj = symbol('l', j)
newFactors.add(
gtsam.GenericProjectionFactorCal3_S2(
zij, data.noiseModels.measurement,
symbol(ord('x'), currPoseIndex), jj, data.K))
symbol('x', currPoseIndex), jj, data.K))
# TODO: initialize with something other than truth
if not result.exists(jj) and not initialEstimates.exists(jj):
lmInit = truth.points[j]
initialEstimates.insert(jj, lmInit)
# Initial estimates for the new pose.
prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex))
prevPose = result.atPose3(symbol('x', prevPoseIndex))
initialEstimates.insert(
symbol(ord('x'), currPoseIndex), prevPose.compose(odometry))
symbol('x', currPoseIndex), prevPose.compose(odometry))
# Update ISAM
# figure(1)tic

24
python/gtsam_unstable_py/python/examples/FixedLagSmootherExample.py
View File

@ -16,16 +16,6 @@ import numpy as np
import gtsam
import gtsam_unstable
def _timestamp_key_value(key, value):
"""
"""
return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
key, value
)
def BatchFixedLagSmootherExample():
"""
Runs a batch fixed smoother on an agent with two odometry
@ -45,21 +35,21 @@ def BatchFixedLagSmootherExample():
# Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0)
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
X1 = 0
new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean)
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
new_timestamps.insert((X1, 0.0))
delta_time = 0.25
time = 0.25
while time <= 3.0:
previous_key = 1000 * (time - delta_time)
current_key = 1000 * time
previous_key = int(1000 * (time - delta_time))
current_key = int(1000 * time)
# assign current key to the current timestamp
new_timestamps.insert(_timestamp_key_value(current_key, time))
new_timestamps.insert((current_key, time))
# Add a guess for this pose to the new values
# Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
@ -69,14 +59,14 @@ def BatchFixedLagSmootherExample():
# Add odometry factors from two different sources with different error
# stats
odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.05]))
new_factors.push_back(gtsam.BetweenFactorPose2(
previous_key, current_key, odometry_measurement_1, odometry_noise_1
))
odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.05, 0.05]))
new_factors.push_back(gtsam.BetweenFactorPose2(
previous_key, current_key, odometry_measurement_2, odometry_noise_2

4
python/gtsam_unstable_py/python/examples/TimeOfArrivalExample.py
View File

@ -12,7 +12,7 @@ Author: Frank Dellaert
# pylint: disable=invalid-name, no-name-in-module
from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
NonlinearFactorGraph, Point3, Values, noiseModel)
from gtsam_unstable import Event, TimeOfArrival, TOAFactor
# units
@ -64,7 +64,7 @@ def create_graph(microphones, simulatedTOA):
graph = NonlinearFactorGraph()
# Create a noise model for the TOA error
model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)
K = len(microphones)
key = 0

21
python/gtsam_unstable_py/python/tests/test_FixedLagSmootherExample.py
View File

@ -16,13 +16,6 @@ import gtsam
import gtsam_unstable
from gtsam.utils.test_case import GtsamTestCase
def _timestamp_key_value(key, value):
return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
key, value
)
class TestFixedLagSmootherExample(GtsamTestCase):
'''
Tests the fixed lag smoother wrapper
@ -47,14 +40,14 @@ class TestFixedLagSmootherExample(GtsamTestCase):
# Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0)
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.1]))
X1 = 0
new_factors.push_back(
gtsam.PriorFactorPose2(
X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean)
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
new_timestamps.insert((X1, 0.0))
delta_time = 0.25
time = 0.25
@ -80,11 +73,11 @@ class TestFixedLagSmootherExample(GtsamTestCase):
# and its two odometers measure the change. The smoothed
# result is then compared to the ground truth
while time <= 3.0:
previous_key = 1000 * (time - delta_time)
current_key = 1000 * time
previous_key = int(1000 * (time - delta_time))
current_key = int(1000 * time)
# assign current key to the current timestamp
new_timestamps.insert(_timestamp_key_value(current_key, time))
new_timestamps.insert((current_key, time))
# Add a guess for this pose to the new values
# Assume that the robot moves at 2 m/s. Position is time[s] *
@ -95,7 +88,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
# Add odometry factors from two different sources with different
# error stats
odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.05]))
new_factors.push_back(
gtsam.BetweenFactorPose2(
@ -105,7 +98,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
odometry_noise_1))
odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.05, 0.05]))
new_factors.push_back(
gtsam.BetweenFactorPose2(