diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 95995a4e6..6881e9a69 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -315,6 +315,20 @@ namespace gtsam { root_ = new_clique; } + /* ************************************************************************* */ + template + BayesTree::BayesTree(const This& other) { + *this = other; + } + + /* ************************************************************************* */ + template + BayesTree& BayesTree::operator=(const This& other) { + this->clear(); + other.cloneTo(*this); + return *this; + } + /* ************************************************************************* */ template void BayesTree::print(const string& s) const { @@ -568,7 +582,16 @@ namespace gtsam { } } -/* ************************************************************************* */ + /* ************************************************************************* */ + template + void BayesTree::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 diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 06a47ba8e..3df697a48 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -54,6 +54,7 @@ namespace gtsam { public: + typedef BayesTree This; typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr > 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& 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& children, bool isRootClique = false); - ///TODO: comment - void cloneTo(shared_ptr& newTree) const { + private: + + /** deep copy to another tree */ + void cloneTo(This& newTree) const { cloneTo(newTree, root(), sharedClique()); } - private: - - /** 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); - } - } + /** deep copy to another tree */ + void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const; /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 82cec6f76..194b616b7 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -134,7 +134,9 @@ namespace gtsam { */ static derived_ptr Create(const std::pair >& result) { return boost::make_shared(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 diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index b0f05a3ee..6bb74dd8a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/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(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(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 ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 3a8d2d3bf..5e7adc19d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/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 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& 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->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. * diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f30574d0b..5ff24070c 100644 --- a/tests/testGaussianISAM2.cpp +++ b/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 - using planarSLAM::PoseKey; - using planarSLAM::PointKey; + ISAM2 clone1; - // 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); + ISAM2 isam = createSlamlikeISAM2(); + clone1 = isam; - Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + ISAM2 clone2(isam); - isam.update(newfactors, init); + // Modify original isam + NonlinearFactorGraph factors; + factors.add(BetweenFactor(Symbol('x',0), Symbol('x',10), + isam.calculateEstimate(Symbol('x',0)).between(isam.calculateEstimate(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 - 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; - } - - // CLONING... - boost::shared_ptr isam2 - = boost::shared_ptr(new ISAM2()); - isam.cloneTo(isam2); - - CHECK(assert_equal(isam, *isam2)); + CHECK(assert_equal(createSlamlikeISAM2(), clone1)); + CHECK(assert_equal(clone1, temp)); } /* ************************************************************************* */