diff --git a/.cproject b/.cproject index 95c0f2a96..df6fdc5f5 100644 --- a/.cproject +++ b/.cproject @@ -309,14 +309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +335,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +343,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +391,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +399,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +407,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +423,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +525,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +541,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +581,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +685,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -985,6 +991,7 @@ make + testGraph.run true false @@ -992,6 +999,7 @@ make + testJunctionTree.run true false @@ -999,6 +1007,7 @@ make + testSymbolicBayesNetB.run true false @@ -1028,6 +1037,30 @@ true true + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + make -j5 @@ -1134,6 +1167,7 @@ make + testErrors.run true false @@ -1179,10 +1213,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1267,10 +1301,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1605,7 +1639,6 @@ make - testSimulated2DOriented.run true false @@ -1645,7 +1678,6 @@ make - testSimulated2D.run true false @@ -1653,7 +1685,6 @@ make - testSimulated3D.run true false @@ -1845,7 +1876,6 @@ make - tests/testGaussianISAM2 true false @@ -1867,102 +1897,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j1 @@ -2164,6 +2098,7 @@ cpack + -G DEB true false @@ -2171,6 +2106,7 @@ cpack + -G RPM true false @@ -2178,6 +2114,7 @@ cpack + -G TGZ true false @@ -2185,6 +2122,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2318,34 +2256,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2389,6 +2391,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/CMakeLists.txt b/CMakeLists.txt index 85984d7e0..5c1c5cdab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -161,7 +161,7 @@ if (DOXYGEN_FOUND) add_subdirectory(doc) endif() -# Set up CPack +## Set up CPack set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") @@ -171,12 +171,18 @@ set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") -set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory -set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makestats.sh$") +#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory +#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error +set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") #set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs +# Deb-package specific cpack +set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") + # print configuration variables message(STATUS "===============================================================") message(STATUS "================ Configuration Options ======================") diff --git a/README b/README index 1a992292a..d7486c8b8 100644 --- a/README +++ b/README @@ -59,10 +59,12 @@ Tested compilers - GCC 4.2-4.7 - Clang 2.9-3.2 - OSX GCC 4.2 + - MSVC 2010, 2012 Tested systems: - Ubuntu 11.04, 11.10, 12.04 - MacOS 10.6, 10.7 + - Windows 7 2) GTSAM makes extensive use of debug assertions, and we highly recommend you work @@ -72,8 +74,8 @@ will run up to 10x faster in Release mode! See the end of this document for additional debugging tips. 3) -GTSAM has Doxygen documentation. To generate, run the the script -makedoc.sh. +GTSAM has Doxygen documentation. To generate, run 'make doc' from your +build directory. 4) The instructions below install the library to the default system install path and diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 240361db7..36f2552a1 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -54,13 +54,9 @@ // for each variable, held in a Values container. #include -// ??? -#include #include #include - - using namespace std; using namespace gtsam; @@ -110,14 +106,6 @@ int main(int argc, char** argv) { parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "simple spcg solver final error = " << graph.error(result) << endl; - } - { parameters.iterativeParams = boost::make_shared(); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); diff --git a/gtsam.h b/gtsam.h index 4aa1480df..9c6f76527 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1022,8 +1022,10 @@ class GaussianFactorGraph { // Conversion to matrices Matrix sparseJacobian_() const; - Matrix denseJacobian() const; - Matrix denseHessian() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + Matrix augmentedHessian() const; + pair hessian() const; }; class GaussianISAM { @@ -1045,6 +1047,46 @@ class GaussianSequentialSolver { Matrix marginalCovariance(size_t j) const; }; +#include +virtual class IterativeOptimizationParameters { + string getKernel() const ; + string getVerbosity() const; + void setKernel(string s) ; + void setVerbosity(string s) ; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + size_t getMinIterations() const ; + size_t getMaxIterations() const ; + size_t getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(size_t value); + void setMaxIterations(size_t value); + void setReset(size_t value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + void print(string s) const; +}; + +class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters); + gtsam::VectorValues optimize() const; +}; + #include class KalmanFilter { KalmanFilter(size_t n); @@ -1288,6 +1330,7 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); bool isMultifrontal() const; bool isSequential() const; @@ -1443,6 +1486,8 @@ class ISAM2 { bool equals(const gtsam::ISAM2& other, double tol) const; void print(string s) const; + void printStats() const; + void saveGraph(string s) const; gtsam::ISAM2Result update(); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); diff --git a/gtsam_unstable/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp similarity index 98% rename from gtsam_unstable/base/DSFVector.cpp rename to gtsam/base/DSFVector.cpp index cdea89f34..6d79dcb53 100644 --- a/gtsam_unstable/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include using namespace std; diff --git a/gtsam_unstable/base/DSFVector.h b/gtsam/base/DSFVector.h similarity index 100% rename from gtsam_unstable/base/DSFVector.h rename to gtsam/base/DSFVector.h diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 6902a4a88..e6a40f727 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -67,11 +67,30 @@ inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);} */ Matrix Matrix_(size_t m, size_t n, ...); +// Matlab-like syntax + /** - * MATLAB like constructors + * Creates an zeros matrix, with matlab-like syntax + * + * Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros, + * don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error. */ Matrix zeros(size_t m, size_t n); + +/** + * Creates an identity matrix, with matlab-like syntax + * + * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, + * don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error. + */ Matrix eye(size_t m, size_t n); + +/** + * Creates a square identity matrix, with matlab-like syntax + * + * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, + * don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error. + */ inline Matrix eye( size_t m ) { return eye(m,m); } Matrix diag(const Vector& v); diff --git a/gtsam_unstable/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp similarity index 99% rename from gtsam_unstable/base/tests/testDSFVector.cpp rename to gtsam/base/tests/testDSFVector.cpp index 8997559f5..c0b72f1a0 100644 --- a/gtsam_unstable/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -24,7 +24,7 @@ using namespace boost::assign; #include -#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 97a7fa1c6..04490dbc6 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -100,7 +100,7 @@ namespace gtsam { ++ firstIndex; // Check that number of variables is in bounds - if(firstIndex + nFrontals >= variableIndex.size()) + if(firstIndex + nFrontals > variableIndex.size()) throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph."); // Get set of involved factors diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index b2f9bc3f1..bfbe716d4 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -60,65 +60,6 @@ TEST(FactorGraph, eliminateFrontals) { EXPECT(assert_equal(expectedCond, *actualCond)); } -///* ************************************************************************* */ -// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) -//{ -// SymbolicFactorGraph G; -// G.push_factor("x1", "x2"); -// G.push_factor("x1", "x3"); -// G.push_factor("x1", "x4"); -// G.push_factor("x2", "x3"); -// G.push_factor("x2", "x4"); -// G.push_factor("x3", "x4"); -// -// SymbolicFactorGraph T, C; -// boost::tie(T, C) = G.splitMinimumSpanningTree(); -// -// SymbolicFactorGraph expectedT, expectedC; -// expectedT.push_factor("x1", "x2"); -// expectedT.push_factor("x1", "x3"); -// expectedT.push_factor("x1", "x4"); -// expectedC.push_factor("x2", "x3"); -// expectedC.push_factor("x2", "x4"); -// expectedC.push_factor("x3", "x4"); -// CHECK(assert_equal(expectedT,T)); -// CHECK(assert_equal(expectedC,C)); -//} - -///* ************************************************************************* */ -///** -// * x1 - x2 - x3 - x4 - x5 -// * | | / | -// * l1 l2 l3 -// */ -// SL-FIX TEST( FactorGraph, removeSingletons ) -//{ -// SymbolicFactorGraph G; -// G.push_factor("x1", "x2"); -// G.push_factor("x2", "x3"); -// G.push_factor("x3", "x4"); -// G.push_factor("x4", "x5"); -// G.push_factor("x2", "l1"); -// G.push_factor("x3", "l2"); -// G.push_factor("x4", "l2"); -// G.push_factor("x4", "l3"); -// -// SymbolicFactorGraph singletonGraph; -// set singletons; -// boost::tie(singletonGraph, singletons) = G.removeSingletons(); -// -// set singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3"; -// CHECK(singletons_excepted == singletons); -// -// SymbolicFactorGraph singletonGraph_excepted; -// singletonGraph_excepted.push_factor("x2", "l1"); -// singletonGraph_excepted.push_factor("x4", "l3"); -// singletonGraph_excepted.push_factor("x1", "x2"); -// singletonGraph_excepted.push_factor("x4", "x5"); -// singletonGraph_excepted.push_factor("x2", "x3"); -// CHECK(singletonGraph_excepted.equals(singletonGraph)); -//} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index cf1d31d85..a89d7a10c 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,9 +20,7 @@ namespace gtsam { */ class ConjugateGradientParameters : public IterativeOptimizationParameters { - public: - typedef IterativeOptimizationParameters Base; typedef boost::shared_ptr shared_ptr; @@ -49,7 +47,21 @@ public: inline double epsilon_rel() const { return epsilon_rel_; } inline double epsilon_abs() const { return epsilon_abs_; } - void print() const { + inline size_t getMinIterations() const { return minIterations_; } + inline size_t getMaxIterations() const { return maxIterations_; } + inline size_t getReset() const { return reset_; } + inline double getEpsilon() const { return epsilon_rel_; } + inline double getEpsilon_rel() const { return epsilon_rel_; } + inline double getEpsilon_abs() const { return epsilon_abs_; } + + inline void setMinIterations(size_t value) { minIterations_ = value; } + inline void setMaxIterations(size_t value) { maxIterations_ = value; } + inline void setReset(size_t value) { reset_ = value; } + inline void setEpsilon(double value) { epsilon_rel_ = value; } + inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } + inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } + + virtual void print(const std::string &s="") const { Base::print(); std::cout << "ConjugateGradientParameters: " << "minIter = " << minIterations_ @@ -61,18 +73,4 @@ public: } }; -//class ConjugateGradientSolver : public IterativeSolver { -// -//public: -// -// typedef ConjugateGradientParameters Parameters; -// -// Parameters parameters_; -// -// ConjugateGradientSolver(const ConjugateGradientParameters ¶meters) : parameters_(parameters) {} -// virtual VectorValues::shared_ptr optimize () = 0; -// virtual const IterativeOptimizationParameters& _params() const = 0; -//}; - - } diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 0ddc4a7b3..5182bb914 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include #include @@ -242,12 +242,12 @@ double determinant(const GaussianBayesNet& bayesNet) { /* ************************************************************************* */ VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { - return gradient(FactorGraph(bayesNet), x0); + return gradient(GaussianFactorGraph(bayesNet), x0); } /* ************************************************************************* */ void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { - gradientAtZero(FactorGraph(bayesNet), g); + gradientAtZero(GaussianFactorGraph(bayesNet), g); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index f9ea3fdc8..04cb4a25a 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -18,7 +18,7 @@ */ #include -#include +#include namespace gtsam { diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 02f37915c..222f025a0 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -166,7 +166,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix GaussianFactorGraph::denseJacobian() const { + Matrix GaussianFactorGraph::augmentedJacobian() const { // Convert to Jacobians FactorGraph jfg; jfg.reserve(this->size()); @@ -182,6 +182,14 @@ namespace gtsam { return combined.matrix_augmented(); } + /* ************************************************************************* */ + std::pair GaussianFactorGraph::jacobian() const { + Matrix augmented = augmentedJacobian(); + return make_pair( + augmented.leftCols(augmented.cols()-1), + augmented.col(augmented.cols()-1)); + } + /* ************************************************************************* */ // Helper functions for Combine static boost::tuple, size_t, size_t> countDims(const FactorGraph& factors, const VariableSlots& variableSlots) { @@ -317,9 +325,7 @@ break; } /* ************************************************************************* */ - static - FastMap findScatterAndDims - (const FactorGraph& factors) { + static FastMap findScatterAndDims(const FactorGraph& factors) { const bool debug = ISDEBUG("findScatterAndDims"); @@ -349,7 +355,7 @@ break; } /* ************************************************************************* */ - Matrix GaussianFactorGraph::denseHessian() const { + Matrix GaussianFactorGraph::augmentedHessian() const { Scatter scatter = findScatterAndDims(*this); @@ -367,6 +373,14 @@ break; return result; } + /* ************************************************************************* */ + std::pair GaussianFactorGraph::hessian() const { + Matrix augmented = augmentedHessian(); + return make_pair( + augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), + augmented.col(augmented.rows()-1).head(augmented.rows()-1)); + } + /* ************************************************************************* */ GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactor>& factors, size_t nrFrontals) { @@ -507,4 +521,126 @@ break; } // \EliminatePreferCholesky + + + /* ************************************************************************* */ + static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { + JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); + if( !result ) { + result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + return result; + } + + /* ************************************************************************* */ + Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back((*Ai)*x); + } + return e; + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) { + multiplyInPlace(fg,x,e.begin()); + } + + /* ************************************************************************* */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { + Errors::iterator ei = e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + *ei = (*Ai)*x; + ei++; + } + } + + /* ************************************************************************* */ + // x += alpha*A'*e + void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha,*(ei++),x); + } + } + + /* ************************************************************************* */ + VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) { + // It is crucial for performance to make a zero-valued clone of x + VectorValues g = VectorValues::Zero(x0); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(Ai->error_vector(x0)); + } + transposeMultiplyAdd(fg, 1.0, e, g); + return g; + } + + /* ************************************************************************* */ + void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) { + // Zero-out the gradient + g.setZero(); + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(-Ai->getb()); + } + transposeMultiplyAdd(fg, 1.0, e, g); + } + + /* ************************************************************************* */ + void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + Index i = 0 ; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + r[i] = Ai->getb(); + i++; + } + VectorValues Ax = VectorValues::SameStructure(r); + multiply(fg,x,Ax); + axpy(-1.0,Ax,r); + } + + /* ************************************************************************* */ + void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { + r.vector() = Vector::Zero(r.dim()); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + SubVector &y = r[i]; + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + y += Ai->getA(j) * x[*j]; + } + ++i; + } + } + + /* ************************************************************************* */ + void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { + x.vector() = Vector::Zero(x.dim()); + Index i = 0; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + x[*j] += Ai->getA(j).transpose() * r[i]; + } + ++i; + } + } + + /* ************************************************************************* */ + boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) { + boost::shared_ptr e(new Errors); + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e->push_back(Ai->error_vector(x)); + } + return e; + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 51748b79b..5e6e792be 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -31,24 +31,6 @@ namespace gtsam { - /** return A*x-b - * \todo Make this a member function - affects SubgraphPreconditioner */ - template - Errors gaussianErrors(const FactorGraph& fg, const VectorValues& x) { - return *gaussianErrors_(fg, x); - } - - /** shared pointer version - * \todo Make this a member function - affects SubgraphPreconditioner */ - template - boost::shared_ptr gaussianErrors_(const FactorGraph& fg, const VectorValues& x) { - boost::shared_ptr e(new Errors); - BOOST_FOREACH(const typename boost::shared_ptr& factor, fg) { - e->push_back(factor->error_vector(x)); - } - return e; - } - /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == GaussianFactor @@ -188,15 +170,43 @@ namespace gtsam { Matrix sparseJacobian_() const; /** - * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b - * with standard deviations are baked into A and b + * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ + * Jacobian matrix, augmented with b with the noise models baked + * into A and b. The negative log-likelihood is + * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. */ - Matrix denseJacobian() const; + Matrix augmentedJacobian() const; + + /** + * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, + * with the noise models baked into A and b. The negative log-likelihood + * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::augmentedJacobian and + * GaussianFactorGraph::sparseJacobian. + */ + std::pair jacobian() const; /** - * Return a dense \f$ n \times n \f$ Hessian matrix, augmented with \f$ A^T b \f$ + * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian + * matrix, augmented with the information vector \f$ \eta \f$. The + * augmented Hessian is + \f[ \left[ \begin{array}{ccc} + \Lambda & \eta \\ + \eta^T & c + \end{array} \right] \f] + and the negative log-likelihood is + \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. */ - Matrix denseHessian() const; + Matrix augmentedHessian() const; + + /** + * Return the dense Hessian \f$ \Lambda \f$ and information vector + * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood + * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also + * GaussianFactorGraph::augmentedHessian. + */ + std::pair hessian() const; private: /** Serialization function */ @@ -294,4 +304,54 @@ namespace gtsam { GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactor>& factors, size_t nrFrontals = 1); + /****** Linear Algebra Opeations ******/ + + /** return A*x */ + Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x); + + /** In-place version e <- A*x that overwrites e. */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e); + + /** In-place version e <- A*x that takes an iterator. */ + void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); + + /** x += alpha*A'*e */ + void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ + VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around zero. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g); + + /* matrix-vector operations */ + void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); + void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x); + + /** shared pointer version + * \todo Make this a member function - affects SubgraphPreconditioner */ + boost::shared_ptr gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x); + + /** return A*x-b + * \todo Make this a member function - affects SubgraphPreconditioner */ + inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { + return *gaussianErrors_(fg, x); } + } // namespace gtsam diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6db6330b0..e152738fb 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -261,12 +261,10 @@ HessianFactor::HessianFactor(const FactorGraph& factors, if (debug) cout << "Combining " << factors.size() << " factors" << endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - if (hessian) - updateATA(*hessian, scatter); - else { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if (jacobianFactor) + if(factor) { + if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) + updateATA(*hessian, scatter); + else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) updateATA(*jacobianFactor, scatter); else throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); @@ -309,16 +307,6 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { } } -/* ************************************************************************* */ -double HessianFactor::constantTerm() const { - return info_(this->size(), this->size())(0,0); -} - -/* ************************************************************************* */ -HessianFactor::constColumn HessianFactor::linearTerm() const { - return info_.rangeColumn(0, this->size(), this->size(), 0); -} - /* ************************************************************************* */ Matrix HessianFactor::computeInformation() const { return info_.full().selfadjointView(); @@ -360,7 +348,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // Apply updates to the upper triangle tic(3, "update"); - assert(this->info_.nBlocks() - 1 == scatter.size()); for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; for(size_t j1=0; j1<=j2; ++j1) { @@ -437,7 +424,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt // Apply updates to the upper triangle tic(3, "update"); - assert(this->info_.nBlocks() - 1 == scatter.size()); for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; for(size_t j1=0; j1<=j2; ++j1) { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 74216f0f0..9e4ed2074 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -120,20 +120,6 @@ namespace gtsam { InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] BlockInfo info_; ///< The block view of the full information matrix. - /** Update the factor by adding the information from the JacobianFactor - * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); - public: typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this @@ -256,16 +242,39 @@ namespace gtsam { */ constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } + /** Return the upper-triangular part of the full *augmented* information matrix, * as described above. See HessianFactor class documentation above to explain that only the * upper-triangular part of the information matrix is stored and returned by this function. */ constBlock info() const { return info_.full(); } + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + Block info() { return info_.full(); } + /** Return the constant term \f$ f \f$ as described above * @return The constant term \f$ f \f$ */ - double constantTerm() const; + double constantTerm() const { return info_(this->size(), this->size())(0,0); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double& constantTerm() { return info_(this->size(), this->size())(0,0); } /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can @@ -273,9 +282,19 @@ namespace gtsam { * @return The linear term \f$ g \f$ */ constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); } + /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ - constColumn linearTerm() const; + constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); } /** Return the augmented information matrix represented by this GaussianFactor. * The augmented information matrix contains the information matrix with an @@ -315,6 +334,20 @@ namespace gtsam { /** split partially eliminated factor */ boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); + /** Update the factor by adding the information from the JacobianFactor + * (used internally during elimination). + * @param update The JacobianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const JacobianFactor& update, const Scatter& scatter); + + /** Update the factor by adding the information from the HessianFactor + * (used internally during elimination). + * @param update The HessianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const HessianFactor& update, const Scatter& scatter); + /** assert invariants */ void assertInvariants() const; diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp new file mode 100644 index 000000000..2bba3e12b --- /dev/null +++ b/gtsam/linear/IterativeSolver.cpp @@ -0,0 +1,50 @@ +/** + * @file IterativeSolver.cpp + * @brief + * @date Sep 3, 2012 + * @author Yong-Dian Jian + */ + +#include +#include +#include + +namespace gtsam { + +std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); } +std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); } + +IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "CG") return IterativeOptimizationParameters::CG; + /* default is cg */ + else return IterativeOptimizationParameters::CG; +} + +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SILENT") return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; + /* default is default */ + else return IterativeOptimizationParameters::SILENT; +} + +std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) { + if ( k == CG ) return "CG"; + else return "UNKNOWN"; +} + +std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) return "SILENT"; + else if (verbosity == COMPLEXITY) return "COMPLEXITY"; + else if (verbosity == ERROR) return "ERROR"; + else return "UNKNOWN"; +} + + +} + + diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 8292b3831..7946874bb 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -13,6 +13,7 @@ #include #include +#include namespace gtsam { @@ -41,23 +42,34 @@ namespace gtsam { inline Kernel kernel() const { return kernel_; } inline Verbosity verbosity() const { return verbosity_; } + /* matlab interface */ + std::string getKernel() const ; + std::string getVerbosity() const; + void setKernel(const std::string &s) ; + void setVerbosity(const std::string &s) ; + void print() const { - const std::string kernelStr[1] = {"cg"}; std::cout << "IterativeOptimizationParameters: " - << "kernel = " << kernelStr[kernel_] - << ", verbosity = " << verbosity_ << std::endl; + << "kernel = " << kernelTranslator(kernel_) + << ", verbosity = " << verbosityTranslator(verbosity_) << std::endl; } + + static Kernel kernelTranslator(const std::string &s); + static Verbosity verbosityTranslator(const std::string &s); + static std::string kernelTranslator(Kernel k); + static std::string verbosityTranslator(Verbosity v); }; class IterativeSolver { public: typedef boost::shared_ptr shared_ptr; - IterativeSolver(){} + IterativeSolver() {} virtual ~IterativeSolver() {} + /* interface to the nonlinear optimizer */ + virtual VectorValues optimize () = 0; /* update interface to the nonlinear optimizer */ virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {} - virtual VectorValues optimize () = 0; }; } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index def36b086..f2bd9c75d 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -322,7 +322,6 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(model_); ar & BOOST_SERIALIZATION_NVP(matrix_); } - }; // JacobianFactor } // gtsam diff --git a/gtsam/linear/JacobianFactorGraph.cpp b/gtsam/linear/JacobianFactorGraph.cpp deleted file mode 100644 index 0ca3b8667..000000000 --- a/gtsam/linear/JacobianFactorGraph.cpp +++ /dev/null @@ -1,122 +0,0 @@ -/** - * @file JacobianFactorGraph.h - * @date Jun 6, 2012 - * @author Yong Dian Jian - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) { - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - e.push_back((*Ai)*x); - } - return e; -} - -/* ************************************************************************* */ -void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) { - multiplyInPlace(fg,x,e.begin()); -} - -/* ************************************************************************* */ -void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { - Errors::iterator ei = e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - *ei = (*Ai)*x; - ei++; - } -} - - -/* ************************************************************************* */ -// x += alpha*A'*e -void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { - Ai->transposeMultiplyAdd(alpha,*(ei++),x); - } -} - -/* ************************************************************************* */ -VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0) { - // It is crucial for performance to make a zero-valued clone of x - VectorValues g = VectorValues::Zero(x0); - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(factor->error_vector(x0)); - } - transposeMultiplyAdd(fg, 1.0, e, g); - return g; -} - -/* ************************************************************************* */ -void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) { - // Zero-out the gradient - g.setZero(); - Errors e; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(-factor->getb()); - } - transposeMultiplyAdd(fg, 1.0, e, g); -} - -/* ************************************************************************* */ -void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - Index i = 0 ; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - r[i] = factor->getb(); - i++; - } - VectorValues Ax = VectorValues::SameStructure(r); - multiply(fg,x,Ax); - axpy(-1.0,Ax,r); -} - -/* ************************************************************************* */ -void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - r.vector() = Vector::Zero(r.dim()); - Index i = 0; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - SubVector &y = r[i]; - for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - y += factor->getA(j) * x[*j]; - } - ++i; - } -} - -/* ************************************************************************* */ -void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x) { - x.vector() = Vector::Zero(x.dim()); - Index i = 0; - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - x[*j] += factor->getA(j).transpose() * r[i]; - } - ++i; - } -} - -/* ************************************************************************* */ -JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg) { - JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph()); - jfg->reserve(gfg.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { - JacobianFactor::shared_ptr castedFactor(boost::dynamic_pointer_cast(factor)); - if(castedFactor) jfg->push_back(castedFactor); - else throw std::invalid_argument("dynamicCastFactors(), dynamic_cast failed, meaning an invalid cast was requested."); - } - return jfg; -} - - - -} // namespace - - diff --git a/gtsam/linear/JacobianFactorGraph.h b/gtsam/linear/JacobianFactorGraph.h deleted file mode 100644 index cb27b507a..000000000 --- a/gtsam/linear/JacobianFactorGraph.h +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file JacobianFactorGraph.cpp - * @date Jun 6, 2012 - * @brief Linear Algebra Operations for a JacobianFactorGraph - * @author Yong Dian Jian - */ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - typedef FactorGraph JacobianFactorGraph; - - /** return A*x */ - Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x); - - /** In-place version e <- A*x that overwrites e. */ - void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e); - - /** In-place version e <- A*x that takes an iterator. */ - void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); - - /** x += alpha*A'*e */ - void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ - VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around zero. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ - void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g); - - /* matrix-vector operations */ - void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); - void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r); - void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x); - - /** dynamic_cast the gaussian factors down to jacobian factors */ - JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg); - -} diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp deleted file mode 100644 index fa0fee82f..000000000 --- a/gtsam/linear/SimpleSPCGSolver.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -/* utility function */ -std::vector extractRowSpec_(const FactorGraph& jfg) { - std::vector spec; spec.reserve(jfg.size()); - BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) { - spec.push_back(jf->rows()); - } - return spec; -} - -std::vector extractColSpec_(const FactorGraph& gfg, const VariableIndex &index) { - const size_t n = index.size(); - std::vector spec(n, 0); - for ( Index i = 0 ; i < n ; ++i ) { - const GaussianFactor &gf = *(gfg[index[i].front()]); - for ( GaussianFactor::const_iterator it = gf.begin() ; it != gf.end() ; ++it ) { - spec[*it] = gf.getDim(it); - } - } - return spec; -} - -SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) - : Base(), parameters_(parameters) -{ - std::vector colSpec = extractColSpec_(gfg, VariableIndex(gfg)); - - nVar_ = colSpec.size(); - - /* Split the factor graph into At (tree) and Ac (constraints) - * Note: This part has to be refined for your graph/application */ - GaussianFactorGraph::shared_ptr At; - boost::tie(At, Ac_) = this->splitGraph(gfg); - - /* construct row vector spec of the new system */ - nAc_ = Ac_->size(); - std::vector rowSpec = extractRowSpec_(*Ac_); - for ( Index i = 0 ; i < nVar_ ; ++i ) { - rowSpec.push_back(colSpec[i]); - } - - /* solve the tree with direct solver. get preconditioner */ - Rt_ = EliminationTree::Create(*At)->eliminate(EliminateQR); - xt_ = boost::make_shared(gtsam::optimize(*Rt_)); - - /* initial value for the lspcg method */ - y0_ = boost::make_shared(VectorValues::Zero(colSpec)); - - /* the right hand side of the new system */ - by_ = boost::make_shared(VectorValues::Zero(rowSpec)); - for ( Index i = 0 ; i < nAc_ ; ++i ) { - JacobianFactor::shared_ptr jf = (*Ac_)[i]; - Vector xi = internal::extractVectorValuesSlices(*xt_, jf->begin(), jf->end()); - (*by_)[i] = jf->getb() - jf->getA()*xi; - } - - /* allocate buffer for row and column vectors */ - tmpY_ = boost::make_shared(VectorValues::Zero(colSpec)); - tmpB_ = boost::make_shared(VectorValues::Zero(rowSpec)); -} - -/* implements the CGLS method in Section 7.4 of Bjork's book */ -VectorValues SimpleSPCGSolver::optimize (const VectorValues &initial) { - - VectorValues::shared_ptr x(new VectorValues(initial)); - VectorValues r = VectorValues::Zero(*by_), - q = VectorValues::Zero(*by_), - p = VectorValues::Zero(*y0_), - s = VectorValues::Zero(*y0_); - - residual(*x, r); - transposeMultiply(r, s) ; - p.vector() = s.vector() ; - - double gamma = s.vector().squaredNorm(), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ; - - const double threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma); - const size_t iMaxIterations = parameters_.maxIterations(); - const Parameters::Verbosity verbosity = parameters_.verbosity(); - - if ( verbosity >= ConjugateGradientParameters::ERROR ) - std::cout << "[SimpleSPCGSolver] epsilon = " << parameters_.epsilon() - << ", max = " << parameters_.maxIterations() - << ", ||r0|| = " << std::sqrt(gamma) - << ", threshold = " << threshold << std::endl; - - size_t k ; - for ( k = 1 ; k < iMaxIterations ; ++k ) { - - multiply(p, q); - alpha = gamma / q.vector().squaredNorm() ; - x->vector() += (alpha * p.vector()); - r.vector() -= (alpha * q.vector()); - transposeMultiply(r, s); - new_gamma = s.vector().squaredNorm(); - beta = new_gamma / gamma ; - p.vector() = s.vector() + beta * p.vector(); - gamma = new_gamma ; - - if ( verbosity >= ConjugateGradientParameters::ERROR) { - std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl; - } - - if ( gamma < threshold ) break ; - } // k - - if ( verbosity >= ConjugateGradientParameters::ERROR ) - std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl; - - /* transform y back to x */ - return this->transform(*x); -} - -void SimpleSPCGSolver::residual(const VectorValues &input, VectorValues &output) { - output.vector() = by_->vector(); - this->multiply(input, *tmpB_); - axpy(-1.0, *tmpB_, output); -} - -void SimpleSPCGSolver::multiply(const VectorValues &input, VectorValues &output) { - this->backSubstitute(input, *tmpY_); - gtsam::multiply(*Ac_, *tmpY_, output); - for ( Index i = 0 ; i < nVar_ ; ++i ) { - output[i + nAc_] = input[i]; - } -} - -void SimpleSPCGSolver::transposeMultiply(const VectorValues &input, VectorValues &output) { - gtsam::transposeMultiply(*Ac_, input, *tmpY_); - this->transposeBackSubstitute(*tmpY_, output); - for ( Index i = 0 ; i < nVar_ ; ++i ) { - output[i] += input[nAc_+i]; - } -} - -void SimpleSPCGSolver::backSubstitute(const VectorValues &input, VectorValues &output) { - for ( Index i = 0 ; i < input.size() ; ++i ) { - output[i] = input[i] ; - } - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, *Rt_) { - const Index key = *(cg->beginFrontals()); - Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); - xS = input[key] - cg->get_S() * xS; - output[key] = cg->get_R().triangularView().solve(xS); - } -} - -void SimpleSPCGSolver::transposeBackSubstitute(const VectorValues &input, VectorValues &output) { - for ( Index i = 0 ; i < input.size() ; ++i ) { - output[i] = input[i] ; - } - BOOST_FOREACH(const boost::shared_ptr cg, *Rt_) { - const Index key = *(cg->beginFrontals()); - output[key] = gtsam::backSubstituteUpper(output[key], Matrix(cg->get_R())); - const Vector d = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()) - - cg->get_S().transpose() * output[key]; - internal::writeVectorValuesSlices(d, output, cg->beginParents(), cg->endParents()); - } -} - -VectorValues SimpleSPCGSolver::transform(const VectorValues &y) { - VectorValues x = VectorValues::Zero(y); - this->backSubstitute(y, x); - axpy(1.0, *xt_, x); - return x; -} - -boost::tuple::shared_ptr> -SimpleSPCGSolver::splitGraph(const GaussianFactorGraph &gfg) { - - VariableIndex index(gfg); - size_t n = index.size(); - std::vector connected(n, false); - - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - FactorGraph::shared_ptr Ac( new FactorGraph()); - - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - bool augment = false ; - - /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; - else { - BOOST_FOREACH ( const Index key, *gf ) { - if ( connected[key] == false ) { - augment = true ; - } - connected[key] = true; - } - } - - if ( augment ) At->push_back(gf); - else Ac->push_back(boost::dynamic_pointer_cast(gf)); - } - - return boost::tie(At, Ac); -} - - - - - - -} diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h deleted file mode 100644 index 994815fa4..000000000 --- a/gtsam/linear/SimpleSPCGSolver.h +++ /dev/null @@ -1,98 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - -struct SimpleSPCGSolverParameters : public ConjugateGradientParameters { - typedef ConjugateGradientParameters Base; - SimpleSPCGSolverParameters() : Base() {} -}; - -/** - * This class gives a simple implementation to the SPCG solver presented in Dellaert et al in IROS'10. - * - * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into - * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. - * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. - * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ - * - * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it - * with the least-squares variation of the conjugate gradient method. - * - * Note: A full SPCG implementation will come up soon in the next release. - * - * \nosubgrouping - */ - -class SimpleSPCGSolver : public IterativeSolver { - -public: - - typedef IterativeSolver Base; - typedef SimpleSPCGSolverParameters Parameters; - typedef boost::shared_ptr shared_ptr; - -protected: - - size_t nVar_ ; ///< number of variables \f$ x \f$ - size_t nAc_ ; ///< number of factors in \f$ A_c \f$ - FactorGraph::shared_ptr Ac_; ///< the constrained part of the graph - GaussianBayesNet::shared_ptr Rt_; ///< the gaussian bayes net of the tree part of the graph - VectorValues::shared_ptr xt_; ///< the solution of the \f$ A_t^{-1} b_t \f$ - VectorValues::shared_ptr y0_; ///< a column zero vector - VectorValues::shared_ptr by_; ///< \f$ [\bar{b_y} ; 0 ] \f$ - VectorValues::shared_ptr tmpY_; ///< buffer for the column vectors - VectorValues::shared_ptr tmpB_; ///< buffer for the row vectors - Parameters parameters_; ///< Parameters for iterative method - -public: - - SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); - virtual ~SimpleSPCGSolver() {} - virtual VectorValues optimize () {return optimize(*y0_);} - -protected: - - VectorValues optimize (const VectorValues &initial); - - /** output = \f$ [\bar{b_y} ; 0 ] - [A_c R_t^{-1} ; I] \f$ input */ - void residual(const VectorValues &input, VectorValues &output); - - /** output = \f$ [A_c R_t^{-1} ; I] \f$ input */ - void multiply(const VectorValues &input, VectorValues &output); - - /** output = \f$ [R_t^{-T} A_c^T, I] \f$ input */ - void transposeMultiply(const VectorValues &input, VectorValues &output); - - /** output = \f$ R_t^{-1} \f$ input */ - void backSubstitute(const VectorValues &rhs, VectorValues &sol) ; - - /** output = \f$ R_t^{-T} \f$ input */ - void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ; - - /** return \f$ R_t^{-1} y + x_t \f$ */ - VectorValues transform(const VectorValues &y); - - /** naively split a gaussian factor graph into tree and constraint parts - * Note: This function has to be refined for your graph/application */ - boost::tuple::shared_ptr> - splitGraph(const GaussianFactorGraph &gfg); -}; - -} diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 91a98a263..8b5a3708f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -17,18 +17,29 @@ #include #include -#include - #include using namespace std; namespace gtsam { + + /* ************************************************************************* */ + static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); + if( !jf ) { + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + result->push_back(jf); + } + return result; + } /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, + SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar) : - Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { + Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { } /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 7b59735d9..f266928d7 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -32,15 +33,14 @@ namespace gtsam { class SubgraphPreconditioner { public: - typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedBayesNet; - typedef boost::shared_ptr > sharedFG; + typedef boost::shared_ptr sharedFG; typedef boost::shared_ptr sharedValues; typedef boost::shared_ptr sharedErrors; private: - sharedFG Ab1_, Ab2_; + sharedFG Ab2_; sharedBayesNet Rc1_; sharedValues xbar_; ///< A1 \ b1 sharedErrors b2bar_; ///< A2*xbar - b2 @@ -48,17 +48,14 @@ namespace gtsam { public: SubgraphPreconditioner(); + /** * Constructor - * @param Ab1: the Graph A1*x=b1 * @param Ab2: the Graph A2*x=b2 * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); - - /** Access Ab1 */ - const sharedFG& Ab1() const { return Ab1_; } + SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); /** Access Ab2 */ const sharedFG& Ab2() const { return Ab2_; } @@ -69,23 +66,23 @@ namespace gtsam { /** Access b2bar */ const sharedErrors b2bar() const { return b2bar_; } - /** - * Add zero-mean i.i.d. Gaussian prior terms to each variable - * @param sigma Standard deviation of Gaussian - */ -// SubgraphPreconditioner add_priors(double sigma) const; + /** + * Add zero-mean i.i.d. Gaussian prior terms to each variable + * @param sigma Standard deviation of Gaussian + */ +// SubgraphPreconditioner add_priors(double sigma) const; /* x = xbar + inv(R1)*y */ VectorValues x(const VectorValues& y) const; /* A zero VectorValues with the structure of xbar */ VectorValues zero() const { - VectorValues V(VectorValues::Zero(*xbar_)) ; + VectorValues V(VectorValues::Zero(*xbar_)); return V ; } /** - * Add constraint part of the error only, used in both calls above + * Add constraint part of the error only * y += alpha*inv(R1')*A2'*e2 * Takes a range indicating e2 !!!! */ diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h deleted file mode 100644 index 28b2caaac..000000000 --- a/gtsam/linear/SubgraphSolver-inl.h +++ /dev/null @@ -1,80 +0,0 @@ -///* ---------------------------------------------------------------------------- -// -// * GTSAM Copyright 2010, Georgia Tech Research Corporation, -// * Atlanta, Georgia 30332-0415 -// * All Rights Reserved -// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) -// -// * See LICENSE for the license information -// -// * -------------------------------------------------------------------------- */ -// -//#pragma once -// -//#include -// -//#include -//#include -//#include -// -//namespace gtsam { -// -//template -//void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { -// -// GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); -// GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); -// -// if (parameters_->verbosity()) cout << "split the graph ..."; -// split(pairs_, *graph, *Ab1, *Ab2) ; -// if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; -// -// // // Add a HardConstraint to the root, otherwise the root will be singular -// // Key root = keys.back(); -// // T_.addHardConstraint(root, theta0[root]); -// // -// // // compose the approximate solution -// // theta_bar_ = composePoses (T_, tree, theta0[root]); -// -// LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! -// SubgraphPreconditioner::sharedBayesNet Rc1 = -// EliminationTree::Create(sacrificialAb1)->eliminate(&EliminateQR); -// SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); -// -// pc_ = boost::make_shared( -// Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); -//} -// -//template -//VectorValues::shared_ptr SubgraphSolver::optimize() const { -// -// // preconditioned conjugate gradient -// VectorValues zeros = pc_->zero(); -// VectorValues ybar = conjugateGradients -// (*pc_, zeros, *parameters_); -// -// boost::shared_ptr xbar = boost::make_shared() ; -// *xbar = pc_->x(ybar); -// return xbar; -//} -// -//template -//void SubgraphSolver::initialize(const GRAPH& G, const Values& theta0) { -// // generate spanning tree -// PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); -// -// // make the ordering -// list keys = predecessorMap2Keys(tree_); -// ordering_ = boost::make_shared(list(keys.begin(), keys.end())); -// -// // build factor pairs -// pairs_.clear(); -// typedef pair EG ; -// BOOST_FOREACH( const EG &eg, tree_ ) { -// Key key1 = Key(eg.first), -// key2 = Key(eg.second) ; -// pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; -// } -//} -// -//} // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 9b37d9a4b..9986fb76c 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -14,100 +14,119 @@ #include #include #include -#include +#include #include #include #include #include - +#include #include #include - #include using namespace std; namespace gtsam { +/**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) : parameters_(parameters) { + initialize(gfg); +} +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters) + : parameters_(parameters) +{ + initialize(*jfg); +} - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); - GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters) + : parameters_(parameters) { - boost::tie(Ab1, Ab2) = splitGraph(gfg) ; + GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(Ab1)->eliminate(&EliminateQR); + initialize(Rc1, boost::make_shared(Ab2)); +} - if (parameters_.verbosity()) - cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; - - // // Add a HardConstraint to the root, otherwise the root will be singular - // Key root = keys.back(); - // T_.addHardConstraint(root, theta0[root]); - // - // // compose the approximate solution - // theta_bar_ = composePoses (T_, tree, theta0[root]); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) + : parameters_(parameters) { GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); - VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + initialize(Rc1, Ab2); +} - // Convert or cast Ab1 to JacobianFactors - boost::shared_ptr > Ab1Jacobians = boost::make_shared >(); - Ab1Jacobians->reserve(Ab1->size()); - BOOST_FOREACH(const boost::shared_ptr& factor, *Ab1) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - Ab1Jacobians->push_back(jf); - else - Ab1Jacobians->push_back(boost::make_shared(*factor)); - } - - // Convert or cast Ab2 to JacobianFactors - boost::shared_ptr > Ab2Jacobians = boost::make_shared >(); - Ab1Jacobians->reserve(Ab2->size()); - BOOST_FOREACH(const boost::shared_ptr& factor, *Ab2) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - Ab2Jacobians->push_back(jf); - else - Ab2Jacobians->push_back(boost::make_shared(*factor)); - } +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters) : parameters_(parameters) +{ + initialize(Rc1, boost::make_shared(Ab2)); +} - pc_ = boost::make_shared(Ab1Jacobians, Ab2Jacobians, Rc1, xbar); +/**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters) +{ + initialize(Rc1, Ab2); } VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } -boost::tuple -SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) { +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) +{ + GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), + Ab2 = boost::make_shared(); - VariableIndex index(gfg); - size_t n = index.size(); - std::vector connected(n, false); + boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + if (parameters_.verbosity()) + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; + + GaussianBayesNet::shared_ptr Rc1 = EliminationTree::Create(*Ab1)->eliminate(&EliminateQR); + VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + pc_ = boost::make_shared(Ab2, Rc1, xbar); +} + +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) +{ + VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); + pc_ = boost::make_shared(Ab2, Rc1, xbar); +} + +boost::tuple +SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { + + const VariableIndex index(jfg); + const size_t n = index.size(); + DSFVector D(n); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + size_t t = 0; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { + + if ( gf->keys().size() > 2 ) { + throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); + } bool augment = false ; /* check whether this factor should be augmented to the "tree" graph */ if ( gf->keys().size() == 1 ) augment = true; else { - BOOST_FOREACH ( const Index key, *gf ) { - if ( connected[key] == false ) { - augment = true ; - connected[key] = true; - } + const Index u = gf->keys()[0], v = gf->keys()[1], + u_root = D.findSet(u), v_root = D.findSet(v); + if ( u_root != v_root ) { + t++; augment = true ; + D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(gf); else Ac->push_back(gf); } @@ -115,7 +134,4 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) { return boost::tie(At, Ac); } - - - } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 60104bbe0..7c60c0588 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,7 +13,9 @@ #include #include +#include #include +#include #include @@ -23,31 +25,61 @@ class SubgraphSolverParameters : public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; SubgraphSolverParameters() : Base() {} + virtual void print(const std::string &s="") const { Base::print(s); } }; /** - * A linear system solver using subgraph preconditioning conjugate gradient + * This class implements the SPCG solver presented in Dellaert et al in IROS'10. + * + * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into + * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. + * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. + * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * + * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it + * with the least-squares variation of the conjugate gradient method. + * + * To use it in nonlinear optimization, please see the following example + * + * LevenbergMarquardtParams parameters; + * parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; + * parameters.iterativeParams = boost::make_shared(); + * LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + * Values result = optimizer.optimize(); + * + * \nosubgrouping */ class SubgraphSolver : public IterativeSolver { public: - typedef SubgraphSolverParameters Parameters; protected: - Parameters parameters_; SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object public: + /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters ¶meters); + + /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); + + /* The same as above, but the A1 is solved before */ + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters); + SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters); - SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); virtual ~SubgraphSolver() {} virtual VectorValues optimize () ; protected: + void initialize(const GaussianFactorGraph &jfg); + void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2); + boost::tuple splitGraph(const GaussianFactorGraph &gfg) ; }; diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 8a052a66f..5cc0841b0 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -122,7 +122,7 @@ namespace gtsam { // conjugate gradient method. // S: linear system, V: step vector, E: errors template - V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters ¶meters, bool steepest = false) { + V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters ¶meters, bool steepest) { CGState state(Ab, x, parameters, steepest); diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index 6dadb2d72..07d4de865 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -19,53 +19,60 @@ #include #include #include -#include +#include #include - #include using namespace std; namespace gtsam { - /* ************************************************************************* */ - void System::print (const string& s) const { - cout << s << endl; - gtsam::print(A_, "A"); - gtsam::print(b_, "b"); - } + /* ************************************************************************* */ + void System::print(const string& s) const { + cout << s << endl; + gtsam::print(A_, "A"); + gtsam::print(b_, "b"); + } - /* ************************************************************************* */ + /* ************************************************************************* */ - Vector steepestDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients (Ab, x, parameters, true); - } + Vector steepestDescent(const System& Ab, const Vector& x, + const ConjugateGradientParameters & parameters) { + return conjugateGradients(Ab, x, parameters, true); + } - Vector conjugateGradientDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients (Ab, x, parameters); - } + Vector conjugateGradientDescent(const System& Ab, const Vector& x, + const ConjugateGradientParameters & parameters) { + return conjugateGradients(Ab, x, parameters); + } - /* ************************************************************************* */ - Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { - System Ab(A, b); - return conjugateGradients (Ab, x, parameters, true); - } + /* ************************************************************************* */ + Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, + const ConjugateGradientParameters & parameters) { + System Ab(A, b); + return conjugateGradients(Ab, x, parameters, true); + } - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) { - System Ab(A, b); - return conjugateGradients (Ab, x, parameters); - } + Vector conjugateGradientDescent(const Matrix& A, const Vector& b, + const Vector& x, const ConjugateGradientParameters & parameters) { + System Ab(A, b); + return conjugateGradients(Ab, x, parameters); + } - /* ************************************************************************* */ - VectorValues steepestDescent(const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors> (fg, x, parameters, true); - } + /* ************************************************************************* */ + VectorValues steepestDescent(const GaussianFactorGraph& fg, + const VectorValues& x, const ConjugateGradientParameters & parameters) { + return conjugateGradients( + fg, x, parameters, true); + } - VectorValues conjugateGradientDescent(const FactorGraph& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) { - return conjugateGradients, VectorValues, Errors> (fg, x, parameters); - } + VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, + const VectorValues& x, const ConjugateGradientParameters & parameters) { + return conjugateGradients( + fg, x, parameters); + } - /* ************************************************************************* */ +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 5ffb366c2..0eb04bf49 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -31,12 +31,11 @@ namespace gtsam { * "Vector" class E needs dot(v,v) * @param Ab, the "system" that needs to be solved, examples below * @param x is the initial estimate - * @param epsilon determines the convergence criterion: norm(g) - V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, size_t maxIterations, bool steepest = false); + template + V conjugateGradients(const S& Ab, V x, + const ConjugateGradientParameters ¶meters, bool steepest = false); /** * Helper class encapsulating the combined system |Ax-b_|^2 @@ -127,11 +126,9 @@ namespace gtsam { const Vector& x, const ConjugateGradientParameters & parameters); - class GaussianFactorGraph; - /** * Method of steepest gradients, Gaussian Factor Graph version - * */ + */ VectorValues steepestDescent( const GaussianFactorGraph& fg, const VectorValues& x, @@ -139,7 +136,7 @@ namespace gtsam { /** * Method of conjugate gradients (CG), Gaussian Factor Graph version - * */ + */ VectorValues conjugateGradientDescent( const GaussianFactorGraph& fg, const VectorValues& x, diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 4c0aa3871..cb835a76a 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -410,7 +410,7 @@ TEST(GaussianFactor, eliminateFrontals) factors.push_back(factor4); // extract the dense matrix for the graph - Matrix actualDense = factors.denseJacobian(); + Matrix actualDense = factors.augmentedJacobian(); EXPECT(assert_equal(2.0 * Ab, actualDense)); // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians @@ -619,7 +619,7 @@ TEST(GaussianFactorGraph, sparseJacobian) { } /* ************************************************************************* */ -TEST(GaussianFactorGraph, denseHessian) { +TEST(GaussianFactorGraph, matrices) { // Create factor graph: // x1 x2 x3 x4 x5 b // 1 2 3 0 0 4 @@ -639,9 +639,24 @@ TEST(GaussianFactorGraph, denseHessian) { 9,10, 0,11,12,13, 0, 0, 0,14,15,16; + Matrix expectedJacobian = jacobian; Matrix expectedHessian = jacobian.transpose() * jacobian; - Matrix actualHessian = gfg.denseHessian(); + Matrix expectedA = jacobian.leftCols(jacobian.cols()-1); + Vector expectedb = jacobian.col(jacobian.cols()-1); + Matrix expectedL = expectedA.transpose() * expectedA; + Vector expectedeta = expectedA.transpose() * expectedb; + + Matrix actualJacobian = gfg.augmentedJacobian(); + Matrix actualHessian = gfg.augmentedHessian(); + Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); + Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); + + EXPECT(assert_equal(expectedJacobian, actualJacobian)); EXPECT(assert_equal(expectedHessian, actualHessian)); + EXPECT(assert_equal(expectedA, actualA)); + EXPECT(assert_equal(expectedb, actualb)); + EXPECT(assert_equal(expectedL, actualL)); + EXPECT(assert_equal(expectedeta, actualeta)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactorGraph.cpp b/gtsam/linear/tests/testJacobianFactorGraph.cpp new file mode 100644 index 000000000..07dda4652 --- /dev/null +++ b/gtsam/linear/tests/testJacobianFactorGraph.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testJacobianFactorGraph.cpp + * @brief Unit tests for GaussianFactorGraph + * @author Yong Dian Jian + **/ + +#include +#include + + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, gradient ) +//{ +// GaussianFactorGraph fg = createGaussianFactorGraph(); +// +// // Construct expected gradient +// VectorValues expected; +// +// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 +// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] +// expected.insert(L(1),Vector_(2, 5.0,-12.5)); +// expected.insert(X(1),Vector_(2, 30.0, 5.0)); +// expected.insert(X(2),Vector_(2,-25.0, 17.5)); +// +// // Check the gradient at delta=0 +// VectorValues zero = createZeroDelta(); +// VectorValues actual = fg.gradient(zero); +// EXPECT(assert_equal(expected,actual)); +// +// // Check it numerically for good measure +// Vector numerical_g = numericalGradient(error,zero,0.001); +// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g)); +// +// // Check the gradient at the solution (should be zero) +// Ordering ord; +// ord += X(2),L(1),X(1); +// GaussianFactorGraph fg2 = createGaussianFactorGraph(); +// VectorValues solution = fg2.optimize(ord); // destructive +// VectorValues actual2 = fg.gradient(solution); +// EXPECT(assert_equal(zero,actual2)); +//} + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, transposeMultiplication ) +//{ +// // create an ordering +// Ordering ord; ord += X(2),L(1),X(1); +// +// GaussianFactorGraph A = createGaussianFactorGraph(ord); +// Errors e; +// e += Vector_(2, 0.0, 0.0); +// e += Vector_(2,15.0, 0.0); +// e += Vector_(2, 0.0,-5.0); +// e += Vector_(2,-7.5,-5.0); +// +// VectorValues expected = createZeroDelta(ord), actual = A ^ e; +// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0); +// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0); +// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0); +// EXPECT(assert_equal(expected,actual)); +//} + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, rhs ) +//{ +// // create an ordering +// Ordering ord; ord += X(2),L(1),X(1); +// +// GaussianFactorGraph Ab = createGaussianFactorGraph(ord); +// Errors expected = createZeroDelta(ord), actual = Ab.rhs(); +// expected.push_back(Vector_(2,-1.0,-1.0)); +// expected.push_back(Vector_(2, 2.0,-1.0)); +// expected.push_back(Vector_(2, 0.0, 1.0)); +// expected.push_back(Vector_(2,-1.0, 1.5)); +// EXPECT(assert_equal(expected,actual)); +//} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index b09133c4d..081910e8c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -27,7 +27,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include #include diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index deb2fc9d1..1cea78837 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -598,6 +598,9 @@ public: const ISAM2Params& params() const { return params_; } + /** prints out clique statistics */ + void printStats() const { getCliqueData().getStats().print(); } + //@} private: diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 52c507374..c3078ea76 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -152,7 +152,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab } // Get information matrix - Matrix augmentedInfo = jointFG.denseHessian(); + Matrix augmentedInfo = jointFG.augmentedHessian(); Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); return JointMarginal(info, dims, variableConversion); diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index f0b3852ef..da14d8982 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include #include diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 9f99948a4..b445af506 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -44,10 +44,23 @@ void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const cout << str; // Print ordering in index order Ordering::InvertedMap inverted = this->invert(); - cout << "key (zero-based order)\n"; + // Print the ordering with varsPerLine ordering entries printed on each line, + // for compactness. + static const size_t varsPerLine = 10; + bool endedOnNewline = false; BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) { - cout << keyFormatter(index_key.second) << " (" << index_key.first << ")\n"; + if(index_key.first % varsPerLine != 0) + cout << ", "; + cout << index_key.first << ":" << keyFormatter(index_key.second); + if(index_key.first % varsPerLine == varsPerLine - 1) { + cout << "\n"; + endedOnNewline = true; + } else { + endedOnNewline = false; + } } + if(!endedOnNewline) + cout << "\n"; cout.flush(); } diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index af3f70ddc..fec460b08 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -16,6 +15,10 @@ namespace gtsam { +void SuccessiveLinearizationParams::setIterativeParams(const SubgraphSolverParameters ¶ms) { + iterativeParams = boost::make_shared(params); +} + void SuccessiveLinearizationParams::print(const std::string& str) const { NonlinearOptimizerParams::print(str); switch ( linearSolverType ) { @@ -60,11 +63,7 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ } else if ( params.isCG() ) { if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); - if ( boost::dynamic_pointer_cast(params.iterativeParams)) { - SimpleSPCGSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); - delta = solver.optimize(); - } - else if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { + if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); delta = solver.optimize(); } diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index e79a364c5..d422c09a9 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -74,6 +74,7 @@ public: std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } + void setIterativeParams(const SubgraphSolverParameters ¶ms); void setOrdering(const Ordering& ordering) { this->ordering = ordering; } private: diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 2df2228af..aae3fc5da 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; TEST(dataSet, findExampleDataFile) { - const string expected_end = "gtsam/examples/Data/example.graph"; + const string expected_end = "examples/Data/example.graph"; const string actual = findExampleDataFile("example"); string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size()); boost::replace_all(actual_end, "\\", "/"); // Convert directory separators to forward-slash diff --git a/gtsam_unstable/base/Dummy.cpp b/gtsam_unstable/base/Dummy.cpp new file mode 100644 index 000000000..90d71d27e --- /dev/null +++ b/gtsam_unstable/base/Dummy.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Dummy.h + * @brief Dummy class for testing MATLAB memory allocation + * @author Andrew Melim + * @author Frank Dellaert + * @date June 14, 2012 + */ + +#include +#include + +namespace gtsam { + +static size_t gDummyCount = 0; + +Dummy::Dummy():id(++gDummyCount) { + std::cout << "Dummy constructor " << id << std::endl; +} + +Dummy::~Dummy() { + std::cout << "Dummy destructor " << id << std::endl; +} + +void Dummy::print(const std::string& s) const { + std::cout << s << "Dummy " << id << std::endl; +} + +unsigned char Dummy::dummyTwoVar(unsigned char a) const { + return a; +} + +} diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index 995186cd6..0ab344f74 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -17,26 +17,16 @@ * @date June 14, 2012 */ -namespace gtsam { +#include - static size_t gDummyCount; +namespace gtsam { struct Dummy { size_t id; - Dummy():id(++gDummyCount) { - std::cout << "Dummy constructor " << id << std::endl; - } - ~Dummy() { - std::cout << "Dummy destructor " << id << std::endl; - } - void print(const std::string& s="") const { - std::cout << s << "Dummy " << id << std::endl; - } - - unsigned char dummyTwoVar(unsigned char a) const { - return a; - } - + Dummy(); + ~Dummy(); + void print(const std::string& s="") const ; + unsigned char dummyTwoVar(unsigned char a) const ; }; } // namespace gtsam diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index fa44d3e93..b5701fb13 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -38,6 +38,15 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza } } +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor, + const boost::optional& linearizationPoint) +: factor_(factor), linearizationPoint_(linearizationPoint) { + // Extract keys stashed in linear factor + BOOST_FOREACH(const Index& idx, factor_->keys()) + keys_.push_back(idx); +} + /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( const JacobianFactor& factor, const Ordering& ordering, @@ -205,7 +214,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& orderin /* ************************************************************************* */ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place - return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor)); + return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor,linearizationPoint_)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index 9affcc29e..324556622 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -23,6 +23,10 @@ protected: GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; + /** direct copy constructor */ + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, + const boost::optional& linearizationPoint); + public: /** Primary constructor: store a linear factor and decode the ordering */ @@ -105,7 +109,7 @@ public: * Clones the underlying linear factor */ NonlinearFactor::shared_ptr clone() const { - return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_)); + return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_)); } // casting syntactic sugar diff --git a/gtsam_unstable/slam/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h index 60a29b3e4..9a56ec891 100644 --- a/gtsam_unstable/slam/PoseRotationPrior.h +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -67,9 +67,9 @@ public: if (H) { *H = gtsam::zeros(rDim, xDim); if (pose_traits::isRotFirst()) - (*H).leftCols(rDim) = eye(rDim); + (*H).leftCols(rDim).setIdentity(rDim, rDim); else - (*H).rightCols(rDim) = eye(rDim); + (*H).rightCols(rDim).setIdentity(rDim, rDim); } return Rotation::Logmap(newR) - Rotation::Logmap(measured_); diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m index 1e408d3d9..b29288fdf 100644 --- a/matlab/+gtsam/VisualISAMInitialize.m +++ b/matlab/+gtsam/VisualISAMInitialize.m @@ -50,7 +50,7 @@ for i=1:2 initialEstimates.insert(jj, truth.points{j}); end if options.pointPriors % add point priors - newFactors.add(priorFactorPoint3(jj, truth.points{j}, noiseModels.point)); + newFactors.add(PriorFactorPoint3(jj, truth.points{j}, noiseModels.point)); end end end diff --git a/matlab/+gtsam/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m index 5ef64cf62..cd7ab188e 100644 --- a/matlab/+gtsam/VisualISAMPlot.m +++ b/matlab/+gtsam/VisualISAMPlot.m @@ -39,11 +39,12 @@ drawnow %% do various optional things if options.saveFigures - fig2 = figure('visible','off'); - newax = copyobj(h,fig2); - colormap(fig2,'hot'); + figToSave = figure('visible','off'); + newax = copyobj(h,figToSave); + colormap(figToSave,'hot'); set(newax, 'units', 'normalized', 'position', [0.13 0.11 0.775 0.815]); - print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',M)); + print(figToSave,'-dpng',sprintf('VisualiSAM%03d.png',M)); + axes(h); end if options.saveDotFiles diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 0be3863a1..16cfb501a 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -2,17 +2,16 @@ # Tests message(STATUS "Installing Matlab Toolbox Tests") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE) # Examples message(STATUS "Installing Matlab Toolbox Examples") # Matlab files: *.m and *.fig -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.fig") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN "*.fig" PATTERN ".svn" EXCLUDE) # Utilities message(STATUS "Installing Matlab Toolbox Utilities") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE) install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}") message(STATUS "Installing Matlab Toolbox Examples (Data)") diff --git a/matlab/gtsam_examples/PlanarSLAMExample_graph.m b/matlab/gtsam_examples/PlanarSLAMExample_graph.m index a65ea5980..9ca88e49a 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_graph.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_graph.m @@ -15,7 +15,7 @@ clear import gtsam.* %% Find data file -datafile = '/Users/dellaert/borg/gtsam/examples/Data/example.graph'; +datafile = findExampleDataFile('example.graph'); %% Initialize graph, initial estimate, and odometry noise model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]); diff --git a/matlab/gtsam_examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m index a45ac40a0..67f22fe1d 100644 --- a/matlab/gtsam_examples/Pose2SLAMwSPCG.m +++ b/matlab/gtsam_examples/Pose2SLAMwSPCG.m @@ -51,7 +51,14 @@ initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -optimizer = DoglegOptimizer(graph, initialEstimate); -result = optimizer.optimizeSafely(); -result.print(sprintf('\nFinal result:\n')); \ No newline at end of file +%% Optimize using Levenberg-Marquardt optimization with SubgraphSolver +params = gtsam.LevenbergMarquardtParams; +subgraphParams = gtsam.SubgraphSolverParameters; +params.setLinearSolverType('CONJUGATE_GRADIENT'); +params.setIterativeParams(subgraphParams); +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimize(); +result.print(sprintf('\nFinal result:\n')); + + + diff --git a/matlab/gtsam_examples/VisualISAM_gui.fig b/matlab/gtsam_examples/VisualISAM_gui.fig index 3c8adb46c..d4d65f565 100644 Binary files a/matlab/gtsam_examples/VisualISAM_gui.fig and b/matlab/gtsam_examples/VisualISAM_gui.fig differ diff --git a/matlab/gtsam_examples/VisualISAM_gui.m b/matlab/gtsam_examples/VisualISAM_gui.m index bc08e7abf..4ccde5e53 100644 --- a/matlab/gtsam_examples/VisualISAM_gui.m +++ b/matlab/gtsam_examples/VisualISAM_gui.m @@ -82,7 +82,7 @@ options.showImages = get(handles.showImagesCB,'Value'); options.hardConstraint = get(handles.hardConstraintCB,'Value'); options.pointPriors = get(handles.pointPriorsCB,'Value'); options.batchInitialization = get(handles.batchInitCB,'Value'); -options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String')); +%options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String')); options.alwaysRelinearize = get(handles.alwaysRelinearizeCB,'Value'); % Display Options diff --git a/matlab/gtsam_examples/gtsamExamples.fig b/matlab/gtsam_examples/gtsamExamples.fig index e51c23468..70e22957a 100644 Binary files a/matlab/gtsam_examples/gtsamExamples.fig and b/matlab/gtsam_examples/gtsamExamples.fig differ diff --git a/matlab/gtsam_examples/gtsamExamples.m b/matlab/gtsam_examples/gtsamExamples.m index 72f611573..094329329 100644 --- a/matlab/gtsam_examples/gtsamExamples.m +++ b/matlab/gtsam_examples/gtsamExamples.m @@ -22,7 +22,7 @@ function varargout = gtsamExamples(varargin) % Edit the above text to modify the response to help gtsamExamples -% Last Modified by GUIDE v2.5 24-Jul-2012 10:18:05 +% Last Modified by GUIDE v2.5 03-Sep-2012 13:34:13 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; @@ -142,6 +142,13 @@ function PlanarSLAMSampling_Callback(hObject, eventdata, handles) axes(handles.axes3); PlanarSLAMExample_sampling +% --- Executes on button press in PlanarSLAMGraph. +function PlanarSLAMGraph_Callback(hObject, eventdata, handles) +axes(handles.axes3); +echo on +PlanarSLAMExample_graph +echo off + % --- Executes on button press in SFM. function SFM_Callback(hObject, eventdata, handles) axes(handles.axes3); diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh new file mode 100644 index 000000000..b78649e15 --- /dev/null +++ b/package_scripts/toolbox_package_unix.sh @@ -0,0 +1,43 @@ +#!/bin/sh + +# Detect platform +os=`uname -s` +arch=`uname -m` +if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then + platform=lin64 +elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then + platform=lin32 +else + echo "Unrecognized platform" + exit 1 +fi + +echo "Platform is ${platform}" + +# Check for empty directory +if [ ! -z "`ls`" ]; then + echo "Please run this script from an empty build directory" + exit 1 +fi + +# Check for boost +if [ -z "$1" -o -z "$2" ]; then + echo "Usage: $0 BOOSTTREE MEX" + echo "BOOSTTREE should be a boost source tree compiled with toolbox_build_boost." + echo "MEX should be the full path of the mex compiler" + exit 1 +fi + +# Run cmake +cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="$PWD/stage" -DBoost_NO_SYSTEM_PATHS:bool=true -DBoost_USE_STATIC_LIBS:bool=true -DBOOST_ROOT="$1" -DGTSAM_BUILD_UNSTABLE:bool=false -DGTSAM_DISABLE_EXAMPLES_ON_INSTALL:bool=true -DGTSAM_DISABLE_TESTS_ON_INSTALL:bool=true -DGTSAM_MEX_BUILD_STATIC_MODULE:bool=true -DMEX_COMMAND="$2" .. + +if [ ! $? ]; then + echo "CMake failed" + exit 1 +fi + +# Compile +make install + +# Create package +tar czf gtsam-toolbox-2.1.0-$platform.tgz -C stage/borg toolbox diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index 100a4db4c..71c8269e9 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -138,9 +138,9 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { + GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { // Create empty graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); @@ -273,7 +273,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createSimpleConstraintGraph() { + GaussianFactorGraph createSimpleConstraintGraph() { // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -293,7 +293,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -310,7 +310,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createSingleConstraintGraph() { + GaussianFactorGraph createSingleConstraintGraph() { // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -335,7 +335,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(f1); fg.push_back(f2); @@ -351,7 +351,7 @@ namespace example { } /* ************************************************************************* */ - JacobianFactorGraph createMultiConstraintGraph() { + GaussianFactorGraph createMultiConstraintGraph() { // unary factor 1 Matrix A = eye(2); Vector b = Vector_(2, -2.0, 2.0); @@ -396,7 +396,7 @@ namespace example { constraintModel)); // construct the graph - JacobianFactorGraph fg; + GaussianFactorGraph fg; fg.push_back(lf1); fg.push_back(lc1); fg.push_back(lc2); @@ -458,7 +458,8 @@ namespace example { xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); // linearize around zero - return boost::make_tuple(*nlfg.linearize(zeros, ordering), xtrue); + boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + return boost::make_tuple(*gfg, xtrue); } /* ************************************************************************* */ @@ -471,9 +472,9 @@ namespace example { } /* ************************************************************************* */ - pair splitOffPlanarTree(size_t N, - const JacobianFactorGraph& original) { - JacobianFactorGraph T, C; + pair splitOffPlanarTree(size_t N, + const GaussianFactorGraph& original) { + GaussianFactorGraph T, C; // Add the x11 constraint to the tree T.push_back(original[0]); diff --git a/tests/smallExample.h b/tests/smallExample.h index 161c45da8..f87847f24 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -23,7 +23,6 @@ #include #include -#include #include namespace gtsam { @@ -66,7 +65,7 @@ namespace gtsam { * create a linear factor graph * The non-linear graph above evaluated at NoisyValues */ - JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering); + GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); /** * create small Chordal Bayes Net x <- y @@ -100,21 +99,21 @@ namespace gtsam { * Creates a simple constrained graph with one linear factor and * one binary equality constraint that sets x = y */ - JacobianFactorGraph createSimpleConstraintGraph(); + GaussianFactorGraph createSimpleConstraintGraph(); VectorValues createSimpleConstraintValues(); /** * Creates a simple constrained graph with one linear factor and * one binary constraint. */ - JacobianFactorGraph createSingleConstraintGraph(); + GaussianFactorGraph createSingleConstraintGraph(); VectorValues createSingleConstraintValues(); /** * Creates a constrained graph with a linear factor and two * binary constraints that share a node */ - JacobianFactorGraph createMultiConstraintGraph(); + GaussianFactorGraph createMultiConstraintGraph(); VectorValues createMultiConstraintValues(); /* ******************************************************* */ @@ -147,8 +146,8 @@ namespace gtsam { * | * -x11-x21-x31 */ - std::pair splitOffPlanarTree( - size_t N, const JacobianFactorGraph& original); + std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraph& original); } // example } // gtsam diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 781685a23..6b03b572b 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -96,7 +96,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { gradientValues.vector() = gradient; // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraph(gbn).denseHessian(); + Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); VectorValues denseMatrixGradient = *allocateVectorValues(gbn); denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10); @@ -200,7 +200,7 @@ TEST(DoglegOptimizer, BT_BN_equivalency) { GaussianFactorGraph expected(gbn); GaussianFactorGraph actual(bt); - EXPECT(assert_equal(expected.denseHessian(), actual.denseHessian())); + EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian())); } /* ************************************************************************* */ @@ -276,7 +276,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { gradientValues.vector() = gradient; // Compute the gradient using dense matrices - Matrix augmentedHessian = GaussianFactorGraph(bt).denseHessian(); + Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); VectorValues denseMatrixGradient = *allocateVectorValues(bt); denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10); diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 0097ec49a..f5419b7ff 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -57,42 +57,16 @@ TEST( GaussianFactor, linearFactor ) JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2)); // create a small linear factor graph - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph - JacobianFactor::shared_ptr lf = fg[1]; + JacobianFactor::shared_ptr lf = + boost::dynamic_pointer_cast(fg[1]); // check if the two factors are the same EXPECT(assert_equal(expected,*lf)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, keys ) -//{ -// // get the factor kf2 from the small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianFactor::shared_ptr lf = fg[1]; -// list expected; -// expected.push_back(kx1); -// expected.push_back(kx2); -// EXPECT(lf->keys() == expected); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, dimensions ) -//{ -// // get the factor kf2 from the small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // Check a single factor -// Dimensions expected; -// insert(expected)(kx1, 2)(kx2, 2); -// Dimensions actual = fg[1]->dimensions(); -// EXPECT(expected==actual); -//} - /* ************************************************************************* */ TEST( GaussianFactor, getDim ) { @@ -110,62 +84,6 @@ TEST( GaussianFactor, getDim ) EXPECT_LONGS_EQUAL(expected, actual); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, combine ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // 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 -// GaussianFactor combined(lfg); -// -// // sigmas -// double sigma2 = 0.1; -// double sigma4 = 0.2; -// Vector sigmas = Vector_(4, sigma4, sigma4, sigma2, sigma2); -// -// // the expected combined linear factor -// Matrix Ax2 = Matrix_(4, 2, // x2 -// -5., 0., -// +0., -5., -// 10., 0., -// +0., 10.); -// -// Matrix Al1 = Matrix_(4, 2, // l1 -// 5., 0., -// 0., 5., -// 0., 0., -// 0., 0.); -// -// Matrix Ax1 = Matrix_(4, 2, // x1 -// 0.00, 0., // f4 -// 0.00, 0., // f4 -// -10., 0., // f2 -// 0.00, -10. // f2 -// ); -// -// // the RHS -// Vector b2(4); -// b2(0) = -1.0; -// b2(1) = 1.5; -// b2(2) = 2.0; -// b2(3) = -1.0; -// -// // use general constructor for making arbitrary factors -// vector > meas; -// meas.push_back(make_pair(kx2, Ax2)); -// meas.push_back(make_pair(kl1, Al1)); -// meas.push_back(make_pair(kx1, Ax1)); -// GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4))); -// EXPECT(assert_equal(expected,combined)); -//} - /* ************************************************************************* */ TEST( GaussianFactor, error ) { @@ -186,54 +104,13 @@ TEST( GaussianFactor, error ) DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, eliminate ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // 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 -// GaussianFactor combined(lfg); -// -// // eliminate the combined factor -// GaussianConditional::shared_ptr actualCG; -// GaussianFactor::shared_ptr actualLF; -// boost::tie(actualCG,actualLF) = combined.eliminate(kx2); -// -// // create expected Conditional Gaussian -// Matrix I = eye(2)*sqrt(125.0); -// Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I; -// Vector d = I*Vector_(2,0.2,-0.14); -// -// // Check the conditional Gaussian -// GaussianConditional -// expectedCG(kx2, d, R11, kl1, S12, kx1, S13, repeat(2, 1.0)); -// -// // the expected linear factor -// I = eye(2)/0.2236; -// Matrix Bl1 = I, Bx1 = -I; -// Vector b1 = I*Vector_(2,0.0,0.2); -// -// GaussianFactor expectedLF(kl1, Bl1, kx1, Bx1, b1, repeat(2,1.0)); -// -// // check if the result matches -// EXPECT(assert_equal(expectedCG,*actualCG,1e-3)); -// EXPECT(assert_equal(expectedLF,*actualLF,1e-3)); -//} - /* ************************************************************************* */ TEST( GaussianFactor, matrix ) { const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version @@ -282,7 +159,7 @@ TEST( GaussianFactor, matrix_aug ) const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // create a small linear factor graph Ordering ordering; ordering += kx1,kx2,kl1; - FactorGraph fg = example::createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering); // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; @@ -328,66 +205,6 @@ void print(const list& i) { cout << endl; } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, sparse ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get the factor kf2 from the factor graph -// GaussianFactor::shared_ptr lf = fg[1]; -// -// // render with a given ordering -// Ordering ord; -// ord += kx1,kx2; -// -// list i,j; -// list s; -// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); -// -// list i1,j1; -// i1 += 1,2,1,2; -// j1 += 1,2,3,4; -// -// list s1; -// s1 += -10,-10,10,10; -// -// EXPECT(i==i1); -// EXPECT(j==j1); -// EXPECT(s==s1); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, sparse2 ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get the factor kf2 from the factor graph -// GaussianFactor::shared_ptr lf = fg[1]; -// -// // render with a given ordering -// Ordering ord; -// ord += kx2,kl1,kx1; -// -// list i,j; -// list s; -// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); -// -// list i1,j1; -// i1 += 1,2,1,2; -// j1 += 5,6,1,2; -// -// list s1; -// s1 += -10,-10,10,10; -// -// EXPECT(i==i1); -// EXPECT(j==j1); -// EXPECT(s==s1); -//} - /* ************************************************************************* */ TEST( GaussianFactor, size ) { diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 20972add9..306c64fb7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,143 +56,17 @@ TEST( GaussianFactorGraph, equals ) { } /* ************************************************************************* */ -//TEST( GaussianFactorGraph, error ) { -// Ordering ordering; ordering += X(1),X(2),L(1); -// FactorGraph fg = createGaussianFactorGraph(ordering); -// VectorValues cfg = createZeroDelta(ordering); -// -// // note the error is the same as in testNonlinearFactorGraph as a -// // zero delta config in the linear graph is equivalent to noisy in -// // non-linear, which is really linear under the hood -// double actual = fg.error(cfg); -// DOUBLES_EQUAL( 5.625, actual, 1e-9 ); -//} +TEST( GaussianFactorGraph, error ) { + Ordering ordering; ordering += X(1),X(2),L(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + VectorValues cfg = createZeroDelta(ordering); -/* ************************************************************************* */ -/* unit test for find seperator */ -/* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, find_separator ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// set separator = fg.find_separator(X(2)); -// set expected; -// expected.insert(X(1)); -// expected.insert(L(1)); -// -// EXPECT(separator.size()==expected.size()); -// set::iterator it1 = separator.begin(), it2 = expected.begin(); -// for(; it1!=separator.end(); it1++, it2++) -// EXPECT(*it1 == *it2); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, combine_factors_x1 ) -//{ -// // create a small example for a linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1)); -// -// // the expected linear factor -// Matrix Al1 = Matrix_(6,2, -// 0., 0., -// 0., 0., -// 0., 0., -// 0., 0., -// 5., 0., -// 0., 5. -// ); -// -// Matrix Ax1 = Matrix_(6,2, -// 10., 0., -// 0., 10., -// -10., 0., -// 0.,-10., -// -5., 0., -// 0.,-5. -// ); -// -// Matrix Ax2 = Matrix_(6,2, -// 0., 0., -// 0., 0., -// 10., 0., -// 0., 10., -// 0., 0., -// 0., 0. -// ); -// -// // the expected RHS vector -// Vector b(6); -// b(0) = -1; -// b(1) = -1; -// b(2) = 2; -// b(3) = -1; -// b(4) = 0; -// b(5) = 1; -// -// vector > meas; -// meas.push_back(make_pair(L(1), Al1)); -// meas.push_back(make_pair(X(1), Ax1)); -// meas.push_back(make_pair(X(2), Ax2)); -// GaussianFactor expected(meas, b, ones(6)); -// //GaussianFactor expected(L(1), Al1, X(1), Ax1, X(2), Ax2, b); -// -// // check if the two factors are the same -// EXPECT(assert_equal(expected,*actual)); -//} -// -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, combine_factors_x2 ) -//{ -// // create a small example for a linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2)); -// -// // the expected linear factor -// Matrix Al1 = Matrix_(4,2, -// // l1 -// 0., 0., -// 0., 0., -// 5., 0., -// 0., 5. -// ); -// -// Matrix Ax1 = Matrix_(4,2, -// // x1 -// -10., 0., // f2 -// 0.,-10., // f2 -// 0., 0., // f4 -// 0., 0. // f4 -// ); -// -// Matrix Ax2 = Matrix_(4,2, -// // x2 -// 10., 0., -// 0., 10., -// -5., 0., -// 0.,-5. -// ); -// -// // the expected RHS vector -// Vector b(4); -// b(0) = 2; -// b(1) = -1; -// b(2) = -1; -// b(3) = 1.5; -// -// vector > meas; -// meas.push_back(make_pair(L(1), Al1)); -// meas.push_back(make_pair(X(1), Ax1)); -// meas.push_back(make_pair(X(2), Ax2)); -// GaussianFactor expected(meas, b, ones(4)); -// -// // check if the two factors are the same -// EXPECT(assert_equal(expected,*actual)); -//} + // note the error is the same as in testNonlinearFactorGraph as a + // zero delta config in the linear graph is equivalent to noisy in + // non-linear, which is really linear under the hood + double actual = fg.error(cfg); + DOUBLES_EQUAL( 5.625, actual, 1e-9 ); +} /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateOne_x1 ) @@ -336,47 +210,6 @@ TEST( GaussianFactorGraph, eliminateAll ) EXPECT(assert_equal(expected,actualQR,tol)); } -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateAll_fast ) -//{ -// // create expected Chordal bayes Net -// Matrix I = eye(2); -// -// Vector d1 = Vector_(2, -0.1,-0.1); -// GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1); -// -// double sig1 = 0.149071; -// Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); -// push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2); -// -// double sig2 = 0.0894427; -// Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); -// push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3); -// -// // Check one ordering -// GaussianFactorGraph fg1 = createGaussianFactorGraph(); -// Ordering ordering; -// ordering += X(2),L(1),X(1); -// GaussianBayesNet actual = fg1.eliminate(ordering, false); -// EXPECT(assert_equal(expected,actual,tol)); -//} - -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, add_priors ) -//{ -// Ordering ordering; ordering += L(1),X(1),X(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianFactorGraph actual = fg.add_priors(3, vector(3,2)); -// GaussianFactorGraph expected = createGaussianFactorGraph(ordering); -// Matrix A = eye(2); -// Vector b = zero(2); -// SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2,3.0); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma))); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, copying ) { @@ -397,46 +230,6 @@ TEST( GaussianFactorGraph, copying ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, matrix ) -//{ -// // render with a given ordering -// Ordering ord; -// ord += X(2),L(1),X(1); -// -// // Create a graph -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// Matrix A; Vector b; -// boost::tie(A,b) = fg.matrix(); -// -// Matrix A1 = Matrix_(2*4,3*2, -// +0., 0., 0., 0., 10., 0., // unary factor on x1 (prior) -// +0., 0., 0., 0., 0., 10., -// 10., 0., 0., 0.,-10., 0., // binary factor on x2,x1 (odometry) -// +0., 10., 0., 0., 0.,-10., -// +0., 0., 5., 0., -5., 0., // binary factor on l1,x1 (z1) -// +0., 0., 0., 5., 0., -5., -// -5., 0., 5., 0., 0., 0., // binary factor on x2,l1 (z2) -// +0., -5., 0., 5., 0., 0. -// ); -// Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5); -// -// EQUALITY(A,A1); -// EXPECT(b==b1); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, sizeOfA ) -//{ -// // create a small linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// pair mn = fg.sizeOfA(); -// EXPECT(8 == mn.first); -// EXPECT(6 == mn.second); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { @@ -451,11 +244,6 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) GaussianFactorGraph fg2(CBN); GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate(); EXPECT(assert_equal(CBN,CBN2)); - - // Base FactorGraph only -// FactorGraph fg3(CBN); -// GaussianBayesNet CBN3 = gtsam::eliminate(fg3,ord); -// EXPECT(assert_equal(CBN,CBN3)); } /* ************************************************************************* */ @@ -469,16 +257,6 @@ TEST( GaussianFactorGraph, getOrdering) EXPECT(assert_equal(expected,actual)); } -// SL-FIX TEST( GaussianFactorGraph, getOrdering2) -//{ -// Ordering expected; -// expected += L(1),X(1); -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// set interested; interested += L(1),X(1); -// Ordering actual = fg.getOrdering(interested); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, optimize_Cholesky ) { @@ -515,24 +293,6 @@ TEST( GaussianFactorGraph, optimize_QR ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// // create a graph -// GaussianFactorGraph fg = createGaussianFactorGraph(ord); -// -// // optimize the graph -// VectorValues actual = fg.optimizeMultiFrontals(ord); -// -// // verify -// VectorValues expected = createCorrectDelta(); -// -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, combine) { @@ -585,48 +345,6 @@ void print(vector v) { cout << endl; } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, factor_lookup) -//{ -// // create a test graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // ask for all factor indices connected to x1 -// list x1_factors = fg.factors(X(1)); -// size_t x1_indices[] = { 0, 1, 2 }; -// list x1_expected(x1_indices, x1_indices + 3); -// EXPECT(x1_factors==x1_expected); -// -// // ask for all factor indices connected to x2 -// list x2_factors = fg.factors(X(2)); -// size_t x2_indices[] = { 1, 3 }; -// list x2_expected(x2_indices, x2_indices + 2); -// EXPECT(x2_factors==x2_expected); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, findAndRemoveFactors ) -//{ -// // create the graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // We expect to remove these three factors: 0, 1, 2 -// GaussianFactor::shared_ptr f0 = fg[0]; -// GaussianFactor::shared_ptr f1 = fg[1]; -// GaussianFactor::shared_ptr f2 = fg[2]; -// -// // call the function -// vector factors = fg.findAndRemoveFactors(X(1)); -// -// // Check the factors -// EXPECT(f0==factors[0]); -// EXPECT(f1==factors[1]); -// EXPECT(f2==factors[2]); -// -// // EXPECT if the factors are deleted from the factor graph -// LONGS_EQUAL(1,fg.nrFactors()); -//} - /* ************************************************************************* */ TEST(GaussianFactorGraph, createSmoother) { @@ -636,35 +354,6 @@ TEST(GaussianFactorGraph, createSmoother) LONGS_EQUAL(5,fg2.size()); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, variables ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Dimensions expected; -// insert(expected)(L(1), 2)(X(1), 2)(X(2), 2); -// Dimensions actual = fg.dimensions(); -// EXPECT(expected==actual); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, keys ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Ordering expected; -// expected += L(1),X(1),X(2); -// EXPECT(assert_equal(expected,fg.keys())); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, involves ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// EXPECT(fg.involves(L(1))); -// EXPECT(fg.involves(X(1))); -// EXPECT(fg.involves(X(2))); -// EXPECT(!fg.involves(X(3))); -//} - /* ************************************************************************* */ double error(const VectorValues& x) { // create an ordering @@ -674,45 +363,13 @@ double error(const VectorValues& x) { return fg.error(x); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, gradient ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // Construct expected gradient -// VectorValues expected; -// -// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 -// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] -// expected.insert(L(1),Vector_(2, 5.0,-12.5)); -// expected.insert(X(1),Vector_(2, 30.0, 5.0)); -// expected.insert(X(2),Vector_(2,-25.0, 17.5)); -// -// // Check the gradient at delta=0 -// VectorValues zero = createZeroDelta(); -// VectorValues actual = fg.gradient(zero); -// EXPECT(assert_equal(expected,actual)); -// -// // Check it numerically for good measure -// Vector numerical_g = numericalGradient(error,zero,0.001); -// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g)); -// -// // Check the gradient at the solution (should be zero) -// Ordering ord; -// ord += X(2),L(1),X(1); -// GaussianFactorGraph fg2 = createGaussianFactorGraph(); -// VectorValues solution = fg2.optimize(ord); // destructive -// VectorValues actual2 = fg.gradient(solution); -// EXPECT(assert_equal(zero,actual2)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, multiplication ) { // create an ordering Ordering ord; ord += X(2),L(1),X(1); - FactorGraph A = createGaussianFactorGraph(ord); + GaussianFactorGraph A = createGaussianFactorGraph(ord); VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected; @@ -723,41 +380,6 @@ TEST( GaussianFactorGraph, multiplication ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// GaussianFactorGraph A = createGaussianFactorGraph(ord); -// Errors e; -// e += Vector_(2, 0.0, 0.0); -// e += Vector_(2,15.0, 0.0); -// e += Vector_(2, 0.0,-5.0); -// e += Vector_(2,-7.5,-5.0); -// -// VectorValues expected = createZeroDelta(ord), actual = A ^ e; -// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0); -// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0); -// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0); -// EXPECT(assert_equal(expected,actual)); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, rhs ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// GaussianFactorGraph Ab = createGaussianFactorGraph(ord); -// Errors expected = createZeroDelta(ord), actual = Ab.rhs(); -// expected.push_back(Vector_(2,-1.0,-1.0)); -// expected.push_back(Vector_(2, 2.0,-1.0)); -// expected.push_back(Vector_(2, 0.0, 1.0)); -// expected.push_back(Vector_(2,-1.0, 1.5)); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ // Extra test on elimination prompted by Michael's email to Frank 1/4/2010 TEST( GaussianFactorGraph, elimination ) @@ -824,22 +446,6 @@ TEST( GaussianFactorGraph, constrained_single ) EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//SL-FIX TEST( GaussianFactorGraph, constrained_single2 ) -//{ -// // get a graph with a constraint in it -// GaussianFactorGraph fg = createSingleConstraintGraph(); -// -// // eliminate and solve -// Ordering ord; -// ord += "yk, x"; -// VectorValues actual = fg.optimize(ord); -// -// // verify -// VectorValues expected = createSingleConstraintValues(); -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, constrained_multi1 ) { @@ -855,68 +461,9 @@ TEST( GaussianFactorGraph, constrained_multi1 ) EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//SL-FIX TEST( GaussianFactorGraph, constrained_multi2 ) -//{ -// // get a graph with a constraint in it -// GaussianFactorGraph fg = createMultiConstraintGraph(); -// -// // eliminate and solve -// Ordering ord; -// ord += "zk, xk, y"; -// VectorValues actual = fg.optimize(ord); -// -// // verify -// VectorValues expected = createMultiConstraintValues(); -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ -SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); - -// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); -// g.add(X(3), I, X(4), I, b, model); -// -// map tree = g.findMinimumSpanningTree(); -// EXPECT(tree[X(1)].compare(X(1))==0); -// EXPECT(tree[X(2)].compare(X(1))==0); -// EXPECT(tree[X(3)].compare(X(1))==0); -// EXPECT(tree[X(4)].compare(X(1))==0); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, split ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); -// -// PredecessorMap tree; -// tree[X(1)] = X(1); -// tree[X(2)] = X(1); -// tree[X(3)] = X(1); -// tree[X(4)] = X(1); -// -// GaussianFactorGraph Ab1, Ab2; -// g.split(tree, Ab1, Ab2); -// LONGS_EQUAL(3, Ab1.size()); -// LONGS_EQUAL(2, Ab2.size()); -//} +static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); /* ************************************************************************* */ TEST(GaussianFactorGraph, replace) @@ -935,91 +482,18 @@ TEST(GaussianFactorGraph, replace) GaussianFactorGraph actual; actual.push_back(f1); -// actual.checkGraphConsistency(); actual.push_back(f2); -// actual.checkGraphConsistency(); actual.push_back(f3); -// actual.checkGraphConsistency(); actual.replace(0, f4); -// actual.checkGraphConsistency(); GaussianFactorGraph expected; expected.push_back(f4); -// actual.checkGraphConsistency(); expected.push_back(f2); -// actual.checkGraphConsistency(); expected.push_back(f3); -// actual.checkGraphConsistency(); EXPECT(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 += X(2), L(1), X(1); -// 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); -// -// EXPECT(assert_equal(expAb, Ab)); -// EXPECT(assert_equal(*expModel, *noise)); -//} - -/* ************************************************************************* */ -/* - * x2 x1 x3 b - * 1 1 1 1 1 0 1 - * 1 1 1 -> 1 1 1 - * 1 1 1 1 - */ -// SL-NEEDED? TEST ( GaussianFactorGraph, eliminateFrontals ) { -// typedef GaussianFactorGraph::sharedFactor Factor; -// SharedDiagonal model(Vector_(1, 0.5)); -// GaussianFactorGraph fg; -// Factor factor1(new JacobianFactor(X(1), Matrix_(1,1,1.), X(2), Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor2(new JacobianFactor(X(2), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor3(new JacobianFactor(X(3), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model)); -// fg.push_back(factor1); -// fg.push_back(factor2); -// fg.push_back(factor3); -// -// Ordering frontals; frontals += X(2), X(1); -// GaussianBayesNet bn = fg.eliminateFrontals(frontals); -// -// GaussianBayesNet bn_expected; -// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.), -// X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); -// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.), -// X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); -// bn_expected.push_back(conditional1); -// bn_expected.push_back(conditional2); -// EXPECT(assert_equal(bn_expected, bn)); -// -// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(X(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.)))); -// GaussianFactorGraph fg_expected; -// fg_expected.push_back(factor_expected); -// EXPECT(assert_equal(fg_expected, fg)); -//} - /* ************************************************************************* */ TEST(GaussianFactorGraph, createSmoother2) { @@ -1049,7 +523,7 @@ TEST(GaussianFactorGraph, hasConstraints) EXPECT(hasConstraints(fgc2)); Ordering ordering; ordering += X(1), X(2), L(1); - FactorGraph fg = createGaussianFactorGraph(ordering); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(!hasConstraints(fg)); } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 7ec4e5317..74417b57a 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -42,12 +42,12 @@ using symbol_shorthand::L; // Some numbers that should be consistent among all smoother tests static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = - 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; + 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1; static const double tol = 1e-4; /* ************************************************************************* */ -TEST_UNSAFE( ISAM, iSAM_smoother ) +TEST( ISAM, iSAM_smoother ) { Ordering ordering; for (int t = 1; t <= 7; t++) ordering += X(t); @@ -76,31 +76,6 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) EXPECT(assert_equal(e, optimized)); } -/* ************************************************************************* */ -// SL-FIX TEST( ISAM, iSAM_smoother2 ) -//{ -// // Create smoother with 7 nodes -// GaussianFactorGraph smoother = createSmoother(7); -// -// // Create initial tree from first 4 timestamps in reverse order ! -// Ordering ord; ord += X(4),X(3),X(2),X(1); -// GaussianFactorGraph factors1; -// for (int i=0;i<7;i++) factors1.push_back(smoother[i]); -// GaussianISAM actual(*inference::Eliminate(factors1)); -// -// // run iSAM with remaining factors -// GaussianFactorGraph factors2; -// for (int i=7;i<13;i++) factors2.push_back(smoother[i]); -// actual.update(factors2); -// -// // Create expected Bayes Tree by solving smoother with "natural" ordering -// Ordering ordering; -// for (int t = 1; t <= 7; t++) ordering += symbol('x', t); -// GaussianISAM expected(smoother.eliminate(ordering)); -// -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* * Bayes tree for smoother with "natural" ordering: C1 x6 x7 diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 67fa287d3..c4b90d882 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 93ecd6dbe..14655a249 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -105,6 +105,108 @@ TEST( Graph, composePoses ) CHECK(assert_equal(expected, *actual)); } +// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) +//{ +// GaussianFactorGraph g; +// Matrix I = eye(2); +// Vector b = Vector_(0, 0, 0); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); +// g.add(X(3), I, X(4), I, b, model); +// +// map tree = g.findMinimumSpanningTree(); +// EXPECT(tree[X(1)].compare(X(1))==0); +// EXPECT(tree[X(2)].compare(X(1))==0); +// EXPECT(tree[X(3)].compare(X(1))==0); +// EXPECT(tree[X(4)].compare(X(1))==0); +//} + +///* ************************************************************************* */ +// SL-FIX TEST( GaussianFactorGraph, split ) +//{ +// GaussianFactorGraph g; +// Matrix I = eye(2); +// Vector b = Vector_(0, 0, 0); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); +// +// PredecessorMap tree; +// tree[X(1)] = X(1); +// tree[X(2)] = X(1); +// tree[X(3)] = X(1); +// tree[X(4)] = X(1); +// +// GaussianFactorGraph Ab1, Ab2; +// g.split(tree, Ab1, Ab2); +// LONGS_EQUAL(3, Ab1.size()); +// LONGS_EQUAL(2, Ab2.size()); +//} + +///* ************************************************************************* */ +// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) +//{ +// SymbolicFactorGraph G; +// G.push_factor("x1", "x2"); +// G.push_factor("x1", "x3"); +// G.push_factor("x1", "x4"); +// G.push_factor("x2", "x3"); +// G.push_factor("x2", "x4"); +// G.push_factor("x3", "x4"); +// +// SymbolicFactorGraph T, C; +// boost::tie(T, C) = G.splitMinimumSpanningTree(); +// +// SymbolicFactorGraph expectedT, expectedC; +// expectedT.push_factor("x1", "x2"); +// expectedT.push_factor("x1", "x3"); +// expectedT.push_factor("x1", "x4"); +// expectedC.push_factor("x2", "x3"); +// expectedC.push_factor("x2", "x4"); +// expectedC.push_factor("x3", "x4"); +// CHECK(assert_equal(expectedT,T)); +// CHECK(assert_equal(expectedC,C)); +//} + +///* ************************************************************************* */ +///** +// * x1 - x2 - x3 - x4 - x5 +// * | | / | +// * l1 l2 l3 +// */ +// SL-FIX TEST( FactorGraph, removeSingletons ) +//{ +// SymbolicFactorGraph G; +// G.push_factor("x1", "x2"); +// G.push_factor("x2", "x3"); +// G.push_factor("x3", "x4"); +// G.push_factor("x4", "x5"); +// G.push_factor("x2", "l1"); +// G.push_factor("x3", "l2"); +// G.push_factor("x4", "l2"); +// G.push_factor("x4", "l3"); +// +// SymbolicFactorGraph singletonGraph; +// set singletons; +// boost::tie(singletonGraph, singletons) = G.removeSingletons(); +// +// set singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3"; +// CHECK(singletons_excepted == singletons); +// +// SymbolicFactorGraph singletonGraph_excepted; +// singletonGraph_excepted.push_factor("x2", "l1"); +// singletonGraph_excepted.push_factor("x4", "l3"); +// singletonGraph_excepted.push_factor("x1", "x2"); +// singletonGraph_excepted.push_factor("x4", "x5"); +// singletonGraph_excepted.push_factor("x2", "x3"); +// CHECK(singletonGraph_excepted.equals(singletonGraph)); +//} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp new file mode 100644 index 000000000..bfa3a8e79 --- /dev/null +++ b/tests/testIterative.cpp @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testIterative.cpp + * @brief Unit tests for iterative methods + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace example; +using symbol_shorthand::X; // to create pose keys +using symbol_shorthand::L; // to create landmark keys + +static ConjugateGradientParameters parameters; +// add following below to add printing: +// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY; + +/* ************************************************************************* */ +TEST( Iterative, steepestDescent ) +{ + // Create factor graph + Ordering ordering; + ordering += L(1), X(1), X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + + // eliminate and solve + VectorValues expected = *GaussianSequentialSolver(fg).optimize(); + + // Do gradient descent + VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally? + VectorValues actual = steepestDescent(fg, zero, parameters); + CHECK(assert_equal(expected,actual,1e-2)); +} + +/* ************************************************************************* */ +TEST( Iterative, conjugateGradientDescent ) +{ + // Create factor graph + Ordering ordering; + ordering += L(1), X(1), X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + + // eliminate and solve + VectorValues expected = *GaussianSequentialSolver(fg).optimize(); + + // get matrices + Matrix A; + Vector b; + Vector x0 = gtsam::zero(6); + boost::tie(A, b) = fg.jacobian(); + Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); + + // Do conjugate gradient descent, System version + System Ab(A, b); + Vector actualX = conjugateGradientDescent(Ab, x0, parameters); + CHECK(assert_equal(expectedX,actualX,1e-9)); + + // Do conjugate gradient descent, Matrix version + Vector actualX2 = conjugateGradientDescent(A, b, x0, parameters); + CHECK(assert_equal(expectedX,actualX2,1e-9)); + + // Do conjugate gradient descent on factor graph + VectorValues zero = VectorValues::Zero(expected); + VectorValues actual = conjugateGradientDescent(fg, zero, parameters); + CHECK(assert_equal(expected,actual,1e-2)); +} + +/* ************************************************************************* */ +TEST( Iterative, conjugateGradientDescent_hard_constraint ) +{ + Values config; + Pose2 pose1 = Pose2(0.,0.,0.); + config.insert(X(1), pose1); + config.insert(X(2), Pose2(1.5,0.,0.)); + + NonlinearFactorGraph graph; + graph.add(NonlinearEquality(X(1), pose1)); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); + + Ordering ordering; + ordering += X(1), X(2); + boost::shared_ptr fg = graph.linearize(config,ordering); + + VectorValues zeros = VectorValues::Zero(2, 3); + + ConjugateGradientParameters parameters; + parameters.setEpsilon_abs(1e-3); + parameters.setEpsilon_rel(1e-5); + parameters.setMaxIterations(100); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); + + VectorValues expected; + expected.insert(0, zero(3)); + expected.insert(1, Vector_(3,-0.5,0.,0.)); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( Iterative, conjugateGradientDescent_soft_constraint ) +{ + Values config; + config.insert(X(1), Pose2(0.,0.,0.)); + config.insert(X(2), Pose2(1.5,0.,0.)); + + NonlinearFactorGraph graph; + graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); + + Ordering ordering; + ordering += X(1), X(2); + boost::shared_ptr fg = graph.linearize(config,ordering); + + VectorValues zeros = VectorValues::Zero(2, 3); + + ConjugateGradientParameters parameters; + parameters.setEpsilon_abs(1e-3); + parameters.setEpsilon_rel(1e-5); + parameters.setMaxIterations(100); + VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); + + VectorValues expected; + expected.insert(0, zero(3)); + expected.insert(1, Vector_(3,-0.5,0.,0.)); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index b1717a79b..c8824749d 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -73,8 +73,8 @@ TEST( NonlinearFactor, equals2 ) Graph::sharedFactor f0 = fg[0], f1 = fg[1]; CHECK(f0->equals(*f0)); -// SL-FIX CHECK(!f0->equals(*f1)); -// SL-FIX CHECK(!f1->equals(*f0)); + CHECK(!f0->equals(*f1)); + CHECK(!f1->equals(*f0)); } /* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 417643e78..39c5e2b70 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -17,8 +17,9 @@ #include #include +#include #include -#include +#include #include #include #include @@ -35,19 +36,30 @@ using namespace gtsam; using namespace example; // define keys -Key i3003 = 3003, i2003 = 2003, i1003 = 1003; -Key i3002 = 3002, i2002 = 2002, i1002 = 1002; -Key i3001 = 3001, i2001 = 2001, i1001 = 1001; +// Create key for simulated planar graph +Symbol key(int x, int y) { + return symbol_shorthand::X(1000*x+y); +} -// TODO fix Ordering::equals, because the ordering *is* correct ! /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, planarOrdering ) -//{ -// // Check canonical ordering -// Ordering expected, ordering = planarOrdering(3); -// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; -// CHECK(assert_equal(expected,ordering)); -//} +TEST( SubgraphPreconditioner, planarOrdering ) { + // Check canonical ordering + Ordering expected, ordering = planarOrdering(3); + expected += + key(3, 3), key(2, 3), key(1, 3), + key(3, 2), key(2, 2), key(1, 2), + key(3, 1), key(2, 1), key(1, 1); + CHECK(assert_equal(expected,ordering)); +} + +/* ************************************************************************* */ +/** unnormalized error */ +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { + double total_error = 0.; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) + total_error += factor->error(x); + return total_error; +} /* ************************************************************************* */ TEST( SubgraphPreconditioner, planarGraph ) @@ -58,7 +70,7 @@ TEST( SubgraphPreconditioner, planarGraph ) boost::tie(A, xtrue) = planarGraph(3); LONGS_EQUAL(13,A.size()); LONGS_EQUAL(9,xtrue.size()); - DOUBLES_EQUAL(0,A.error(xtrue),1e-9); // check zero error for xtrue + DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue // Check that xtrue is optimal GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); @@ -67,143 +79,143 @@ TEST( SubgraphPreconditioner, planarGraph ) } /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, splitOffPlanarTree ) -//{ -// // Build a planar graph -// GaussianFactorGraph A; -// VectorValues xtrue; -// boost::tie(A, xtrue) = planarGraph(3); -// -// // Get the spanning tree and constraints, and check their sizes -// JacobianFactorGraph T, C; -// // TODO big mess: GFG and JFG mess !!! -// boost::tie(T, C) = splitOffPlanarTree(3, A); -// LONGS_EQUAL(9,T.size()); -// LONGS_EQUAL(4,C.size()); -// -// // Check that the tree can be solved to give the ground xtrue -// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); -// VectorValues xbar = optimize(*R1); -// CHECK(assert_equal(xtrue,xbar)); -//} +TEST( SubgraphPreconditioner, splitOffPlanarTree ) +{ + // Build a planar graph + GaussianFactorGraph A; + VectorValues xtrue; + boost::tie(A, xtrue) = planarGraph(3); + + // Get the spanning tree and constraints, and check their sizes + GaussianFactorGraph T, C; + boost::tie(T, C) = splitOffPlanarTree(3, A); + LONGS_EQUAL(9,T.size()); + LONGS_EQUAL(4,C.size()); + + // Check that the tree can be solved to give the ground xtrue + GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); + VectorValues xbar = optimize(*R1); + CHECK(assert_equal(xtrue,xbar)); +} /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, system ) -//{ -// // Build a planar graph -// JacobianFactorGraph Ab; -// VectorValues xtrue; -// size_t N = 3; -// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b -// -// // Get the spanning tree and corresponding ordering -// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 -// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); -// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); -// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); -// -// // Eliminate the spanning tree to build a prior -// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 -// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 -// -// // Create Subgraph-preconditioned system -// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); -// -// // Create zero config -// VectorValues zeros = VectorValues::Zero(xbar); -// -// // Set up y0 as all zeros -// VectorValues y0 = zeros; -// -// // y1 = perturbed y0 -// VectorValues y1 = zeros; -// y1[i2003] = Vector_(2, 1.0, -1.0); -// -// // Check corresponding x values -// VectorValues expected_x1 = xtrue, x1 = system.x(y1); -// expected_x1[i2003] = Vector_(2, 2.01, 2.99); -// expected_x1[i3003] = Vector_(2, 3.01, 2.99); -// CHECK(assert_equal(xtrue, system.x(y0))); -// CHECK(assert_equal(expected_x1,system.x(y1))); -// -// // Check errors -//// DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO ! -//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! -// DOUBLES_EQUAL(0,error(system,y0),1e-9); -// DOUBLES_EQUAL(3,error(system,y1),1e-9); -// -// // Test gradient in x -// VectorValues expected_gx0 = zeros; -// VectorValues expected_gx1 = zeros; -// CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); -// expected_gx1[i1003] = Vector_(2, -100., 100.); -// expected_gx1[i2002] = Vector_(2, -100., 100.); -// expected_gx1[i2003] = Vector_(2, 200., -200.); -// expected_gx1[i3002] = Vector_(2, -100., 100.); -// expected_gx1[i3003] = Vector_(2, 100., -100.); -// CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); -// -// // Test gradient in y -// VectorValues expected_gy0 = zeros; -// VectorValues expected_gy1 = zeros; -// expected_gy1[i1003] = Vector_(2, 2., -2.); -// expected_gy1[i2002] = Vector_(2, -2., 2.); -// expected_gy1[i2003] = Vector_(2, 3., -3.); -// expected_gy1[i3002] = Vector_(2, -1., 1.); -// expected_gy1[i3003] = Vector_(2, 1., -1.); -// CHECK(assert_equal(expected_gy0,gradient(system,y0))); -// CHECK(assert_equal(expected_gy1,gradient(system,y1))); -// -// // Check it numerically for good measure -// // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) -// // Vector numerical_g1 = numericalGradient (error, y1, 0.001); -// // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., -// // 3., -3., 0., 0., -1., 1., 1., -1.); -// // CHECK(assert_equal(expected_g1,numerical_g1)); -//} + +TEST( SubgraphPreconditioner, system ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); + + // Eliminate the spanning tree to build a prior + SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + SubgraphPreconditioner system(Ab2, Rc1, xbarShared); + + // Create zero config + VectorValues zeros = VectorValues::Zero(xbar); + + // Set up y0 as all zeros + VectorValues y0 = zeros; + + // y1 = perturbed y0 + VectorValues y1 = zeros; + y1[1] = Vector_(2, 1.0, -1.0); + + // Check corresponding x values + VectorValues expected_x1 = xtrue, x1 = system.x(y1); + expected_x1[1] = Vector_(2, 2.01, 2.99); + expected_x1[0] = Vector_(2, 3.01, 2.99); + CHECK(assert_equal(xtrue, system.x(y0))); + CHECK(assert_equal(expected_x1,system.x(y1))); + + // Check errors + DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); + DOUBLES_EQUAL(3,error(Ab,x1),1e-9); + DOUBLES_EQUAL(0,error(system,y0),1e-9); + DOUBLES_EQUAL(3,error(system,y1),1e-9); + + // Test gradient in x + VectorValues expected_gx0 = zeros; + VectorValues expected_gx1 = zeros; + CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); + expected_gx1[2] = Vector_(2, -100., 100.); + expected_gx1[4] = Vector_(2, -100., 100.); + expected_gx1[1] = Vector_(2, 200., -200.); + expected_gx1[3] = Vector_(2, -100., 100.); + expected_gx1[0] = Vector_(2, 100., -100.); + CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); + + // Test gradient in y + VectorValues expected_gy0 = zeros; + VectorValues expected_gy1 = zeros; + expected_gy1[2] = Vector_(2, 2., -2.); + expected_gy1[4] = Vector_(2, -2., 2.); + expected_gy1[1] = Vector_(2, 3., -3.); + expected_gy1[3] = Vector_(2, -1., 1.); + expected_gy1[0] = Vector_(2, 1., -1.); + CHECK(assert_equal(expected_gy0,gradient(system,y0))); + CHECK(assert_equal(expected_gy1,gradient(system,y1))); + + // Check it numerically for good measure + // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) + // Vector numerical_g1 = numericalGradient (error, y1, 0.001); + // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., + // 3., -3., 0., 0., -1., 1., 1., -1.); + // CHECK(assert_equal(expected_g1,numerical_g1)); +} /* ************************************************************************* */ -//TEST( SubgraphPreconditioner, conjugateGradients ) -//{ -// // Build a planar graph -// GaussianFactorGraph Ab; -// VectorValues xtrue; -// size_t N = 3; -// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b -// -// // Get the spanning tree and corresponding ordering -// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 -// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); -// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); -// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); -// -// // Eliminate the spanning tree to build a prior -// Ordering ordering = planarOrdering(N); -// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 -// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 -// -// // Create Subgraph-preconditioned system -// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible -// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); -// -// // Create zero config y0 and perturbed config y1 -// VectorValues y0 = VectorValues::Zero(xbar); -// -// VectorValues y1 = y0; -// y1[i2003] = Vector_(2, 1.0, -1.0); -// VectorValues x1 = system.x(y1); -// -// // Solve for the remaining constraints using PCG -// ConjugateGradientParameters parameters; -//// VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); -//// CHECK(assert_equal(y0,actual)); -// -// // Compare with non preconditioned version: -// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); -// CHECK(assert_equal(xtrue,actual2,1e-4)); -//} +TEST( SubgraphPreconditioner, conjugateGradients ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); + SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); + + // Eliminate the spanning tree to build a prior + Ordering ordering = planarOrdering(N); + SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 + VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + + // Create Subgraph-preconditioned system + VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + SubgraphPreconditioner system(Ab2, Rc1, xbarShared); + + // Create zero config y0 and perturbed config y1 + VectorValues y0 = VectorValues::Zero(xbar); + + VectorValues y1 = y0; + y1[1] = Vector_(2, 1.0, -1.0); + VectorValues x1 = system.x(y1); + + // Solve for the remaining constraints using PCG + ConjugateGradientParameters parameters; + VectorValues actual = conjugateGradients(system, y1, parameters); + CHECK(assert_equal(y0,actual)); + + // Compare with non preconditioned version: + VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); + CHECK(assert_equal(xtrue,actual2,1e-4)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp new file mode 100644 index 000000000..700f35261 --- /dev/null +++ b/tests/testSubgraphSolver.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSubgraphSolver.cpp + * @brief Unit tests for SubgraphSolver + * @author Yong-Dian Jian + **/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +using namespace boost::assign; + +using namespace std; +using namespace gtsam; +using namespace example; + +/* ************************************************************************* */ +/** unnormalized error */ +static double error(const GaussianFactorGraph& fg, const VectorValues& x) { + double total_error = 0.; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg) + total_error += factor->error(x); + return total_error; +} + + +/* ************************************************************************* */ +TEST( SubgraphSolver, constructor1 ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // The first constructor just takes a factor graph (and parameters) + // and it will split the graph into A1 and A2, where A1 is a spanning tree + SubgraphSolverParameters parameters; + SubgraphSolver solver(Ab, parameters); + VectorValues optimized = solver.optimize(); // does PCG optimization + DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); +} + +/* ************************************************************************* */ +TEST( SubgraphSolver, constructor2 ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + + // The second constructor takes two factor graphs, + // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) + SubgraphSolverParameters parameters; + SubgraphSolver solver(Ab1_, Ab2_, parameters); + VectorValues optimized = solver.optimize(); + DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); +} + +/* ************************************************************************* */ +TEST( SubgraphSolver, constructor3 ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + size_t N = 3; + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + + // Get the spanning tree and corresponding ordering + GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + + // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT + GaussianBayesNet::shared_ptr Rc1 = // + EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + + // The third constructor allows the caller to pass an already solved preconditioner Rc1_ + // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before + SubgraphSolverParameters parameters; + SubgraphSolver solver(Rc1, Ab2_, parameters); + VectorValues optimized = solver.optimize(); + DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/wrap/Class.cpp b/wrap/Class.cpp index c5c306ce6..374324558 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -332,7 +332,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const { string up_name = boost::to_upper_copy(name); - proxyFile.oss << "%" << up_name << "("; + proxyFile.oss << "%" << name << "("; unsigned int i = 0; BOOST_FOREACH(const Argument& arg, argList) { @@ -352,7 +352,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const BOOST_FOREACH(ArgumentList argList, m.argLists) { string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << up_name << "("; + proxyFile.oss << "%" << m.name << "("; unsigned int i = 0; BOOST_FOREACH(const Argument& arg, argList) { @@ -373,7 +373,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const BOOST_FOREACH(ArgumentList argList, m.argLists) { string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << up_name << "("; + proxyFile.oss << "%" << m.name << "("; unsigned int i = 0; BOOST_FOREACH(const Argument& arg, argList) { diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index 913a869b5..d1c336c5f 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -1,15 +1,15 @@ %-------Constructors------- -%POINT2() -%POINT2(double x, double y) +%Point2() +%Point2(double x, double y) % %-------Methods------- -%ARGCHAR(char a) : returns void -%ARGUCHAR(unsigned char a) : returns void -%DIM() : returns int -%RETURNCHAR() : returns char -%VECTORCONFUSION() : returns VectorNotEigen -%X() : returns double -%Y() : returns double +%argChar(char a) : returns void +%argUChar(unsigned char a) : returns void +%dim() : returns int +%returnChar() : returns char +%vectorConfusion() : returns VectorNotEigen +%x() : returns double +%y() : returns double % %-------Static Methods------- % diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 965815dc6..f73b740ac 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -1,12 +1,12 @@ %-------Constructors------- -%POINT3(double x, double y, double z) +%Point3(double x, double y, double z) % %-------Methods------- -%NORM() : returns double +%norm() : returns double % %-------Static Methods------- -%STATICFUNCTIONRET(double z) : returns Point3 -%STATICFUNCTION() : returns double +%StaticFunctionRet(double z) : returns Point3 +%staticFunction() : returns double % %For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html classdef Point3 < handle diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index d39025971..1e07974ee 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -1,27 +1,27 @@ %-------Constructors------- -%TEST() -%TEST(double a, Matrix b) +%Test() +%Test(double a, Matrix b) % %-------Methods------- -%ARG_EIGENCONSTREF(Matrix value) : returns void -%CREATE_MIXEDPTRS() : returns pair< Test, SharedTest > -%CREATE_PTRS() : returns pair< SharedTest, SharedTest > -%PRINT() : returns void -%RETURN_POINT2PTR(bool value) : returns Point2 -%RETURN_TEST(Test value) : returns Test -%RETURN_TESTPTR(Test value) : returns Test -%RETURN_BOOL(bool value) : returns bool -%RETURN_DOUBLE(double value) : returns double -%RETURN_FIELD(Test t) : returns bool -%RETURN_INT(int value) : returns int -%RETURN_MATRIX1(Matrix value) : returns Matrix -%RETURN_MATRIX2(Matrix value) : returns Matrix -%RETURN_PAIR(Vector v, Matrix A) : returns pair< Vector, Matrix > -%RETURN_PTRS(Test p1, Test p2) : returns pair< SharedTest, SharedTest > -%RETURN_SIZE_T(size_t value) : returns size_t -%RETURN_STRING(string value) : returns string -%RETURN_VECTOR1(Vector value) : returns Vector -%RETURN_VECTOR2(Vector value) : returns Vector +%arg_EigenConstRef(Matrix value) : returns void +%create_MixedPtrs() : returns pair< Test, SharedTest > +%create_ptrs() : returns pair< SharedTest, SharedTest > +%print() : returns void +%return_Point2Ptr(bool value) : returns Point2 +%return_Test(Test value) : returns Test +%return_TestPtr(Test value) : returns Test +%return_bool(bool value) : returns bool +%return_double(double value) : returns double +%return_field(Test t) : returns bool +%return_int(int value) : returns int +%return_matrix1(Matrix value) : returns Matrix +%return_matrix2(Matrix value) : returns Matrix +%return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > +%return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest > +%return_size_t(size_t value) : returns size_t +%return_string(string value) : returns string +%return_vector1(Vector value) : returns Vector +%return_vector2(Vector value) : returns Vector % %-------Static Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns1/ClassA.m b/wrap/tests/expected_namespaces/+ns1/ClassA.m index 2ce280041..ee87c12d7 100644 --- a/wrap/tests/expected_namespaces/+ns1/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns1/ClassA.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSA() +%ClassA() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns1/ClassB.m b/wrap/tests/expected_namespaces/+ns1/ClassB.m index 5c4d82882..7f393f03e 100644 --- a/wrap/tests/expected_namespaces/+ns1/ClassB.m +++ b/wrap/tests/expected_namespaces/+ns1/ClassB.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSB() +%ClassB() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m b/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m index ff5ed9e3c..4ad4a14f0 100644 --- a/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m +++ b/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSB() +%ClassB() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index d8f1dbc88..c56377c2a 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -1,13 +1,13 @@ %-------Constructors------- -%CLASSA() +%ClassA() % %-------Methods------- -%MEMBERFUNCTION() : returns double -%NSARG(ClassB arg) : returns int -%NSRETURN(double q) : returns ns2::ns3::ClassB +%memberFunction() : returns double +%nsArg(ClassB arg) : returns int +%nsReturn(double q) : returns ns2::ns3::ClassB % %-------Static Methods------- -%AFUNCTION() : returns double +%afunction() : returns double % %For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html classdef ClassA < handle diff --git a/wrap/tests/expected_namespaces/+ns2/ClassC.m b/wrap/tests/expected_namespaces/+ns2/ClassC.m index 86cf9819f..050ef19f4 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassC.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassC.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSC() +%ClassC() % %-------Methods------- % diff --git a/wrap/tests/expected_namespaces/ClassD.m b/wrap/tests/expected_namespaces/ClassD.m index 13bac35d6..41e9e63ec 100644 --- a/wrap/tests/expected_namespaces/ClassD.m +++ b/wrap/tests/expected_namespaces/ClassD.m @@ -1,5 +1,5 @@ %-------Constructors------- -%CLASSD() +%ClassD() % %-------Methods------- %