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

Merge pull request #903 from borglab/feature/python-examples

release/4.3a0
Varun Agrawal 2021-10-23 01:06:09 -04:00 committed by GitHub
parent cb0e62b1ad f8f93cab21
commit c56579c61d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 481 additions and 400 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

179
python/gtsam/examples/CustomFactorExample.py
View File

@ -9,15 +9,17 @@ CustomFactor demo that simulates a 1-D sensor fusion task.
Author: Fan Jiang, Frank Dellaert
"""
from functools import partial
from typing import List, Optional
import gtsam
import numpy as np
from typing import List, Optional
from functools import partial
I = np.eye(1)
def simulate_car():
# Simulate a car for one second
def simulate_car() -> List[float]:
"""Simulate a car for one second"""
x0 = 0
dt = 0.25 # 4 Hz, typical GPS
v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast
@ -26,21 +28,87 @@ def simulate_car():
return x
def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> float:
"""GPS Factor error function
:param measurement: GPS measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
estimate = values.atVector(key)
error = estimate - measurement
if jacobians is not None:
jacobians[0] = I
return error
def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> float:
"""Odometry Factor error function
:param measurement: Odometry measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key1 = this.keys()[0]
key2 = this.keys()[1]
pos1, pos2 = values.atVector(key1), values.atVector(key2)
error = measurement - (pos1 - pos2)
if jacobians is not None:
jacobians[0] = I
jacobians[1] = -I
return error
def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> float:
"""Landmark Factor error function
:param measurement: Landmark measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
pos = values.atVector(key)
error = pos - measurement
if jacobians is not None:
jacobians[0] = I
return error
def main():
"""Main runner."""
x = simulate_car()
print(f"Simulated car trajectory: {x}")
# %%
add_noise = True # set this to False to run with "perfect" measurements
# GPS measurements
sigma_gps = 3.0 # assume GPS is +/- 3m
g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
for k in range(5)]
g = [
x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
for k in range(5)
]
# Odometry measurements
sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz
o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0)
for k in range(4)]
o = [
x[k + 1] - x[k] +
(np.random.normal(scale=sigma_odo) if add_noise else 0)
for k in range(4)
]
# Landmark measurements:
sigma_lm = 1 # assume landmark measurement is accurate up to 1m
@ -61,30 +129,12 @@ print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
factor_graph = gtsam.NonlinearFactorGraph()
# Add factors for GPS measurements
I = np.eye(1)
gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""GPS Factor error function
:param measurement: GPS measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
estimate = values.atVector(key)
error = estimate - measurement
if jacobians is not None:
jacobians[0] = I
return error
# Add the GPS factors
for k in range(5):
gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]])))
gf = gtsam.CustomFactor(gps_model, [unknown[k]],
partial(error_gps, np.array([g[k]])))
factor_graph.add(gf)
# New Values container
@ -102,36 +152,19 @@ optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
result = optimizer.optimize()
# calculate the error from ground truth
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
error = np.array([(result.atVector(unknown[k]) - x[k])[0]
for k in range(5)])
print("Result with only GPS")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
print(result, np.round(error, 2),
f"\nJ(X)={0.5 * np.sum(np.square(error))}")
# Adding odometry will improve things a lot
odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""Odometry Factor error function
:param measurement: Odometry measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key1 = this.keys()[0]
key2 = this.keys()[1]
pos1, pos2 = values.atVector(key1), values.atVector(key2)
error = measurement - (pos1 - pos2)
if jacobians is not None:
jacobians[0] = I
jacobians[1] = -I
return error
for k in range(4):
odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]])))
odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]],
partial(error_odom, np.array([o[k]])))
factor_graph.add(odof)
params = gtsam.GaussNewtonParams()
@ -139,41 +172,35 @@ optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
result = optimizer.optimize()
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
error = np.array([(result.atVector(unknown[k]) - x[k])[0]
for k in range(5)])
print("Result with GPS+Odometry")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
print(result, np.round(error, 2),
f"\nJ(X)={0.5 * np.sum(np.square(error))}")
# This is great, but GPS noise is still apparent, so now we add the two landmarks
lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
"""Landmark Factor error function
:param measurement: Landmark measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
pos = values.atVector(key)
error = pos - measurement
if jacobians is not None:
jacobians[0] = I
return error
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0]))))
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3]))))
factor_graph.add(
gtsam.CustomFactor(lm_model, [unknown[0]],
partial(error_lm, np.array([lm_0 + z_0]))))
factor_graph.add(
gtsam.CustomFactor(lm_model, [unknown[3]],
partial(error_lm, np.array([lm_3 + z_3]))))
params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
result = optimizer.optimize()
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
error = np.array([(result.atVector(unknown[k]) - x[k])[0]
for k in range(5)])
print("Result with GPS+Odometry+Landmark")
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
print(result, np.round(error, 2),
f"\nJ(X)={0.5 * np.sum(np.square(error))}")
if __name__ == "__main__":
main()

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

@ -13,13 +13,8 @@ Author: Mandy Xie
from __future__ import print_function
import numpy as np
import gtsam
import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot
# ENU Origin is where the plane was in hold next to runway
lat0 = 33.86998
lon0 = -84.30626
@ -29,6 +24,9 @@ h0 = 274
GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25)
def main():
"""Main runner."""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
@ -54,3 +52,6 @@ optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
if __name__ == "__main__":
main()

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

@ -13,17 +13,18 @@ Author: Frank Dellaert
from __future__ import print_function
import numpy as np
import gtsam
import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
import numpy as np
# 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]))
def main():
"""Main runner"""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
@ -57,13 +58,15 @@ print("\nFinal Result:\n{}".format(result))
# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for i in range(1, 4):
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
print("X{} covariance:\n{}\n".format(i,
marginals.marginalCovariance(i)))
fig = plt.figure(0)
for i in range(1, 4):
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()
if __name__ == "__main__":
main()

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

@ -13,16 +13,19 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
from __future__ import print_function
import numpy as np
import gtsam
from gtsam.symbol_shorthand import X, L
import numpy as np
from gtsam.symbol_shorthand import L, X
# 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]))
def main():
"""Main runner"""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
@ -34,21 +37,27 @@ L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(gtsam.BetweenFactorPose2(
X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(gtsam.BearingRangeFactor2D(
X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
# Print graph
print("Factor Graph:\n{}".format(graph))
@ -71,11 +80,18 @@ print("Initial Estimate:\n{}".format(initial_estimate))
# Here we will use the default set of parameters. See the
# documentation for the full set of parameters.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
# Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))
for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
(L2, "L2")]:
print("{} covariance:\n{}\n".format(s,
marginals.marginalCovariance(key)))
if __name__ == "__main__":
main()

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

@ -15,21 +15,17 @@ from __future__ import print_function
import math
import numpy as np
import gtsam
import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=float)
def main():
"""Main runner."""
# 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(gtsam.Point3(0.3, 0.3, 0.1))
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(
gtsam.Point3(0.2, 0.2, 0.1))
# 1. Create a factor graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
@ -40,20 +36,25 @@ graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
# 2b. Add odometry factors
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2),
ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2),
ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2),
ODOMETRY_NOISE))
# 2c. Add the loop closure constraint
# This factor encodes the fact that we have returned to the same pose. In real
# systems, these constraints may be identified in many ways, such as appearance-based
# techniques with camera images. We will use another Between Factor to enforce this constraint:
graph.add(gtsam.BetweenFactorPose2(
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2),
ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph)) # print
# 3. Create the data structure to hold the initial_estimate estimate to the
@ -86,11 +87,16 @@ print("Final Result:\n{}".format(result))
# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for i in range(1, 6):
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
print("X{} covariance:\n{}\n".format(i,
marginals.marginalCovariance(i)))
fig = plt.figure(0)
for i in range(1, 6):
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()
if __name__ == "__main__":
main()

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

@ -12,32 +12,38 @@ and does the optimization. Output is written on a file, in g2o format
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import argparse
import math
import numpy as np
import matplotlib.pyplot as plt
import gtsam
import matplotlib.pyplot as plt
from gtsam.utils import plot
def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=float)
def main():
"""Main runner."""
parser = argparse.ArgumentParser(
description="A 2D Pose SLAM example that reads input from g2o, "
"converts it to a factor graph and does the optimization. "
"Output is written on a file, in g2o format")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument('-o', '--output',
parser.add_argument(
'-o',
'--output',
help="the path to the output file with optimized graph")
parser.add_argument('-m', '--maxiter', type=int,
parser.add_argument('-m',
'--maxiter',
type=int,
help="maximum number of iterations for optimizer")
parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'],
default="none", help="Type of kernel used")
parser.add_argument("-p", "--plot", action="store_true",
parser.add_argument('-k',
'--kernel',
choices=['none', 'huber', 'tukey'],
default="none",
help="Type of kernel used")
parser.add_argument("-p",
"--plot",
action="store_true",
help="Flag to plot results")
args = parser.parse_args()
@ -53,7 +59,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(gtsam.Point3(1e-6, 1e-6, 1e-8))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
@ -69,7 +75,6 @@ print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("\nFactor Graph:\n{}".format(graph))
print("\nInitial Estimate:\n{}".format(initial))
@ -86,3 +91,7 @@ if args.plot:
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()
if __name__ == "__main__":
main()

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

@ -8,13 +8,14 @@
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import argparse
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam.utils import plot
from mpl_toolkits.mplot3d import Axes3D
def vector6(x, y, z, a, b, c):
@ -22,13 +23,20 @@ def vector6(x, y, z, a, b, c):
return np.array([x, y, z, a, b, c], dtype=float)
def main():
"""Main runner."""
parser = argparse.ArgumentParser(
description="A 3D Pose SLAM example that reads input from g2o, and "
"initializes Pose3")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument('-o', '--output',
parser.add_argument(
'-o',
'--output',
help="the path to the output file with optimized graph")
parser.add_argument("-p", "--plot", action="store_true",
parser.add_argument("-p",
"--plot",
action="store_true",
help="Flag to plot results")
args = parser.parse_args()
@ -39,15 +47,16 @@ 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,
1e-4, 1e-4, 1e-4))
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()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
@ -69,3 +78,7 @@ if args.plot:
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()
if __name__ == "__main__":
main()

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

@ -13,10 +13,12 @@ Author: Luca Carlone, Frank Dellaert (python port)
from __future__ import print_function
import gtsam
import numpy as np
import gtsam
def main():
"""Main runner."""
# Read graph from file
g2oFile = gtsam.findExampleDataFile("pose3example.txt")
@ -29,7 +31,11 @@ priorModel = gtsam.noiseModel.Diagonal.Variances(
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
# Initializing Pose3 - chordal relaxation"
# Initializing Pose3 - chordal relaxation
initialization = gtsam.InitializePose3.initialize(graph)
print(initialization)
if __name__ == "__main__":
main()