Update more to match cython
parent
7b4266ed6b
commit
bc95b41efc
|
@ -53,16 +53,15 @@ 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
|
||||
graphWithPrior = graph
|
||||
priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
|
||||
graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
|
||||
graph.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(graphWithPrior, initial, params)
|
||||
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
|
||||
# ... and optimize
|
||||
result = optimizer.optimize()
|
||||
|
||||
|
|
|
@ -43,18 +43,17 @@ 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()[0]
|
||||
graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
|
||||
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
|
||||
|
||||
params = gtsam.GaussNewtonParams()
|
||||
params.setVerbosity("Termination") # this will show info about stopping conds
|
||||
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
|
||||
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
|
||||
result = optimizer.optimize()
|
||||
print("Optimization complete")
|
||||
|
||||
print("initial error = ", graphWithPrior.error(initial))
|
||||
print("final error = ", graphWithPrior.error(result))
|
||||
print("initial error = ", graph.error(initial))
|
||||
print("final error = ", graph.error(result))
|
||||
|
||||
if args.output is None:
|
||||
print("Final Result:\n{}".format(result))
|
||||
|
|
|
@ -12,7 +12,7 @@ a single variable with a single factor.
|
|||
|
||||
import numpy as np
|
||||
import gtsam
|
||||
|
||||
from gtsam.gtsam.symbol_shorthand import X
|
||||
|
||||
def main():
|
||||
"""
|
||||
|
@ -33,7 +33,7 @@ def main():
|
|||
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
|
||||
prior.print_('goal angle')
|
||||
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
|
||||
key = gtsam.symbol('x', 1)
|
||||
key = X(1)
|
||||
factor = gtsam.PriorFactorRot2(key, prior, model)
|
||||
|
||||
"""
|
||||
|
|
|
@ -13,23 +13,13 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
|
|||
|
||||
from __future__ import print_function
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam.gtsam.symbol_shorthand import L, X
|
||||
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))
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
|
||||
def visual_ISAM2_plot(result):
|
||||
|
|
Loading…
Reference in New Issue