Replace push_back with emplace_shared. Address PR feedback.

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:

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));
 }
 
 /* ************************************************************************* */