Method to add a factor without having to do shared factor dance

cpp/Pose2Graph.h

@@ -7,20 +7,16 @@
 
 #pragma once
 
-
 #include "NonlinearFactorGraph.h"
-#include "FactorGraph.h"
 #include "Pose2Factor.h"
 #include "Pose2Config.h"
-#include "Testable.h"
-#include "Ordering.h"
 
 namespace gtsam{
 
 /**
  * Non-linear factor graph for visual SLAM
  */
-class Pose2Graph : public gtsam::NonlinearFactorGraph<Pose2Config>{
+class Pose2Graph : public gtsam::NonlinearFactorGraph<Pose2Config> {
 
 public:
 
@@ -32,6 +28,13 @@ public:
    */
   bool equals(const Pose2Graph& p, double tol=1e-9) const;
 
+  /**
+   * Add a factor without having to do shared factor dance
+   */
+  inline void add(const std::string& key1, const std::string& key2,
+			const Pose2& measured, const Matrix& covariance) {
+    push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
+  }
 
 private:
 	/** Serialization function */