Merge pull request #155 from borglab/feature/exposeGPSFactor

expose GPSFactor in python wrapper, and add example for it

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))
+

gtsam.h

@@ -2936,6 +2936,31 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
   gtsam::Unit3 bRef() const;
 };
 
+#include <gtsam/navigation/GPSFactor.h>
+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 <gtsam/navigation/Scenario.h>
 virtual class Scenario {
   gtsam::Pose3 pose(double t) const;