python
gtsam
examples
GPSFactorExample.py
Go to the documentation of this file.
1
"""
2
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3
Atlanta, Georgia 30332-0415
4
All Rights Reserved
5
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6
7
See LICENSE for the license information
8
9
Simple robot motion example, with prior and one GPS measurements
10
Author: Mandy Xie
11
"""
12
# pylint: disable=invalid-name, E1101
13
14
from
__future__
import
print_function
15
16
import
gtsam
17
18
# ENU Origin is where the plane was in hold next to runway
19
lat0 = 33.86998
20
lon0 = -84.30626
21
h0 = 274
22
23
# Create noise models
24
GPS_NOISE =
gtsam.noiseModel.Isotropic.Sigma
(3, 0.1)
25
PRIOR_NOISE =
gtsam.noiseModel.Isotropic.Sigma
(6, 0.25)
26
27
28
def
main
():
29
"""Main runner."""
30
# Create an empty nonlinear factor graph
31
graph =
gtsam.NonlinearFactorGraph
()
32
33
# Add a prior on the first point, setting it to the origin
34
# A prior factor consists of a mean and a noise model (covariance matrix)
35
priorMean =
gtsam.Pose3
()
# prior at origin
36
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
37
38
# Add GPS factors
39
gps =
gtsam.Point3
(lat0, lon0, h0)
40
graph.add(
gtsam.GPSFactor
(1, gps, GPS_NOISE))
41
print
(
"\nFactor Graph:\n{}"
.
format
(graph))
42
43
# Create the data structure to hold the initialEstimate estimate to the solution
44
# For illustrative purposes, these have been deliberately set to incorrect values
45
initial =
gtsam.Values
()
46
initial.insert(1,
gtsam.Pose3
())
47
print
(
"\nInitial Estimate:\n{}"
.
format
(initial))
48
49
# optimize using Levenberg-Marquardt optimization
50
params =
gtsam.LevenbergMarquardtParams
()
51
optimizer =
gtsam.LevenbergMarquardtOptimizer
(graph, initial, params)
52
result = optimizer.optimize()
53
print
(
"\nFinal Result:\n{}"
.
format
(result))
54
55
56
if
__name__ ==
"__main__"
:
57
main
()
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
Definition:
openglsupport.cpp:226
gtsam.examples.GPSFactorExample.main
def main()
Definition:
GPSFactorExample.py:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition:
Matrix.cpp:155
gtsam::Pose3
Definition:
Pose3.h:37
gtsam::NonlinearFactorGraph
Definition:
NonlinearFactorGraph.h:55
gtsam::LevenbergMarquardtOptimizer
Definition:
LevenbergMarquardtOptimizer.h:35
gtsam::Values
Definition:
Values.h:65
gtsam::LevenbergMarquardtParams
Definition:
LevenbergMarquardtParams.h:35
gtsam::Point3
Vector3 Point3
Definition:
Point3.h:38
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition:
NoiseModel.cpp:624
gtsam::GPSFactor
Definition:
GPSFactor.h:35
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:40