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

Migrated all examples and tests

release/4.3a0
Fan Jiang 2020-08-18 11:02:35 -04:00
parent 8a130c1a54
commit 03306cba9c
49 changed files with 491 additions and 375 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
python/gtsam/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/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()

137
python/gtsam/examples/ImuFactorExample.py
View File

@ -12,14 +12,13 @@ Author: Frank Dellaert, Varun Agrawal
from __future__ import print_function
import argparse
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.symbol_shorthand import B, V, X
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
@ -33,24 +32,27 @@ np.set_printoptions(precision=3, suppress=True)
class ImuFactorExample(PreintegrationExample):
def __init__(self):
def __init__(self, twist_scenario="sick_twist"):
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))
forward_twist = (np.zeros(3), self.velocity)
loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
sick_twist = (
np.array([math.radians(30), -math.radians(30), 0]), self.velocity)
twist_scenarios = dict(
zero_twist=(np.zeros(3), np.zeros(3)),
forward_twist=(np.zeros(3), self.velocity),
loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity),
sick_twist=(np.array([math.radians(30), -math.radians(30), 0]),
self.velocity)
)
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)
super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
bias, dt)
def addPrior(self, i, graph):
state = self.scenario.navState(i)
@ -59,65 +61,73 @@ class ImuFactorExample(PreintegrationExample):
graph.push_back(gtsam.PriorFactorVector(
V(i), state.velocity(), self.velNoise))
def run(self):
def run(self, T=12, compute_covariances=False, verbose=True):
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
T = 12
num_poses = T + 1 # assumes 1 factor per second
num_poses = T # assumes 1 factor per second
initial = gtsam.Values()
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)
# simulate the loop
i = 0 # state index
actual_state_i = self.scenario.navState(0)
initial_state_i = self.scenario.navState(0)
initial.insert(X(i), initial_state_i.pose())
initial.insert(V(i), initial_state_i.velocity())
# add prior on beginning
self.addPrior(0, graph)
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
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)
# Plot every second
if k % int(1 / self.dt) == 0:
self.plotGroundTruthPose(t)
if (k+1) % int(1 / self.dt) == 0:
# Plot every second
self.plotGroundTruthPose(t, scale=1)
plt.title("Ground Truth Trajectory")
# create IMU factor every second
if (k + 1) % int(1 / self.dt) == 0:
factor = gtsam.ImuFactor(X(i), V(i), X(
i + 1), V(i + 1), BIAS_KEY, pim)
# create IMU factor every second
factor = gtsam.ImuFactor(X(i), V(i),
X(i + 1), V(i + 1),
BIAS_KEY, pim)
graph.push_back(factor)
if True:
if verbose:
print(factor)
print(pim.predict(actual_state_i, self.actualBias))
pim.resetIntegration()
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
translationNoise = gtsam.Point3(*np.random.randn(3)*1)
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
actual_state_i = self.scenario.navState(t + self.dt)
print("Actual state at {0}:\n{1}".format(
t+self.dt, actual_state_i))
noisy_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
initial.insert(X(i+1), noisy_state_i.pose())
initial.insert(V(i+1), noisy_state_i.velocity())
i += 1
# add priors on beginning and end
self.addPrior(0, graph)
self.addPrior(num_poses - 1, graph)
# add priors on end
# self.addPrior(num_poses - 1, graph)
initial.print_("Initial values:")
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
@ -125,29 +135,46 @@ class ImuFactorExample(PreintegrationExample):
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
result.print_("Optimized values:")
if compute_covariances:
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n",
marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1)
plot_pose3(POSES_FIG+1, pose_i, 1)
i += 1
plt.title("Estimated Trajectory")
gtsam.utils.plot.set_axes_equal(POSES_FIG)
gtsam.utils.plot.set_axes_equal(POSES_FIG+1)
print(result.atimuBias_ConstantBias(BIAS_KEY))
print("Bias Values", result.atConstantBias(BIAS_KEY))
plt.ioff()
plt.show()
if __name__ == '__main__':
ImuFactorExample().run()
parser = argparse.ArgumentParser("ImuFactorExample.py")
parser.add_argument("--twist_scenario",
default="sick_twist",
choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist"))
parser.add_argument("--time", "-T", default=12,
type=int, help="Total time in seconds")
parser.add_argument("--compute_covariances",
default=False, action='store_true')
parser.add_argument("--verbose", default=False, action='store_true')
args = parser.parse_args()
ImuFactorExample(args.twist_scenario).run(
args.time, args.compute_covariances, args.verbose)

130
python/gtsam/examples/ImuFactorExample2.py → python/gtsam/examples/ImuFactorISAM2Example.py
View File

@ -1,6 +1,6 @@
"""
iSAM2 example with ImuFactor.
Author: Robert Truax (C++), Frank Dellaert (Python)
Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal
"""
# pylint: disable=invalid-name, E1101
@ -9,7 +9,6 @@ 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,
@ -17,9 +16,8 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
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 gtsam.symbol_shorthand import B, V, X
from gtsam.utils import plot
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
@ -28,46 +26,45 @@ def vector3(x, y, z):
return np.array([x, y, z], dtype=np.float)
def ISAM2_plot(values, fignum=0):
"""Plot poses."""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
i = 0
min_bounds = 0, 0, 0
max_bounds = 0, 0, 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()
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
i += 1
# draw
axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
plt.pause(1)
# IMU preintegration parameters
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
g = 9.81
n_gravity = vector3(0, 0, -g)
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
I = np.eye(3)
PARAMS.setAccelerometerCovariance(I * 0.1)
PARAMS.setGyroscopeCovariance(I * 0.1)
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))
def preintegration_parameters():
# IMU preintegration parameters
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
I = np.eye(3)
PARAMS.setAccelerometerCovariance(I * 0.1)
PARAMS.setGyroscopeCovariance(I * 0.1)
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))
return PARAMS, BIAS_COVARIANCE, DELTA
def get_camera(radius):
up = Point3(0, 0, 1)
target = Point3(0, 0, 0)
position = Point3(radius, 0, 0)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
return camera
def get_scenario(radius, pose_0, angular_velocity, delta_t):
"""Create the set of ground-truth landmarks and poses"""
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
return scenario
def IMU_example():
@ -75,23 +72,17 @@ def IMU_example():
# 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())
camera = get_camera(radius)
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
angular_velocity = math.radians(180) # rad/sec
delta_t = 1.0/18 # makes for 10 degrees per step
angular_velocity = math.radians(180) # rad/sec
scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
# Create a factor graph
newgraph = NonlinearFactorGraph()
graph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = ISAM2()
@ -102,23 +93,23 @@ def IMU_example():
# 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))
graph.push_back(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 = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
biasnoise)
newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
graph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
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)
newgraph.push_back(velprior)
graph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
@ -139,9 +130,9 @@ def IMU_example():
if i % 5 == 0:
biasKey += 1
factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
graph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
@ -152,21 +143,24 @@ def IMU_example():
# Add Imu Factor
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
graph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(newgraph, initialEstimate)
isam.update(graph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
plot.plot_incremental_trajectory(0, result,
start=i, scale=3, time_interval=0.01)
# reset
newgraph = NonlinearFactorGraph()
graph = NonlinearFactorGraph()
initialEstimate.clear()
plt.show()
if __name__ == '__main__':
IMU_example()

4
python/gtsam/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/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

12
python/gtsam/examples/PlanarSLAMExample.py
View File

@ -13,15 +13,15 @@ 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
from gtsam.symbol_shorthand import X, L
# 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()

5
python/gtsam/examples/Pose2SLAMExample.py
View File

@ -27,10 +27,9 @@ 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))
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()

4
python/gtsam/examples/Pose2SLAMExample_g2o.py
View File

@ -53,7 +53,7 @@ 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))
priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
@ -82,7 +82,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities_extractPose2(result)
resultPoses = gtsam.utilities.extractPose2(result)
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()

6
python/gtsam/examples/Pose3SLAMExample_g2o.py
View File

@ -39,11 +39,11 @@ 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 ")
firstKey = initial.keys().at(0)
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
@ -65,7 +65,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities_allPose3s(result)
resultPoses = gtsam.utilities.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()

4
python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
View File

@ -24,9 +24,9 @@ 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)
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
# Initializing Pose3 - chordal relaxation"

47
python/gtsam/examples/PreintegrationExample.py
View File

@ -10,12 +10,11 @@ A script validating the Preintegration of IMU measurements
import math
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
IMU_FIG = 1
POSES_FIG = 2
@ -68,60 +67,62 @@ 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)
fig, self.axes = plt.subplots(4, 3)
fig.set_tight_layout(True)
def plotImu(self, t, measuredOmega, measuredAcc):
plt.figure(IMU_FIG)
# plot angular velocity
omega_b = self.scenario.omega_b(t)
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
plt.subplot(4, 3, i + 1)
plt.scatter(t, omega_b[i], color='k', marker='.')
plt.scatter(t, measuredOmega[i], color=color, marker='.')
plt.xlabel('angular velocity ' + label)
ax = self.axes[0][i]
ax.scatter(t, omega_b[i], color='k', marker='.')
ax.scatter(t, measuredOmega[i], color=color, marker='.')
ax.set_xlabel('angular velocity ' + label)
# plot acceleration in nav
acceleration_n = self.scenario.acceleration_n(t)
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
plt.subplot(4, 3, i + 4)
plt.scatter(t, acceleration_n[i], color=color, marker='.')
plt.xlabel('acceleration in nav ' + label)
ax = self.axes[1][i]
ax.scatter(t, acceleration_n[i], color=color, marker='.')
ax.set_xlabel('acceleration in nav ' + label)
# plot acceleration in body
acceleration_b = self.scenario.acceleration_b(t)
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
plt.subplot(4, 3, i + 7)
plt.scatter(t, acceleration_b[i], color=color, marker='.')
plt.xlabel('acceleration in body ' + label)
ax = self.axes[2][i]
ax.scatter(t, acceleration_b[i], color=color, marker='.')
ax.set_xlabel('acceleration in body ' + label)
# plot actual specific force, as well as corrupted
actual = self.runner.actualSpecificForce(t)
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
plt.subplot(4, 3, i + 10)
plt.scatter(t, actual[i], color='k', marker='.')
plt.scatter(t, measuredAcc[i], color=color, marker='.')
plt.xlabel('specific force ' + label)
ax = self.axes[3][i]
ax.scatter(t, actual[i], color='k', marker='.')
ax.scatter(t, measuredAcc[i], color=color, marker='.')
ax.set_xlabel('specific force ' + label)
def plotGroundTruthPose(self, t):
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
# plot ground truth pose, as well as prediction from integrated IMU measurements
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(np.abs(t)), self.maxDim])
ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)
ax.set_zlim3d(-self.maxDim, self.maxDim)
plt.pause(0.01)
plt.pause(time_interval)
def run(self):
def run(self, T=12):
# simulate the loop
T = 12
for i, t in enumerate(np.arange(0, T, self.dt)):
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)

19
python/gtsam/examples/README.md
View File

@ -1,17 +1,12 @@
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.
# Porting Progress
| C++ Example Name | Ported |
|-------------------------------------------------------|--------|
| CameraResectioning | |
| CreateSFMExampleData | |
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython |
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python |
| ImuFactorExample2 | X |
| ImuFactorsExample | |
| ISAM2Example_SmartFactor | |
@ -25,7 +20,7 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan
| Pose2SLAMExample_g2o | X |
| Pose2SLAMExample_graph | |
| Pose2SLAMExample_graphviz | |
| Pose2SLAMExample_lago | lago not exposed through cython |
| Pose2SLAMExample_lago | lago not yet exposed through Python |
| Pose2SLAMStressTest | |
| Pose2SLAMwSPCG | |
| Pose3SLAMExample_changeKeys | |
@ -47,11 +42,11 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan
| StereoVOExample | |
| StereoVOExample_large | |
| TimeTBB | |
| UGM_chain | discrete functionality not exposed |
| UGM_small | discrete functionality not exposed |
| UGM_chain | discrete functionality not yet exposed |
| UGM_small | discrete functionality not yet exposed |
| VisualISAM2Example | X |
| VisualISAMExample | X |
Extra Examples (with no C++ equivalent)
- PlanarManipulatorExample
- SFMData
- SFMData

19
python/gtsam/examples/SFMExample.py
View File

@ -13,14 +13,15 @@ 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 import symbol_shorthand
L = symbol_shorthand.L
X = symbol_shorthand.X
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
from gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values)
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
from gtsam.utils import plot
@ -56,7 +57,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,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]))
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)
graph.push_back(factor)
@ -85,7 +86,7 @@ def main():
# 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)
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
@ -97,7 +98,7 @@ def main():
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
transformed_point = point + 0.1*np.random.randn(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')

3
python/gtsam/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/examples/SimpleRotation.py
View File

@ -10,10 +10,9 @@ 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
from gtsam.symbol_shorthand import X
def main():
"""
@ -33,7 +32,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))
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = X(1)
factor = gtsam.PriorFactorRot2(key, prior, model)

11
python/gtsam/examples/VisualISAM2Example.py
View File

@ -17,8 +17,7 @@ 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.symbol_shorthand import L, X
from gtsam.examples import SFMdata
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
@ -64,7 +63,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 +109,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 +122,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)

30
python/gtsam/examples/VisualISAMExample.py
View File

@ -15,13 +15,11 @@ 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,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values)
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values, Point3)
from gtsam.symbol_shorthand import X, L
def main():
"""
@ -34,7 +32,8 @@ 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()
@ -55,11 +54,13 @@ def main():
# 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, X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20))
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
t=Point3(0.05, -0.10, 0.20))
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
@ -71,12 +72,13 @@ 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]))
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)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
@ -84,8 +86,8 @@ def main():
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(L(j), initial_lj)
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)

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

2
python/gtsam/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)

6
python/gtsam/tests/test_GaussianFactorGraph.py
View File

@ -16,7 +16,7 @@ import unittest
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
from gtsam.symbol_shorthand import X
from gtsam.utils.test_case import GtsamTestCase
@ -28,8 +28,8 @@ def create_graph():
x1 = X(1)
x2 = 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/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/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/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/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))

10
python/gtsam/tests/test_NonlinearOptimizer.py
View File

@ -15,11 +15,11 @@ from __future__ import print_function
import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
from gtsam import (DoglegOptimizer, DoglegParams,
DummyPreconditionerParameters, GaussNewtonOptimizer,
GaussNewtonParams, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, PCGSolverParameters,
DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
Point2, PriorFactorPoint2, Values)
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
PCGSolverParameters, Point2, PriorFactorPoint2, Values)
from gtsam.utils.test_case import GtsamTestCase
KEY1 = 1
@ -30,7 +30,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

4
python/gtsam/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/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/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/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))

8
python/gtsam/tests/test_PriorFactor.py
View File

@ -23,14 +23,14 @@ 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)
@ -45,14 +45,14 @@ class TestPriorFactor(GtsamTestCase):
# define and add Pose3 prior
key = 5
priorPose3 = gtsam.Pose3()
model = gtsam.noiseModel_Unit.Create(6)
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)
model = gtsam.noiseModel.Unit.Create(3)
graph.addPriorVector(key, priorVector, model)
self.assertEqual(graph.size(), 2)

27
python/gtsam/tests/test_SFMExample.py
View File

@ -15,8 +15,9 @@ import numpy as np
import gtsam
import gtsam.utils.visual_data_generator as generator
from gtsam import symbol
from gtsam.noiseModel import Isotropic, Diagonal
from gtsam.utils.test_case import GtsamTestCase
from gtsam.symbol_shorthand import X, P
class TestSFMExample(GtsamTestCase):
@ -34,29 +35,29 @@ class TestSFMExample(GtsamTestCase):
graph = gtsam.NonlinearFactorGraph()
# Add factors for all measurements
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma)
measurementNoise = 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))
X(i), P(j), data.K))
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0),
posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(X(0),
truth.cameras[0].pose(), posePriorNoise))
pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0),
pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(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(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(P(j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
@ -66,16 +67,16 @@ class TestSFMExample(GtsamTestCase):
# Marginalization
marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(symbol(ord('p'), 0))
marginals.marginalCovariance(symbol(ord('x'), 0))
marginals.marginalCovariance(P(0))
marginals.marginalCovariance(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(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(P(j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
if __name__ == "__main__":

5
python/gtsam/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/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/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/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/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/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/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/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/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):

11
python/gtsam/utils/circlePose3.py
View File

@ -1,9 +1,10 @@
import gtsam
import math
import numpy as np
from math import pi, cos, sin
from math import pi
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 +19,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
@ -29,7 +26,7 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
for i in range(numPoses):
key = gtsam.symbol(symbolChar, i)
gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0)
gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0)
oRi = gtsam.Rot3.Yaw(
-theta) # negative yaw goes counterclockwise, with Z down !
gTi = gtsam.Pose3(gRo.compose(oRi), gti)

63
python/gtsam/utils/plot.py
View File

@ -109,7 +109,7 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
# 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 = t
# draw the camera axes
x_axis = origin + gRp[:, 0] * axis_length
@ -169,9 +169,9 @@ def plot_point3_on_axes(axes, point, linespec, P=None):
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)
axes.plot([point[0]], [point[1]], [point[2]], linespec)
if P is not None:
plot_covariance_ellipse_3d(axes, point.vector(), P)
plot_covariance_ellipse_3d(axes, point, P)
def plot_point3(fignum, point, linespec, P=None,
@ -221,9 +221,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
keys = values.keys()
# Plot points and covariance matrices
for i in range(keys.size()):
for key in keys:
try:
key = keys.at(i)
point = values.atPoint3(key)
if marginals is not None:
covariance = marginals.marginalCovariance(key)
@ -253,7 +252,7 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
"""
# 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
@ -319,19 +318,17 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
pose3Values = gtsam.utilities_allPose3s(values)
pose3Values = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
lastIndex = None
lastKey = None
for i in range(keys.size()):
key = keys.at(i)
for key in keys:
try:
pose = pose3Values.atPose3(key)
except:
print("Warning: no Pose3 at key: {0}".format(key))
if lastIndex is not None:
lastKey = keys.at(lastIndex)
if lastKey is not None:
try:
lastPose = pose3Values.atPose3(lastKey)
except:
@ -346,11 +343,10 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
lastIndex = i
lastKey = key
# Draw final pose
if lastIndex is not None:
lastKey = keys.at(lastIndex)
if lastKey is not None:
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals:
@ -366,3 +362,40 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())
def plot_incremental_trajectory(fignum, values, start=0,
scale=1, marginals=None,
time_interval=0.0):
"""
Incrementally 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.
start (int): Starting index to start plotting from.
scale (float): Value to scale the poses by.
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
Used to plot uncertainty bounds.
time_interval (float): Time in seconds to pause between each rendering.
Used to create animation effect.
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
pose3Values = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
for key in keys[start:]:
if values.exists(key):
pose_i = values.atPose3(key)
plot_pose3(fignum, pose_i, scale)
# Update the plot space to encompass all plotted points
axes.autoscale()
# Set the 3 axes equal
set_axes_equal(fignum)
# Pause for a fixed amount of seconds
plt.pause(time_interval)

6
python/gtsam/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))

37
python/gtsam/utils/visual_data_generator.py
View File

@ -1,9 +1,10 @@
from __future__ import print_function
import numpy as np
import math
from math import pi
import gtsam
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
class Options:
@ -30,8 +31,8 @@ class GroundTruth:
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.cameras = [gtsam.Pose3()] * nrCameras
self.points = [gtsam.Point3(0, 0, 0)] * nrPoints
self.cameras = [Pose3()] * nrCameras
self.points = [Point3(0, 0, 0)] * nrPoints
def print_(self, s=""):
print(s)
@ -55,20 +56,20 @@ class Data:
def __init__(self, K=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
# 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):
@ -84,8 +85,8 @@ 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] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),
@ -98,12 +99,12 @@ def generate_data(options):
height = 10
r = 40
for i in range(options.nrCameras):
theta = i * 2 * np.pi / options.nrCameras
t = gtsam.Point3(r * np.cos(theta), r * np.sin(theta), height)
truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
gtsam.Point3(0, 0, 0),
gtsam.Point3(0, 0, 1),
truth.K)
theta = i * 2 * pi / options.nrCameras
t = Point3(r * math.cos(theta), r * math.sin(theta), height)
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
Point3(0, 0, 0),
Point3(0, 0, 1),
truth.K)
# Create measurements
for j in range(nrPoints):
# All landmarks seen in every frame

22
python/gtsam/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/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/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/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(