diff --git a/configure.ac b/configure.ac index 585439326..a71e09e61 100644 --- a/configure.ac +++ b/configure.ac @@ -78,6 +78,17 @@ AC_ARG_ENABLE([spqr], AM_CONDITIONAL([USE_SPQR], [test x$spqr = xtrue]) +# enable profiling +AC_ARG_ENABLE([profiling], + [ --enable-profiling Enable profiling], + [case "${enableval}" in + yes) profiling=true ;; + no) profiling=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-profiling]) ;; + esac],[profiling=false]) + +AM_CONDITIONAL([USE_PROFILING], [test x$profiling = xtrue]) + # Checks for programs. AC_PROG_CXX AC_PROG_CC diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index b92d45fde..65cb1ca04 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -313,74 +313,6 @@ Matrix GaussianFactor::matrix_augmented(const Ordering& ordering, bool weight) c return Ab; } -/* ************************************************************************* */ -std::pair GaussianFactor::combineFactorsAndCreateMatrix( - const vector& factors, - const Ordering& order, const Dimensions& dimensions) { - // find the size of Ab - size_t m = 0, n = 1; - - // number of rows - BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { - m += factor->numberOfRows(); - } - - // find the number of columns - BOOST_FOREACH(const Symbol& key, order) { - n += dimensions.at(key); - } - - // Allocate the new matrix - Matrix Ab = zeros(m,n); - - // Allocate a sigmas vector to make into a full noisemodel - Vector sigmas = ones(m); - - // copy data over - size_t cur_m = 0; - bool constrained = false; - bool unit = true; - BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { - // loop through ordering - size_t cur_n = 0; - BOOST_FOREACH(const Symbol& key, order) { - // copy over matrix if it exists - if (factor->involves(key)) { - insertSub(Ab, factor->get_A(key), cur_m, cur_n); - } - // move onto next element - cur_n += dimensions.at(key); - } - // copy over the RHS - insertColumn(Ab, factor->get_b(), cur_m, n-1); - - // check if the model is unit already - if (!boost::shared_dynamic_cast(factor->get_model())) { - unit = false; - const Vector& subsigmas = factor->get_model()->sigmas(); - subInsert(sigmas, subsigmas, cur_m); - - // check for constraint - if (boost::shared_dynamic_cast(factor->get_model())) - constrained = true; - } - - // move to next row - cur_m += factor->numberOfRows(); - } - - // combine the noisemodels - SharedDiagonal model; - if (unit) { - model = noiseModel::Unit::Create(m); - } else if (constrained) { - model = noiseModel::Constrained::MixedSigmas(sigmas); - } else { - model = noiseModel::Diagonal::Sigmas(sigmas); - } - return make_pair(Ab, model); -} - /* ************************************************************************* */ boost::tuple, list, list > GaussianFactor::sparse(const Dimensions& columnIndices) const { diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 0820b8e32..cd6d4ffac 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -259,17 +259,6 @@ public: */ void append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos); - /** - * Returns the augmented matrix version of a set of factors - * with the corresponding noiseModel - * @param factors is the set of factors to combine - * @param ordering of variables needed for matrix column order - * @return the augmented matrix and a noise model - */ - static std::pair combineFactorsAndCreateMatrix( - const std::vector& factors, - const Ordering& order, const Dimensions& dimensions); - }; // GaussianFactor /* ************************************************************************* */ diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index fbdd0ac01..592426510 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -29,6 +29,8 @@ using namespace boost::assign; // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) +namespace gtsam { + // Explicitly instantiate so we don't have to include everywhere template class FactorGraph; @@ -134,6 +136,75 @@ GaussianFactorGraph::eliminateOne(const Symbol& key, bool old) { #endif } +/* ************************************************************************* */ +template +std::pair combineFactorsAndCreateMatrix( + const Factors& factors, + const Ordering& order, const Dimensions& dimensions) { + // find the size of Ab + size_t m = 0, n = 1; + + // number of rows + BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { + m += factor->numberOfRows(); + } + + // find the number of columns + BOOST_FOREACH(const Symbol& key, order) { + n += dimensions.at(key); + } + + // Allocate the new matrix + Matrix Ab = zeros(m,n); + + // Allocate a sigmas vector to make into a full noisemodel + Vector sigmas = ones(m); + + // copy data over + size_t cur_m = 0; + bool constrained = false; + bool unit = true; + BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) { + // loop through ordering + size_t cur_n = 0; + BOOST_FOREACH(const Symbol& key, order) { + // copy over matrix if it exists + if (factor->involves(key)) { + insertSub(Ab, factor->get_A(key), cur_m, cur_n); + } + // move onto next element + cur_n += dimensions.at(key); + } + // copy over the RHS + insertColumn(Ab, factor->get_b(), cur_m, n-1); + + // check if the model is unit already + if (!boost::shared_dynamic_cast(factor->get_model())) { + unit = false; + const Vector& subsigmas = factor->get_model()->sigmas(); + subInsert(sigmas, subsigmas, cur_m); + + // check for constraint + if (boost::shared_dynamic_cast(factor->get_model())) + constrained = true; + } + + // move to next row + cur_m += factor->numberOfRows(); + } + + // combine the noisemodels + SharedDiagonal model; + if (unit) { + model = noiseModel::Unit::Create(m); + } else if (constrained) { + model = noiseModel::Constrained::MixedSigmas(sigmas); + } else { + model = noiseModel::Diagonal::Sigmas(sigmas); + } + return make_pair(Ab, model); +} + /* ************************************************************************* */ GaussianConditional::shared_ptr GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { @@ -159,8 +230,7 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { // combine the factors to get a noisemodel and a combined matrix Matrix Ab; SharedDiagonal model; - boost::tie(Ab, model) = - GaussianFactor::combineFactorsAndCreateMatrix(factors,render,dimensions); + boost::tie(Ab, model) = combineFactorsAndCreateMatrix(factors,render,dimensions); // eliminate that joint factor GaussianFactor::shared_ptr factor; @@ -187,6 +257,25 @@ GaussianFactorGraph::eliminate(const Ordering& ordering, bool old) return chordalBayesNet; } +/* ************************************************************************* */ +GaussianBayesNet +GaussianFactorGraph::eliminateFrontals(const Ordering& frontals) +{ + Matrix Ab; SharedDiagonal model; + Dimensions dimensions = this->dimensions(); + boost::tie(Ab, model) = combineFactorsAndCreateMatrix(*this, keys(), dimensions); + + // eliminate that joint factor + GaussianFactor::shared_ptr factor; +// GaussianConditional::shared_ptr conditional; + GaussianBayesNet bn; +// boost::tie(bn, factor) = +// GaussianFactor::eliminateMatrix(Ab, model, frontals, dimensions); + + return bn; +} + + /* ************************************************************************* */ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old) { @@ -436,3 +525,11 @@ boost::shared_ptr GaussianFactorGraph::conjugateGradientDescent_( } /* ************************************************************************* */ + +template std::pair combineFactorsAndCreateMatrix >( + const vector& factors, const Ordering& order, const Dimensions& dimensions); + +template std::pair combineFactorsAndCreateMatrix( + const GaussianFactorGraph& factors, const Ordering& order, const Dimensions& dimensions); + +} // namespace gtsam diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index d2ae8d830..26e9ad0b2 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -141,6 +141,11 @@ namespace gtsam { */ GaussianBayesNet eliminate(const Ordering& ordering, bool enableJoinFactor = true); + /** + * Eliminate multiple variables at once, mostly used to eliminate frontal variables + */ + GaussianBayesNet eliminateFrontals(const Ordering& frontals); + /** * optimize a linear factor graph * @param ordering fg in order @@ -261,4 +266,17 @@ namespace gtsam { const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3, size_t maxIterations = 0) const; }; + + /** + * Returns the augmented matrix version of a set of factors + * with the corresponding noiseModel + * @param factors is the set of factors to combine + * @param ordering of variables needed for matrix column order + * @return the augmented matrix and a noise model + */ + template + std::pair combineFactorsAndCreateMatrix( + const Factors& factors, + const Ordering& order, const Dimensions& dimensions); + } // namespace gtsam diff --git a/cpp/JunctionTree-inl.h b/cpp/JunctionTree-inl.h new file mode 100644 index 000000000..c30831d87 --- /dev/null +++ b/cpp/JunctionTree-inl.h @@ -0,0 +1,115 @@ +/* + * JunctionTree-inl.h + * + * Created on: Feb 4, 2010 + * Author: nikai + * Description: the junction tree + */ + +#include +#include +#include +#include "Pose2.h" +#include "BayesTree-inl.h" +#include "SymbolicFactorGraph.h" + +#include "JunctionTree.h" + +#define DEBUG(i) \ + if (verboseLevel>i) cout + +namespace gtsam { + + using namespace std; + + /* ************************************************************************* */ + /** + * Linear JunctionTree + */ + template + void JunctionTree::SubFG::printTree(const string& indent) const { + print(indent); + BOOST_FOREACH(const shared_ptr& child, children_) + child->printTree(indent+" "); + } + + /* ************************************************************************* */ + template + pair::sharedClique> + JunctionTree::eliminateOneClique(sharedSubFG current, BayesTree& bayesTree) { + FG fg; // factor graph will be assembled from local factors and marginalized children + list children; + fg.push_back(*current); // add the local factor graph + BOOST_FOREACH(sharedSubFG& child, current->children_) { + // receive the factors from the child and its clique point + FG fgChild; sharedClique cliqueChild; + boost::tie(fgChild, cliqueChild) = eliminateOneClique(child, bayesTree); + if (!cliqueChild.get()) throw runtime_error("eliminateOneClique: child clique is invalid!"); + + fg.push_back(fgChild); + children.push_back(cliqueChild); + } + + // eliminate the combined factors + // warning: fg is being eliminated in-place and will contain marginal afterwards +// BayesNet bn = fg.eliminate(current->frontal_); + BayesNet bn = fg.eliminateFrontals(current->frontal_); + + // create a new clique corresponding the combined factors + sharedClique new_clique = bayesTree.insert(bn, children); + + return make_pair(fg, new_clique); + } + + /* ************************************************************************* */ + template + BayesTree JunctionTree::eliminate() { + BayesTree bayesTree; + eliminateOneClique(rootFG_, bayesTree); + return bayesTree; + } + + /* ************************************************************************* */ + template + void JunctionTree::iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current) { + if (!current.get()) current = rootFG_; +// iterateBFS(visitor, current); + } + + /* ************************************************************************* */ + template + void JunctionTree::iterSubGraphsBFS(VisitorSubFG visitor) { +// iterateDFS(visitor, rootFG_); + } + + /* ************************************************************************* */ + /** + * Linear JunctionTree + */ + template + void LinearJunctionTree::btreeBackSubstitue(typename BayesTree::sharedClique current, VectorConfig& config) { + // solve the bayes net in the current node + typename BayesNet::const_reverse_iterator it = current->rbegin(); + for (; it!=current->rend(); it++) { + Vector x = (*it)->solve(config); // Solve for that variable + config.insert((*it)->key(),x); // store result in partial solution + } + + // solve the bayes nets in the child nodes + BOOST_FOREACH(sharedClique child, current->children_) { + btreeBackSubstitue(child, config); + } + } + + /* ************************************************************************* */ + template + VectorConfig LinearJunctionTree::optimize() { + // eliminate from leaves to the root + BayesTree bayesTree = JunctionTree::eliminate(); + + VectorConfig result; + btreeBackSubstitue(bayesTree.root(), result); + return result; + } + +} //namespace gtsam diff --git a/cpp/JunctionTree.h b/cpp/JunctionTree.h new file mode 100644 index 000000000..9d12b5700 --- /dev/null +++ b/cpp/JunctionTree.h @@ -0,0 +1,145 @@ +/* + * JunctionTree.h + * + * Created on: Feb 4, 2010 + * Author: nikai + * Description: The junction tree + */ + +#pragma once + +#include +#include +#include +#include "GaussianConditional.h" +#include "GaussianFactorGraph.h" +#include "BayesTree.h" + +namespace gtsam { + + /* ************************************************************************* */ + template + class JunctionTree/*: public BayesTree*/ { + public: + typedef typename BayesTree::sharedClique sharedClique; + + // the threshold for the sizes of submaps. Smaller ones will be absorbed into the separator + static const int const_minNodesPerMap_default = 10; + static const int const_minNodesPerMap_ultra = 1; + + // when to stop partitioning + static const int const_numNodeStopPartition_default = 50; + static const int const_numNodeStopPartition_ultra = 3; // so that A,B,C all have one variable + + + // the class for subgraphs that also include the pointers to the parents and two children + class SubFG : public FG { + public: + typedef typename boost::shared_ptr shared_ptr; + shared_ptr parent_; // the parent subgraph node + Ordering frontal_; // the frontal varaibles + Unordered separator_; // the separator variables + + friend class JunctionTree; + + public: + std::vector children_; // the child cliques + + // empty constructor + SubFG() {} + + // constructor with all the information + SubFG(const FG& fgLocal, const Ordering& frontal, const Unordered& separator, + const shared_ptr& parent) + : frontal_(frontal), separator_(separator), FG(fgLocal), parent_(parent) {} + + // constructor for an empty graph + SubFG(const Ordering& frontal, const Unordered& separator, const shared_ptr& parent) + : frontal_(frontal), separator_(separator), parent_(parent) {} + + const Ordering& frontal() const { return frontal_;} + const Unordered& separator() const { return separator_;} + std::vector& children() { return children_; } // TODO:: add const + + // add a child node + void addChild(const shared_ptr& child) { children_.push_back(child); } + + void printTree(const std::string& indent) const; + }; + + // typedef for shared pointers to cliques + typedef typename SubFG::shared_ptr sharedSubFG; + typedef boost::function VisitorSubFG; + + protected: + // Root clique + sharedSubFG rootFG_; + + private: + + // utility function called by eliminateBottomUp + std::pair eliminateOneClique(sharedSubFG fg_, BayesTree& bayesTree); + + public: + + JunctionTree() : verboseLevel(0) {} + + // return the root graph + sharedSubFG rootFG() const { return rootFG_; } + + // eliminate the factors in the subgraphs + BayesTree eliminate(); + + // print the object + void print(const std::string& str) const { + if (rootFG_.get()) rootFG_->printTree(""); + } + + // iterate over all the subgraphs from root to leaves in the DFS order, recursive + void iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current = sharedSubFG()); + + // iterate over all the subgraphs from root to leaves in the BFS order, non-recursive + void iterSubGraphsBFS(VisitorSubFG visitor); + + // the output level + int verboseLevel; + }; // JunctionTree + + /* ************************************************************************* */ + /** + * Linear JunctionTree which can do optimization + */ + template + class LinearJunctionTree: public JunctionTree { + public: + typedef JunctionTree Base; + typedef typename BayesTree::sharedClique sharedClique; + typedef typename JunctionTree::sharedSubFG sharedSubFG; + + protected: + // back-substitute in topological sort order (parents first) + void btreeBackSubstitue(typename BayesTree::sharedClique current, VectorConfig& config); + + public : + + LinearJunctionTree() : Base() {} + + // constructor + LinearJunctionTree(const FG& fg, const Ordering& ordering, int numNodeStopPartition = Base::const_numNodeStopPartition_default, + int minNodesPerMap = Base::const_minNodesPerMap_default) : + Base(fg, ordering, numNodeStopPartition, minNodesPerMap) {} + + // optimize the linear graph + VectorConfig optimize(); + }; // Linear JunctionTree + + class SymbolicConditional; + class SymbolicFactorGraph; + + /** + * recursive partitioning + */ + typedef JunctionTree SymbolicTSAM; + typedef JunctionTree GaussianTSAM; + +} // namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 9866ee315..f1c07d6ad 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -69,6 +69,7 @@ testSymbolicBayesNet_LDADD = libgtsam.la headers += inference.h inference-inl.h headers += graph.h graph-inl.h headers += FactorGraph.h FactorGraph-inl.h +headers += JunctionTree.h JunctionTree-inl.h headers += BayesNet.h BayesNet-inl.h headers += BayesTree.h BayesTree-inl.h headers += ISAM.h ISAM-inl.h GaussianISAM.h @@ -290,6 +291,7 @@ timeVectorConfig_LDADD = libgtsam.la # create both dynamic and static libraries AM_CXXFLAGS = -I$(boost) -fPIC +AM_LDFLAGS = lib_LTLIBRARIES = libgtsam.la libgtsam_la_SOURCES = $(sources) libgtsam_la_CPPFLAGS = $(AM_CXXFLAGS) @@ -300,11 +302,18 @@ if DEBUG AM_CXXFLAGS += -g endif +if USE_PROFILING +AM_CXXFLAGS += -pg +libgtsam_la_CPPFLAGS += -pg +AM_LDFLAGS += -pg +libgtsam_la_LDFLAGS += -pg +endif + # install the header files include_HEADERS = $(headers) AM_CXXFLAGS += -I.. -AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serialization) +AM_LDFLAGS += -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serialization) # adding cblas implementation - split into a default linux version using the # autotools script, and a mac version that is hardcoded diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index 026c8f01f..95ae38ddc 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -45,6 +45,13 @@ namespace gtsam { return bayesNet; } + /* ************************************************************************* */ + SymbolicBayesNet + SymbolicFactorGraph::eliminateFrontals(const Ordering& ordering) + { + return eliminate(ordering); + } + /* ************************************************************************* */ void saveGraph(const SymbolicFactorGraph& fg, const SymbolicConfig& config, const std::string& s) { diff --git a/cpp/SymbolicFactorGraph.h b/cpp/SymbolicFactorGraph.h index 8210aa0b3..addf48de3 100644 --- a/cpp/SymbolicFactorGraph.h +++ b/cpp/SymbolicFactorGraph.h @@ -81,6 +81,10 @@ namespace gtsam { */ SymbolicBayesNet eliminate(const Ordering& ordering); + /** + * Same as eliminate in the SymbolicFactorGraph case + */ + SymbolicBayesNet eliminateFrontals(const Ordering& ordering); }; // save graph to the graphviz format diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index a18c77c83..8630ae937 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -761,35 +761,6 @@ TEST ( GaussianFactor, constraint_eliminate2 ) GaussianConditional expectedCG("x", d, R, "y", S, zero(2)); CHECK(assert_equal(expectedCG, *actualCG, 1e-4)); } -/* ************************************************************************* */ -TEST ( GaussianFactor, combine_matrix ) { - // create a small linear factor graph - GaussianFactorGraph fg = createGaussianFactorGraph(); - Dimensions dimensions = fg.dimensions(); - - // get two factors from it and insert the factors into a vector - vector lfg; - lfg.push_back(fg[4 - 1]); - lfg.push_back(fg[2 - 1]); - - // combine in a factor - Matrix Ab; SharedDiagonal noise; - Ordering order; order += "x2", "l1", "x1"; - boost::tie(Ab, noise) = GaussianFactor::combineFactorsAndCreateMatrix(lfg, order, dimensions); - - // the expected augmented matrix - Matrix expAb = Matrix_(4, 7, - -5., 0., 5., 0., 0., 0.,-1.0, - +0., -5., 0., 5., 0., 0., 1.5, - 10., 0., 0., 0.,-10., 0., 2.0, - +0., 10., 0., 0., 0.,-10.,-1.0); - - // expected noise model - SharedDiagonal expModel = noiseModel::Unit::Create(4); - - CHECK(assert_equal(expAb, Ab)); - CHECK(assert_equal(*expModel, *noise)); -} /* ************************************************************************* */ TEST ( GaussianFactor, exploding_MAST_factor ) { diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 78f1b8e70..568a91cfb 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -24,8 +24,11 @@ using namespace boost::assign; #include "smallExample.h" #include "GaussianBayesNet.h" #include "numericalDerivative.h" +#include "SymbolicFactorGraph.h" +#include "BayesTree.h" #include "inference-inl.h" // needed for eliminate and marginals + using namespace gtsam; using namespace example; @@ -472,6 +475,38 @@ TEST( GaussianFactorGraph, optimize ) CHECK(assert_equal(expected,actual)); } +/* ************************************************************************* * + Bayes tree for smoother with "nested dissection" ordering: + C1 x5 x6 x4 + C2 x3 x2 : x4 + C3 x1 : x2 + C4 x7 : x6 + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, optimizeMultiFrontal ) +{ + // create a graph + GaussianFactorGraph fg = createSmoother(7); + + // create an ordering + Ordering ordering; + ordering += "x1","x3","x5","x7","x2","x6","x4"; + + // Symbolic factorization + // GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree + SymbolicFactorGraph sfg(fg); + SymbolicBayesNet sbn = sfg.eliminate(ordering); + BayesTree sbt(sbn); + +// // optimize the graph +// VectorConfig actual = fg.optimizeMultiFrontal(sbt); +// +// // verify +// VectorConfig expected = createCorrectDelta(); +// +// CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ TEST( GaussianFactorGraph, combine) { @@ -899,6 +934,36 @@ TEST(GaussianFactorGraph, replace) CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST ( GaussianFactorGraph, combine_matrix ) { + // create a small linear factor graph + GaussianFactorGraph fg = createGaussianFactorGraph(); + Dimensions dimensions = fg.dimensions(); + + // get two factors from it and insert the factors into a vector + vector lfg; + lfg.push_back(fg[4 - 1]); + lfg.push_back(fg[2 - 1]); + + // combine in a factor + Matrix Ab; SharedDiagonal noise; + Ordering order; order += "x2", "l1", "x1"; + boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions); + + // the expected augmented matrix + Matrix expAb = Matrix_(4, 7, + -5., 0., 5., 0., 0., 0.,-1.0, + +0., -5., 0., 5., 0., 0., 1.5, + 10., 0., 0., 0.,-10., 0., 2.0, + +0., 10., 0., 0., 0.,-10.,-1.0); + + // expected noise model + SharedDiagonal expModel = noiseModel::Unit::Create(4); + + CHECK(assert_equal(expAb, Ab)); + CHECK(assert_equal(*expModel, *noise)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 6a5b57942..872240149 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -36,7 +36,7 @@ const double tol = 1e-5; typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ -TEST( NonlinearOptimizer, delta ) +TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta ) { shared_ptr fg(new example::Graph( example::createNonlinearFactorGraph())); @@ -236,6 +236,31 @@ TEST( NonlinearOptimizer, SubgraphSolver ) CHECK(assert_equal(expected, *optimized.config(), 1e-5)); } +/* ************************************************************************* */ +//TEST( NonlinearOptimizer, MultiFrontalSolver ) +//{ +// shared_ptr fg(new example::Graph( +// example::createNonlinearFactorGraph())); +// Optimizer::shared_config initial = example::sharedNoisyConfig(); +// +// Config expected; +// expected.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0)); +// expected.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0)); +// expected.insert(simulated2D::PointKey(1), Point2(0.0, -1.0)); +// +// Optimizer::shared_solver solver; +// +// // Check one ordering +// shared_ptr ord1(new Ordering()); +// *ord1 += "x2","l1","x1"; +// solver = Optimizer::shared_solver(new Optimizer::solver(ord1)); +// Optimizer optimizer1(fg, initial, solver); +// +// Config actual = optimizer1.levenbergMarquardt(); +// CHECK(assert_equal(actual,expected)); +//} + + /* ************************************************************************* */ int main() { TestResult tr;