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
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
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
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:156
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:625
gtsam::GPSFactor
Definition: GPSFactor.h:35


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:38