Fixed deep copy in iSAM2 and added unit test to reproduce problem

gtsam/inference/BayesTree-inl.h

@@ -315,6 +315,20 @@ namespace gtsam {
 		root_ = new_clique;
 	}
 
+  /* ************************************************************************* */
+  template<class CONDITIONAL, class CLIQUE>
+  BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) {
+    *this = other;
+  }
+
+  /* ************************************************************************* */
+  template<class CONDITIONAL, class CLIQUE>
+  BayesTree<CONDITIONAL,CLIQUE>& BayesTree<CONDITIONAL,CLIQUE>::operator=(const This& other) {
+    this->clear();
+    other.cloneTo(*this);
+    return *this;
+  }
+
 	/* ************************************************************************* */
 	template<class CONDITIONAL, class CLIQUE>
 	void BayesTree<CONDITIONAL,CLIQUE>::print(const string& s) const {
@@ -569,6 +583,15 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+  template<class CONDITIONAL, class CLIQUE>
+	void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(
+	    This& newTree, const sharedClique& subtree, const sharedClique& parent) const {
+    sharedClique newClique(subtree->clone());
+    newTree.addClique(newClique, parent);
+    BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
+      cloneTo(newTree, childClique, newClique);
+    }
+  }
 
 }
 /// namespace gtsam

gtsam/inference/BayesTree.h

@@ -54,6 +54,7 @@ namespace gtsam {
 
 	public:
 
+	  typedef BayesTree<CONDITIONAL, CLIQUE> This;
 	  typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
 		typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
 		typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
@@ -146,6 +147,12 @@ namespace gtsam {
 		/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
 		BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
 
+		/** Copy constructor */
+		BayesTree(const This& other);
+
+		/** Assignment operator */
+		This& operator=(const This& other);
+
 		/// @}
 		/// @name Advanced Constructors
 		/// @{
@@ -273,21 +280,15 @@ namespace gtsam {
 		sharedClique insert(const sharedConditional& clique,
 				std::list<sharedClique>& children, bool isRootClique = false);
 
-		///TODO: comment
+  private:
-		void cloneTo(shared_ptr& newTree) const {
+
+    /** deep copy to another tree */
+		void cloneTo(This& newTree) const {
 		  cloneTo(newTree, root(), sharedClique());
 		}
 
-  private:
+		/** deep copy to another tree */
-
+		void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
-		/** deep copy from another tree */
-		void cloneTo(shared_ptr& newTree, const sharedClique& subtree, const sharedClique& parent) const {
-		  sharedClique newClique(subtree->clone());
-		  newTree->addClique(newClique, parent);
-		  BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
-		    cloneTo(newTree, childClique, newClique);
-		  }
-		}
 
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/inference/BayesTreeCliqueBase.h

@@ -134,7 +134,9 @@ namespace gtsam {
      */
      static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
 
-     ///TODO: comment
+     /** Returns a new clique containing a copy of the conditional but without
+      * the parent and child clique pointers.
+      */
      derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); }
 
     /** Permute the variables in the whole subtree rooted at this clique */
@@ -165,6 +167,17 @@ namespace gtsam {
     void assertInvariants() const;
 
   private:
+
+    /** Cliques cannot be copied except by the clone() method, which does not
+     * copy the parent and child pointers.
+     */
+    BayesTreeCliqueBase(const This& other) { assert(false); }
+
+    /** Cliques cannot be copied except by the clone() method, which does not
+     * copy the parent and child pointers.
+     */
+    This& operator=(const This& other) { assert(false); return *this; }
+
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>

gtsam/nonlinear/ISAM2.cpp

@@ -42,7 +42,6 @@ static const double batchThreshold = 0.65;
 ISAM2::ISAM2(const ISAM2Params& params):
     delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
     deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
-  // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
 }
@@ -51,11 +50,51 @@ ISAM2::ISAM2(const ISAM2Params& params):
 ISAM2::ISAM2():
     delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
     deltaDoglegUptodate_(true), deltaUptodate_(true) {
-  // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
 }
 
+/* ************************************************************************* */
+ISAM2::ISAM2(const ISAM2& other):
+    delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) {
+  *this = other;
+}
+
+/* ************************************************************************* */
+ISAM2& ISAM2::operator=(const ISAM2& rhs) {
+  // Copy BayesTree
+  this->Base::operator=(rhs);
+
+  // Copy our variables
+  // When we have Permuted<...>, it is only necessary to copy this permuted
+  // view and not the original, because copying the permuted view automatically
+  // copies the original.
+  theta_ = rhs.theta_;
+  variableIndex_ = rhs.variableIndex_;
+  delta_ = rhs.delta_;
+  deltaNewton_ = rhs.deltaNewton_;
+  RgProd_ = rhs.RgProd_;
+  deltaDoglegUptodate_ = rhs.deltaDoglegUptodate_;
+  deltaUptodate_ = rhs.deltaUptodate_;
+  deltaReplacedMask_ = rhs.deltaReplacedMask_;
+  nonlinearFactors_ = rhs.nonlinearFactors_;
+  ordering_ = rhs.ordering_;
+  params_ = rhs.params_;
+  doglegDelta_ = rhs.doglegDelta_;
+
+#ifndef NDEBUG
+  lastRelinVariables_ = rhs.lastRelinVariables_;
+#endif
+  lastAffectedVariableCount = rhs.lastAffectedVariableCount;
+  lastAffectedFactorCount = rhs.lastAffectedFactorCount;
+  lastAffectedCliqueCount = rhs.lastAffectedCliqueCount;
+  lastAffectedMarkedCount = rhs.lastAffectedMarkedCount;
+  lastBacksubVariableCount = rhs.lastBacksubVariableCount;
+  lastNnzTop = rhs.lastNnzTop;
+
+  return *this;
+}
+
 /* ************************************************************************* */
 FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
   static const bool debug = false;

gtsam/nonlinear/ISAM2.h

@@ -329,6 +329,10 @@ protected:
    * delta will always be updated if necessary when requested with getDelta()
    * or calculateEstimate().
    *
+   * This does not need to be permuted because any change in variable ordering
+   * that would cause a permutation will also mark variables as needing to be
+   * updated in this mask.
+   *
    * This is \c mutable because it is used internally to not update delta_
    * until it is needed.
    */
@@ -356,6 +360,7 @@ private:
 
 public:
 
+  typedef ISAM2 This; ///< This class
   typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
 
   /** Create an empty ISAM2 instance */
@@ -364,39 +369,16 @@ public:
   /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
   ISAM2();
 
+  /** Copy constructor */
+  ISAM2(const ISAM2& other);
+
+  /** Assignment operator */
+  ISAM2& operator=(const ISAM2& rhs);
+
   typedef Base::Clique Clique; ///< A clique
   typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique
   typedef Base::Cliques Cliques; ///< List of Clique typedef from base class
 
-  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->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_;
-    newISAM2->deltaNewton_ = deltaNewton_;
-    newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_;
-    newISAM2->RgProd_ = RgProd_;
-    newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_;
-    newISAM2->deltaUptodate_ = deltaUptodate_;
-    newISAM2->deltaReplacedMask_ = deltaReplacedMask_;
-    newISAM2->nonlinearFactors_ = nonlinearFactors_;
-    newISAM2->ordering_ = ordering_;
-    newISAM2->params_ = params_;
-    newISAM2->doglegDelta_ = doglegDelta_;
-#ifndef NDEBUG
-    newISAM2->lastRelinVariables_ = lastRelinVariables_;
-#endif
-    newISAM2->lastAffectedVariableCount = lastAffectedVariableCount;
-    newISAM2->lastAffectedFactorCount = lastAffectedFactorCount;
-    newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount;
-    newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount;
-    newISAM2->lastBacksubVariableCount = lastBacksubVariableCount;
-    newISAM2->lastNnzTop = lastNnzTop;
-  }
-
   /**
    * Add new factors, updating the solution and relinearizing as needed.
    *

tests/testGaussianISAM2.cpp

@@ -28,6 +28,102 @@ using boost::shared_ptr;
 
 const double tol = 1e-4;
 
+ISAM2 createSlamlikeISAM2() {
+
+  // Pose and landmark key types from planarSLAM
+  using planarSLAM::PoseKey;
+  using planarSLAM::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
+  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
+  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);
+
+    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);
+  }
+
+  // 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);
+
+    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);
+
+    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);
+
+    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);
+
+    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;
+  }
+
+  return isam;
+}
+
 /* ************************************************************************* */
 TEST_UNSAFE(ISAM2, AddVariables) {
 
@@ -778,105 +874,30 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
 /* ************************************************************************* */
 TEST(ISAM2, clone) {
 
-  // Pose and landmark key types from planarSLAM
+  ISAM2 clone1;
-  using planarSLAM::PoseKey;
-  using planarSLAM::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
-  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
-  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;
+    ISAM2 isam = createSlamlikeISAM2();
-    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    clone1 = isam;
-    fullgraph.push_back(newfactors);
 
-    Values init;
+    ISAM2 clone2(isam);
-    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);
+    // Modify original isam
+    NonlinearFactorGraph factors;
+    factors.add(BetweenFactor<Pose2>(Symbol('x',0), Symbol('x',10),
+        isam.calculateEstimate<Pose2>(Symbol('x',0)).between(isam.calculateEstimate<Pose2>(Symbol('x',10))), sharedUnit(3)));
+    isam.update(factors);
+
+    CHECK(assert_equal(createSlamlikeISAM2(), clone2));
   }
 
-  EXPECT(isam_check(fullgraph, fullinit, isam));
+  // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced
+  // if the references in the iSAM2 copy point to the old instance which deleted at
+  // the end of the {...} section above.
+  ISAM2 temp = createSlamlikeISAM2();
 
-  // Add odometry from time 0 to time 5
+  CHECK(assert_equal(createSlamlikeISAM2(), clone1));
-  for( ; i<5; ++i) {
+  CHECK(assert_equal(clone1, temp));
-    planarSLAM::Graph newfactors;
-    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
-    fullgraph.push_back(newfactors);
-
-    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);
-
-    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);
-
-    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);
-
-    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<ISAM2 > isam2
-      = boost::shared_ptr<ISAM2 >(new ISAM2());
-  isam.cloneTo(isam2);
-
-  CHECK(assert_equal(isam, *isam2));
 }
 
 /* ************************************************************************* */