diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index ebe59d135..1b762e210 100644 --- a/gtsam/inference/BayesTree.h +++ b/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& 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. diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 421d03ad5..99d717681 100644 --- a/gtsam/inference/Permutation.h +++ b/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 operator=(const Permuted& 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); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 7f4b90748..2d889ee64 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/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 diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index f2de0be4c..88e4364ae 100644 --- a/gtsam/linear/HessianFactor.h +++ b/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( + shared_ptr(new HessianFactor(*this))); + } + /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "") const; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b8f40d6ef..09089df20 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/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( + 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; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index d4d177815..fe29936cb 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -27,12 +27,18 @@ namespace gtsam { */ template > class GaussianISAM2 : public ISAM2 { + typedef ISAM2 Base; public: /** Create an empty ISAM2 instance */ GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ GaussianISAM2() : ISAM2() {} + + void cloneTo(boost::shared_ptr& newGaussianISAM2) const { + boost::shared_ptr isam2 = boost::static_pointer_cast(newGaussianISAM2); + Base::cloneTo(isam2); + } }; // optimize the BayesTree, starting from the root diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 1fd077c3c..50b22f1c5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/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& newISAM2) const { + boost::shared_ptr bayesTree = boost::static_pointer_cast(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::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename BayesTree::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: diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index ad9855fa5..979f7c7c8 100644 --- a/tests/testGaussianISAM2.cpp +++ b/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 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 > isam2 + = boost::shared_ptr >(new GaussianISAM2()); + isam.cloneTo(isam2); + + CHECK(assert_equal(isam, *isam2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */