Merge pull request #338 from borglab/feature/add-prior-wrapper

Add addPrior to wrapper

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

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;