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

Update more to match cython

release/4.3a0
Fan Jiang 2020-07-27 15:16:26 -04:00
parent 7b4266ed6b
commit bc95b41efc
4 changed files with 12 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
assert args.kernel == "none", "Supplied kernel type is not yet implemented" assert args.kernel == "none", "Supplied kernel type is not yet implemented"
# Add prior on the pose having index (key) = 0 # Add prior on the pose having index (key) = 0
graphWithPrior = graph
priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) 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 = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") params.setVerbosity("Termination")
params.setMaxIterations(maxIterations) params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5) # parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ... # Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
# ... and optimize # ... and optimize
result = optimizer.optimize() result = optimizer.optimize()

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

@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4)) 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ") print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys()[0] firstKey = initial.keys()[0]
graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams() 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(graphWithPrior, initial, params) optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize() result = optimizer.optimize()
print("Optimization complete") print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial)) print("initial error = ", graph.error(initial))
print("final error = ", graphWithPrior.error(result)) print("final error = ", graph.error(result))
if args.output is None: if args.output is None:
print("Final Result:\n{}".format(result)) print("Final Result:\n{}".format(result))

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

@ -12,7 +12,7 @@ a single variable with a single factor.
import numpy as np import numpy as np
import gtsam import gtsam
from gtsam.gtsam.symbol_shorthand import X
def main(): def main():
""" """
@ -33,7 +33,7 @@ def main():
prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
prior.print_('goal angle') 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 = gtsam.symbol('x', 1) key = X(1)
factor = gtsam.PriorFactorRot2(key, prior, model) factor = gtsam.PriorFactorRot2(key, prior, model)
""" """

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

@ -13,23 +13,13 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
from __future__ import print_function 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
import gtsam.utils.plot as gtsam_plot 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 from gtsam.examples import SFMdata
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def X(i):
"""Create key for pose i."""
return int(gtsam.symbol('x', i))
def L(j):
"""Create key for landmark j."""
return int(gtsam.symbol('l', j))
def visual_ISAM2_plot(result): def visual_ISAM2_plot(result):