diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h index db18f2093..39cda6771 100644 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ b/gtsam/inference/GenericMultifrontalSolver-inl.h @@ -39,6 +39,22 @@ namespace gtsam { structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) { } + /* ************************************************************************* */ + template + void GenericMultifrontalSolver::print(const std::string& s) const { + this->structure_->print(s + " structure:\n"); + this->junctionTree_->print(s + " jtree:"); + } + + /* ************************************************************************* */ + template + bool GenericMultifrontalSolver::equals( + const GenericMultifrontalSolver& expected, double tol) const { + if (!this->structure_->equals(*expected.structure_, tol)) return false; + if (!this->junctionTree_->equals(*expected.junctionTree_, tol)) return false; + return true; + } + /* ************************************************************************* */ template void GenericMultifrontalSolver::replaceFactors(const sharedGraph& graph) { diff --git a/gtsam/inference/GenericMultifrontalSolver.h b/gtsam/inference/GenericMultifrontalSolver.h index a64df38e9..2d50b2ec1 100644 --- a/gtsam/inference/GenericMultifrontalSolver.h +++ b/gtsam/inference/GenericMultifrontalSolver.h @@ -72,7 +72,17 @@ namespace gtsam { GenericMultifrontalSolver(const sharedGraph& factorGraph, const VariableIndex::shared_ptr& variableIndex); - /// @} + /// @} + /// @name Testable + /// @{ + + /** Print to cout */ + void print(const std::string& name = "GenericMultifrontalSolver: ") const; + + /** Test whether is equal to another */ + bool equals(const GenericMultifrontalSolver& other, double tol = 1e-9) const; + + /// @} /// @name Standard Interface /// @{ diff --git a/gtsam/inference/SymbolicMultifrontalSolver.h b/gtsam/inference/SymbolicMultifrontalSolver.h index 454be1d00..802b10d72 100644 --- a/gtsam/inference/SymbolicMultifrontalSolver.h +++ b/gtsam/inference/SymbolicMultifrontalSolver.h @@ -43,6 +43,12 @@ namespace gtsam { SymbolicMultifrontalSolver(const SymbolicFactorGraph::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {}; + /** Print to cout */ + void print(const std::string& name = "SymbolicMultifrontalSolver: ") const { Base::print(name); }; + + /** Test whether is equal to another */ + bool equals(const SymbolicMultifrontalSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); }; + /** * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate.