Add addPrior method to NonlinearFactorGraph and corresponding unit test.

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -24,6 +24,7 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/inference/FactorGraph.h>
+#include <gtsam/slam/PriorFactor.h>
 
 #include <boost/shared_ptr.hpp>
 #include <functional>
@@ -202,6 +203,29 @@ namespace gtsam {
       push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
     }
 
+    /**
+     * Directly add a PriorFactor to the factor graph.
+     * @param key    Variable key
+     * @param prior  The variable's prior value
+     * @param model  Noise model for prior factor
+     */
+    template<typename T>
+    void addPrior(Key key, const T& prior,
+                  const SharedNoiseModel& model = nullptr) {
+      push_back(boost::make_shared<PriorFactor<T>>(key, prior, model));
+    }
+
+    /**
+     * Directly add a PriorFactor to the factor graph.
+     * @param key         Variable key
+     * @param prior       The variable's prior value
+     * @param covariance  Covariance matrix.
+     */
+    template<typename T>
+    void addPrior(Key key, const T& prior, const Matrix& covariance) {
+      push_back(boost::make_shared<PriorFactor<T>>(key, prior, covariance));
+    }
+
   private:
 
     /**

tests/testNonlinearFactorGraph.cpp

@@ -25,6 +25,7 @@
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
@@ -242,6 +243,49 @@ TEST(testNonlinearFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(4, bn->size());
 }
 
+/* ************************************************************************* */
+TEST(testNonlinearFactorGraph, addPrior) {
+  Key k(0);
+
+  // Factor graph.
+  auto graph = NonlinearFactorGraph();
+
+  // Add a prior factor for key k.
+  auto model_double = noiseModel::Isotropic::Sigma(1, 1);
+  graph.addPrior<double>(k, 10.0, model_double);
+
+  // Assert the graph has 0 error with the correct values.
+  Values values;
+  values.insert(k, 10.0);
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(values), 1e-16);
+
+  // Assert the graph has some error with incorrect values.
+  values.clear();
+  values.insert(k, 11.0);
+  EXPECT(0.0 != graph.error(values));
+
+  // Clear the factor graph and values.
+  values.clear();
+  graph.erase(graph.begin(), graph.end());
+
+  // Add a Pose3 prior to the factor graph. Use a gaussian noise model by
+  // providing the covariance matrix.
+  Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
+  covariance_pose3.setIdentity();
+  Pose3 pose{Rot3(), Point3()};
+  graph.addPrior<Pose3>(k, pose, covariance_pose3);
+
+  // Assert the graph has 0 error with the correct values.
+  values.insert(k, pose);
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(values), 1e-16);
+
+  // Assert the graph has some error with incorrect values.
+  values.clear();
+  Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)};
+  values.insert(k, pose_incorrect);
+  EXPECT(0.0 != graph.error(values));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */