diff --git a/cython/gtsam/examples/GPSFactorExample.py b/cython/gtsam/examples/GPSFactorExample.py new file mode 100644 index 000000000..493a07725 --- /dev/null +++ b/cython/gtsam/examples/GPSFactorExample.py @@ -0,0 +1,56 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Simple robot motion example, with prior and one GPS measurements +Author: Mandy Xie +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + +# ENU Origin is where the plane was in hold next to runway +lat0 = 33.86998 +lon0 = -84.30626 +h0 = 274 + +# Create noise models +GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) +PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25) + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first point, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose3() # prior at origin +graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) + +# Add GPS factors +gps = gtsam.Point3(lat0, lon0, h0) +graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) +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.Pose3()) +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)) + diff --git a/gtsam.h b/gtsam.h index 708753749..070a3c42f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2936,6 +2936,31 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { gtsam::Unit3 bRef() const; }; +#include +virtual class GPSFactor : gtsam::NonlinearFactor{ + GPSFactor(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +virtual class GPSFactor2 : gtsam::NonlinearFactor { + GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor2& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + #include virtual class Scenario { gtsam::Pose3 pose(double t) const;