From 9887d4467c3efe31c4c3c0ea0f63f2f46a1f134b Mon Sep 17 00:00:00 2001
From: alescontrela <alejandroescontrela@gmail.com>
Date: Fri, 10 Apr 2020 21:32:19 -0400
Subject: [PATCH] Replace push_back with emplace_shared. Address PR feedback.

---
 gtsam/nonlinear/NonlinearFactorGraph.h | 12 ++++++++----
 tests/testNonlinearFactorGraph.cpp     | 12 ++++++------
 2 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 29fe12cc1..c9a28dcb8 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -204,7 +204,7 @@ namespace gtsam {
     }
 
     /**
-     * Directly add a PriorFactor to the factor graph.
+     * Convenience method which adds a PriorFactor to the factor graph.
      * @param key    Variable key
      * @param prior  The variable's prior value
      * @param model  Noise model for prior factor
@@ -212,18 +212,22 @@ namespace gtsam {
     template<typename T>
     void addPrior(Key key, const T& prior,
                   const SharedNoiseModel& model = nullptr) {
-      push_back(boost::make_shared<PriorFactor<T>>(key, prior, model));
+      emplace_shared<PriorFactor<T>>(key, prior, model);
     }
 
     /**
-     * Directly add a PriorFactor to the factor graph.
+     * Convenience method which adds a PriorFactor to the factor graph.
      * @param key         Variable key
      * @param prior       The variable's prior value
      * @param covariance  Covariance matrix.
+     * 
+     * Note that the smart noise model associated with the prior factor
+     * automatically picks the right noise model (e.g. a diagonal noise model
+     * if the provided covariance matrix is diagonal).
      */
     template<typename T>
     void addPrior(Key key, const T& prior, const Matrix& covariance) {
-      push_back(boost::make_shared<PriorFactor<T>>(key, prior, covariance));
+      emplace_shared<PriorFactor<T>>(key, prior, covariance);
     }
 
   private:
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 442d03307..d669c8937 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -252,17 +252,17 @@ TEST(testNonlinearFactorGraph, addPrior) {
 
   // Add a prior factor for key k.
   auto model_double = noiseModel::Isotropic::Sigma(1, 1);
-  graph.addPrior<double>(k, 10.0, model_double);
+  graph.addPrior<double>(k, 10, 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);
+  EXPECT_DOUBLES_EQUAL(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));
+  EXPECT(0 != graph.error(values));
 
   // Clear the factor graph and values.
   values.clear();
@@ -272,18 +272,18 @@ TEST(testNonlinearFactorGraph, addPrior) {
   // providing the covariance matrix.
   Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
   covariance_pose3.setIdentity();
-  Pose3 pose{Rot3(), Point3()};
+  Pose3 pose{Rot3(), Point3(0, 0, 0)};
   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);
+  EXPECT_DOUBLES_EQUAL(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));
+  EXPECT(0 != graph.error(values));
 }
 
 /* ************************************************************************* */