# How to use the gtsam.LevenbergMarquardtOptimizer function in gtsam

## To help you get started, we’ve selected a few gtsam examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

borglab / gtsam / cython / gtsam / examples / PlanarSLAMExample.py View on Github
``````initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))

# Print
print("Initial Estimate:\n{}".format(initial_estimate))

# Optimize using Levenberg-Marquardt optimization. The optimizer
# accepts an optional set of configuration parameters, controlling
# things like convergence criteria, the type of linear system solver
# to use, and the amount of information displayed during optimization.
# 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)
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)))``````
borglab / gtsam / cython / gtsam / examples / SimpleRotation.py View on Github
``````this structure.
"""
initial = gtsam.Values()
initial.print_('initial estimate')

"""
Step 4: Optimize

After formulating the problem with a graph of constraints
and an initial estimate, executing optimization is as simple
as calling a general optimization function with the graph and
initial estimate.  This will yield a new RotValues structure
with the final state of the optimization.
"""
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
result.print_('final result')
``````
borglab / gtsam / cython / gtsam / utils / visual_isam.py View on Github
``````if not initialEstimates.exists(jj):
initialEstimates.insert(jj, truth.points[j])
if options.pointPriors:  # add point priors
gtsam.PriorFactorPoint3(jj, truth.points[j],
data.noiseModels.pointPrior))

# Add odometry between frames 0 and 1
gtsam.BetweenFactorPose3(
symbol(ord('x'), 0),
symbol(ord('x'), 1), data.odometry[1], data.noiseModels.odometry))

# Update ISAM
if options.batchInitialization:  # Do a full optimize for first two poses
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors,
initialEstimates)
fullyOptimized = batchOptimizer.optimize()
isam.update(newFactors, fullyOptimized)
else:
isam.update(newFactors, initialEstimates)

# figure(1)tic
# t=toc plot(frame_i,t,'r.') tic
result = isam.calculateEstimate()
# t=toc plot(frame_i,t,'g.')

return isam, result, nextPoseIndex``````
borglab / gtsam / python / gtsam_examples / OdometryExample.py View on Github
``````# Create odometry (Between) factors between consecutive poses
graph.print("\nFactor Graph:\n")

# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
initial.print("\nInitial Estimate:\n")

# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
result.print("\nFinal Result:\n")``````
borglab / gtsam / cython / gtsam / examples / OdometryExample.py View on Github
``````# Create odometry (Between) factors between consecutive poses
print("\nFactor Graph:\n{}".format(graph))

# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
print("\nInitial Estimate:\n{}".format(initial))

# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
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)))

fig = plt.figure(0)
for i in range(1, 4):
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()``````
borglab / gtsam / cython / gtsam / examples / ImuFactorExample.py View on Github
``````graph.push_back(factor)
if True:
print(factor)
print(pim.predict(actual_state_i, self.actualBias))
pim.resetIntegration()
actual_state_i = self.scenario.navState(t + self.dt)
i += 1

# add priors on beginning and end

# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
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))))

# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1)``````

## gtsam

Georgia Tech Smoothing And Mapping library

BSD-2-Clause