37 lines
1.5 KiB
Python
37 lines
1.5 KiB
Python
#!/usr/bin/env python
|
|
from __future__ import print_function
|
|
import gtsam
|
|
import numpy as np
|
|
|
|
# Create an empty nonlinear factor graph
|
|
graph = gtsam.NonlinearFactorGraph()
|
|
|
|
# Add a prior on the first pose, setting it to the origin
|
|
# A prior factor consists of a mean and a noise model (covariance matrix)
|
|
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]))
|
|
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
|
|
|
|
# Add odometry factors
|
|
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
|
# For simplicity, we will use the same noise model for each odometry factor
|
|
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
|
# Create odometry (Between) factors between consecutive poses
|
|
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
|
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
|
|
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")
|