support cloning (deep copy) for ISAM2

gtsam/inference/BayesTree.h

@@ -81,6 +81,20 @@ namespace gtsam {
 
 			Clique(const sharedConditional& conditional);
 
+			void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const {
+			  sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_));
+			  sharedClique newClique = newTree.addClique(newConditional, parent_clique);
+			  if (cachedFactor_)
+			    newClique->cachedFactor_ = cachedFactor_->clone();
+			  else newClique->cachedFactor_ = typename FactorType::shared_ptr();
+			  if (!parent_clique) {
+			    newTree.root_ = newClique;
+			  }
+			  BOOST_FOREACH(const shared_ptr& childClique, children_) {
+			    childClique->cloneToBayesTree(newTree, newClique);
+			  }
+			}
+
 			/** print this node */
 			void print(const std::string& s = "") const;
 
@@ -264,6 +278,11 @@ namespace gtsam {
 		/** check equality */
 		bool equals(const BayesTree<CONDITIONAL>& other, double tol = 1e-9) const;
 
+		/** deep copy from another tree */
+		void cloneTo(shared_ptr& newTree) const {
+		  root_->cloneToBayesTree(*newTree);
+		}
+
 		/**
 		 * Find parent clique of a conditional.  It will look at all parents and
 		 * return the one with the lowest index in the ordering.

gtsam/inference/Permutation.h

@@ -174,6 +174,13 @@ public:
   /** Access the container through the permutation (const version) */
   const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
 
+	/** Assignment operator for cloning in ISAM2 */
+  Permuted<CONTAINER> operator=(const Permuted<CONTAINER>& other) {
+    permutation_ = other.permutation_;
+    container_ = other.container_;
+    return *this;
+  }
+
   /** Permute this view by applying a permutation to the underlying permutation */
   void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }
 

gtsam/linear/GaussianFactor.h

@@ -88,6 +88,9 @@ namespace gtsam {
     /** Return the dimension of the variable pointed to by the given key iterator */
     virtual size_t getDim(const_iterator variable) const = 0;
 
+    /** Clone a factor (make a deep copy) */
+    virtual GaussianFactor::shared_ptr clone() const = 0;
+
     /**
      * Permutes the GaussianFactor, but for efficiency requires the permutation
      * to already be inverted.  This acts just as a change-of-name for each

gtsam/linear/HessianFactor.h

@@ -191,6 +191,12 @@ namespace gtsam {
     /** Destructor */
 		virtual ~HessianFactor() {}
 
+		/** Clone this JacobianFactor */
+		virtual GaussianFactor::shared_ptr clone() const {
+		  return boost::static_pointer_cast<GaussianFactor>(
+		      shared_ptr(new HessianFactor(*this)));
+		}
+
     /** Print the factor for debugging and testing (implementing Testable) */
     virtual void print(const std::string& s = "") const;
 

gtsam/linear/JacobianFactor.h

@@ -136,6 +136,12 @@ namespace gtsam {
 
     virtual ~JacobianFactor() {}
 
+    /** Clone this JacobianFactor */
+    virtual GaussianFactor::shared_ptr clone() const {
+      return boost::static_pointer_cast<GaussianFactor>(
+          shared_ptr(new JacobianFactor(*this)));
+    }
+
     // Implementing Testable interface
     virtual void print(const std::string& s = "") const;
     virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;

gtsam/nonlinear/GaussianISAM2.h

@@ -27,12 +27,18 @@ namespace gtsam {
  */
 template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
 class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
+  typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
 public:
   /** Create an empty ISAM2 instance */
   GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
 
   /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
   GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
+
+  void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
+    boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
+    Base::cloneTo(isam2);
+  }
 };
 
 // optimize the BayesTree, starting from the root

gtsam/nonlinear/ISAM2.h

@@ -167,6 +167,27 @@ public:
   /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
   ISAM2();
 
+  void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
+    boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
+    Base::cloneTo(bayesTree);
+    newISAM2->theta_ = theta_;
+    newISAM2->variableIndex_ = variableIndex_;
+    newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
+    newISAM2->delta_ = delta_;
+    newISAM2->nonlinearFactors_ = nonlinearFactors_;
+    newISAM2->ordering_ = ordering_;
+    newISAM2->params_ = params_;
+#ifndef NDEBUG
+    newISAM2->lastRelinVariables_ = lastRelinVariables_;
+#endif
+    newISAM2->lastAffectedVariableCount = lastAffectedVariableCount;
+    newISAM2->lastAffectedFactorCount = lastAffectedFactorCount;
+    newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount;
+    newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount;
+    newISAM2->lastBacksubVariableCount = lastBacksubVariableCount;
+    newISAM2->lastNnzTop = lastNnzTop;
+  }
+
   typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///< Shared pointer to a clique
   typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///< List of Clique typedef from base class
 
@@ -235,6 +256,8 @@ public:
   size_t lastBacksubVariableCount;
   size_t lastNnzTop;
 
+  ISAM2Params params() const { return params_; }
+
   //@}
 
 private:

tests/testGaussianISAM2.cpp

@@ -292,6 +292,110 @@ TEST(ISAM2, slamlike_solution)
   EXPECT(isam_check(fullgraph, fullinit, isam));
 }
 
+/* ************************************************************************* */
+TEST_UNSAFE(ISAM2, clone) {
+
+  // Pose and landmark key types from planarSLAM
+  typedef planarSLAM::PoseKey PoseKey;
+  typedef planarSLAM::PointKey PointKey;
+
+  // Set up parameters
+  SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
+  SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
+
+  // These variables will be reused and accumulate factors and values
+  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(0.001, 0.0, 0, false, true));
+  planarSLAM::Values fullinit;
+  planarSLAM::Graph fullgraph;
+
+  // i keeps track of the time step
+  size_t i = 0;
+
+  // Add a prior at time 0 and update isam
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+    fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  EXPECT(isam_check(fullgraph, fullinit, isam));
+
+  // Add odometry from time 0 to time 5
+  for( ; i<5; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 5 to 6 and landmark measurement at time 5
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+    fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Add odometry from time 6 to time 10
+  for( ; i<10; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 10 to 11 and landmark measurement at time 10
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // CLONING...
+  boost::shared_ptr<GaussianISAM2<planarSLAM::Values> > isam2
+      = boost::shared_ptr<GaussianISAM2<planarSLAM::Values> >(new GaussianISAM2<planarSLAM::Values>());
+  isam.cloneTo(isam2);
+
+  CHECK(assert_equal(isam, *isam2));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */