From b266287cca3511b15dd6c2e6af50bd62e0750351 Mon Sep 17 00:00:00 2001
From: alescontrela <alejandroescontrela@gmail.com>
Date: Fri, 10 Apr 2020 13:01:26 -0400
Subject: [PATCH] Add addPrior method to NonlinearFactorGraph and corresponding
 unit test.

---
 gtsam/nonlinear/NonlinearFactorGraph.h | 24 ++++++++++++++
 tests/testNonlinearFactorGraph.cpp     | 44 ++++++++++++++++++++++++++
 2 files changed, 68 insertions(+)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 902a47a0f..29fe12cc1 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/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:
 
     /**
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 6a7a963bc..442d03307 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/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); }
 /* ************************************************************************* */