-
Notifications
You must be signed in to change notification settings - Fork 793
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #1808 from borglab/python-localization-example
Showing
6 changed files
with
91 additions
and
5 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,68 @@ | ||
""" | ||
A simple 2D pose slam example with "GPS" measurements | ||
- The robot moves forward 2 meter each iteration | ||
- The robot initially faces along the X axis (horizontal, to the right in 2D) | ||
- We have full odometry between pose | ||
- We have "GPS-like" measurements implemented with a custom factor | ||
""" | ||
import numpy as np | ||
|
||
import gtsam | ||
from gtsam import BetweenFactorPose2, Pose2, noiseModel | ||
from gtsam_unstable import PartialPriorFactorPose2 | ||
|
||
|
||
def main(): | ||
# 1. Create a factor graph container and add factors to it. | ||
graph = gtsam.NonlinearFactorGraph() | ||
|
||
# 2a. Add odometry factors | ||
# For simplicity, we will use the same noise model for each odometry factor | ||
odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1])) | ||
|
||
# Create odometry (Between) factors between consecutive poses | ||
graph.push_back( | ||
BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) | ||
graph.push_back( | ||
BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)) | ||
|
||
# 2b. Add "GPS-like" measurements | ||
# We will use PartialPrior factor for this. | ||
unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1, | ||
0.1])) # 10cm std on x,y | ||
|
||
graph.push_back( | ||
PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise)) | ||
graph.push_back( | ||
PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise)) | ||
graph.push_back( | ||
PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise)) | ||
graph.print("\nFactor Graph:\n") | ||
|
||
# 3. Create the data structure to hold the initialEstimate estimate to the solution | ||
# For illustrative purposes, these have been deliberately set to incorrect values | ||
initialEstimate = gtsam.Values() | ||
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) | ||
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) | ||
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)) | ||
initialEstimate.print("\nInitial Estimate:\n") | ||
|
||
# 4. 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. | ||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) | ||
result = optimizer.optimize() | ||
result.print("Final Result:\n") | ||
|
||
# 5. Calculate and print marginal covariances for all variables | ||
marginals = gtsam.Marginals(graph, result) | ||
print("x1 covariance:\n", marginals.marginalCovariance(1)) | ||
print("x2 covariance:\n", marginals.marginalCovariance(2)) | ||
print("x3 covariance:\n", marginals.marginalCovariance(3)) | ||
|
||
|
||
if __name__ == "__main__": | ||
main() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters