From 9234c7993ca9ec751c2204548335eb6984261c31 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <vagrawal@ihmc.us>
Date: Tue, 2 Jun 2020 14:29:03 -0500
Subject: [PATCH] wrap the addPrior method and added a test for python

---
 cython/gtsam/tests/test_PriorFactor.py | 22 ++++++++++++++++++++++
 gtsam.h                                |  3 +++
 2 files changed, 25 insertions(+)

diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py
index 0acf50c2c..66207b800 100644
--- a/cython/gtsam/tests/test_PriorFactor.py
+++ b/cython/gtsam/tests/test_PriorFactor.py
@@ -35,5 +35,27 @@ class TestPriorFactor(GtsamTestCase):
         values.insert(key, priorVector)
         self.assertEqual(factor.error(values), 0)
 
+    def test_AddPrior(self):
+        """
+        Test adding prior factors directly to factor graph via the .addPrior method.
+        """
+        # define factor graph
+        graph = gtsam.NonlinearFactorGraph()
+
+        # define and add Pose3 prior
+        key = 5
+        priorPose3 = gtsam.Pose3()
+        model = gtsam.noiseModel_Unit.Create(6)
+        graph.addPriorPose3(key, priorPose3, model)
+        self.assertEqual(graph.size(), 1)
+
+        # define and add Vector prior
+        key = 3
+        priorVector = np.array([0., 0., 0.])
+        model = gtsam.noiseModel_Unit.Create(3)
+        graph.addPriorVector(key, priorVector, model)
+        self.assertEqual(graph.size(), 2)
+
+
 if __name__ == "__main__":
     unittest.main()
diff --git a/gtsam.h b/gtsam.h
index 7a1a8864d..94e8baee4 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2050,6 +2050,9 @@ class NonlinearFactorGraph {
   gtsam::KeySet keys() const;
   gtsam::KeyVector keyVector() const;
 
+  template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
+  void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
+
   // NonlinearFactorGraph
   void printErrors(const gtsam::Values& values) const;
   double error(const gtsam::Values& values) const;