diff --git a/.cproject b/.cproject index 6adea9f16..bd149acf7 100644 --- a/.cproject +++ b/.cproject @@ -390,6 +390,7 @@ make + testErrors.run true false @@ -403,6 +404,14 @@ true true + + make + -j2 + tests/testSPQRUtil.valgrind + true + true + true + make -j2 @@ -517,6 +526,7 @@ make + testBayesTree.run true false @@ -524,6 +534,7 @@ make + testBinaryBayesNet.run true false @@ -571,6 +582,7 @@ make + testSymbolicBayesNet.run true false @@ -578,6 +590,7 @@ make + testSymbolicFactor.run true false @@ -585,6 +598,7 @@ make + testSymbolicFactorGraph.run true false @@ -664,7 +678,6 @@ make - testGraph.run true false @@ -760,7 +773,6 @@ make - testInference.run true false @@ -768,7 +780,6 @@ make - testGaussianBayesNet.run true false @@ -776,7 +787,6 @@ make - testGaussianFactor.run true false @@ -784,7 +794,6 @@ make - testJunctionTree.run true false @@ -792,7 +801,6 @@ make - testSymbolicBayesNet.run true false @@ -800,7 +808,6 @@ make - testSymbolicFactorGraph.run true false @@ -1016,7 +1023,6 @@ make - testSimulated2DOriented.run true false @@ -1056,7 +1062,6 @@ make - testSimulated2D.run true false @@ -1064,7 +1069,6 @@ make - testSimulated3D.run true false @@ -1150,46 +1154,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -1286,6 +1250,46 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + make -j2 diff --git a/README b/README index 4fae6798a..ea26b1cbb 100644 --- a/README +++ b/README @@ -37,7 +37,6 @@ Finally, there are some local libraries built needed in the rest of the code: colamd COLAMD and CCOLAMD by Tim Davis needed for re-ordering CppUnitLite unit test library doc documentation - ldl LDL by Tim Davis (will probably go) m4 local M4 macros spqr_mini Core frontal solver from SuiteSparse (will probably go as not LGPL) diff --git a/base/Makefile.am b/base/Makefile.am index e5f2a65a0..6742c15f4 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -59,10 +59,6 @@ if USE_LAPACK AM_CPPFLAGS += -DGT_USE_LAPACK endif -if USE_LDL -AM_CPPFLAGS += -DGT_USE_LDL -endif - # On Mac, we compile using the BLAS/LAPACK headers in the Accelerate framework if USE_ACCELERATE_MACOS AM_CPPFLAGS += -I/System/Library/Frameworks/vecLib.framework/Headers @@ -75,9 +71,6 @@ TESTS = $(check_PROGRAMS) AM_DEFAULT_SOURCE_EXT = .cpp AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization) LDADD = libbase.la ../CppUnitLite/libCppUnitLite.a -if USE_LDL -LDADD += libldl.la -endif if USE_BLAS_LINUX AM_LDFLAGS += -lcblas -latlas @@ -99,4 +92,7 @@ endif %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ #---------------------------------------------------------------------------------------------------- diff --git a/base/Matrix.cpp b/base/Matrix.cpp index dc97a7b2e..4bb0aaa2c 100644 --- a/base/Matrix.cpp +++ b/base/Matrix.cpp @@ -973,90 +973,6 @@ Matrix RtR(const Matrix &A) return trans(LLt(A)); } -/* ************************************************************************* */ -#ifdef GT_USE_LDL -Vector solve_ldl(const Matrix& M, const Vector& rhs) { - - int N = M.size1(); // size of the matrix - - // count the nonzero entries above diagonal - double thresh = 1e-9; - int nrANZ = 0; // # of nonzeros on diagonal and upper triangular part of A - for (int i=0; i thresh) - ++nrANZ; - - // components to pass in - int * Ap = new int[N+1], - * Ai = new int[nrANZ]; - double * Ax = new double[nrANZ], - * b = new double[N]; - - // set ending value for Ap - Ap[N] = nrANZ; - - // copy in the full A matrix to compressed column form - int t = 0; // count the elements added - for (int j=0; j thresh) { - Ai[t] = i; - Ax[t++] = m; - } - } - } - - // copy in RHS - for (int i = 0; i < N; ++i) - b[i] = rhs(i); - - // workspace variables - double * D = new double[N], - * Y = new double[N]; - int * Lp = new int [N+1], - * Parent = new int [N], - * Lnz = new int [N], - * Flag = new int [N], - * Pattern = new int [N]; - - // factorize A into LDL' (P and Pinv not used) - LDL_symbolic (N, Ap, Ai, Lp, Parent, Lnz, Flag, NULL, NULL); - - int nrLNZ = Lp[N]; // # of nonzeros below the diagonal of L - - // after getting size of L, initialize storage arrays - double * Lx = new double[nrLNZ]; - int * Li = new int [nrLNZ]; - - int d = LDL_numeric (N, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D, Y, Pattern, - Flag, NULL, NULL); - - if (d == N) { - /* solve Ax=b, overwriting b with the solution x */ - LDL_lsolve (N, b, Lp, Li, Lx) ; - LDL_dsolve (N, b, D) ; - LDL_ltsolve (N, b, Lp, Li, Lx) ; - } else { - throw runtime_error("ldl_numeric failed"); - } - - // copy solution out - Vector result = Vector_(N, b); - - // cleanup - delete[] Ap; delete[] Ai; - delete[] Ax; delete[] b; - - delete[] Lx; delete[] D; delete[] Y; - delete[] Li; delete[] Lp; delete[] Parent; delete[] Lnz; delete[] Flag; delete[] Pattern; - - return result; -} -#endif - /* * This is not ultra efficient, but not terrible, either. */ diff --git a/base/Matrix.h b/base/Matrix.h index b915b6658..92373dc55 100644 --- a/base/Matrix.h +++ b/base/Matrix.h @@ -388,11 +388,6 @@ Matrix RtR(const Matrix& A); /** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */ Matrix cholesky_inverse(const Matrix &A); -/** Solve Ax=b with S.P.D. matrix using Davis' LDL code */ -#ifdef GT_USE_LDL -Vector solve_ldl(const Matrix& A, const Vector& b); -#endif - /** * Numerical exponential map, naive approach, not industrial strength !!! * @param A matrix to exponentiate diff --git a/configure.ac b/configure.ac index f8162d76e..2956a4986 100644 --- a/configure.ac +++ b/configure.ac @@ -14,7 +14,7 @@ AC_CONFIG_SRCDIR([base/DSFVector.cpp]) AC_CONFIG_SRCDIR([geometry/Cal3_S2.cpp]) AC_CONFIG_SRCDIR([inference/SymbolicFactor.cpp]) AC_CONFIG_SRCDIR([linear/GaussianFactor.cpp]) -AC_CONFIG_SRCDIR([nonlinear/ConstraintOptimizer.cpp]) +AC_CONFIG_SRCDIR([nonlinear/NonlinearOptimizer.cpp]) AC_CONFIG_SRCDIR([slam/pose2SLAM.cpp]) AC_CONFIG_SRCDIR([tests/testSQP.cpp]) AC_CONFIG_SRCDIR([wrap/wrap.cpp]) @@ -49,9 +49,6 @@ case $host_os in ;; esac -# search for a blas implementation. Kai: this can not be easily turned off for Mac. -# AX_BLAS(true) - # enable BLAS with general purpose script AC_ARG_ENABLE([blas], [ --enable-blas Enable external BLAS library], @@ -92,17 +89,6 @@ AC_ARG_ENABLE([spqr], AM_CONDITIONAL([USE_SPQR], [test x$spqr = xtrue]) -# enable using LDL library from SuiteSparse -AC_ARG_ENABLE([ldl], - [ --enable-ldl Enable LDL library support], - [case "${enableval}" in - yes) ldl=true ;; - no) ldl=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-ldl]) ;; - esac],[ldl=false]) - -AM_CONDITIONAL([USE_LDL], [test x$ldl = xtrue]) - # enable profiling AC_ARG_ENABLE([profiling], [ --enable-profiling Enable profiling], @@ -128,10 +114,6 @@ AM_CONDITIONAL([ENABLE_SERIALIZATION], [test x$serialization = xtrue]) # Checks for programs. AC_PROG_CXX AC_PROG_CC -# FIXME: Need to use boost macros to get serialization library linked -#AX_BOOST_BASE([1.37.0]) -#AX_BOOST_SERIALIZATION -#AX_BOOST_BASE([1.33]) # does not work on windows, even after compiling & installing boost manually # Checks for libraries. LT_INIT @@ -169,15 +151,5 @@ AC_ARG_WITH([boost], [--with-boost has to be specified]) ]) AC_SUBST([boost]) - -# ask for boost serialization -#AC_ARG_WITH([boost_serialization], -# [AS_HELP_STRING([--with-boost-serialization], -# [(optional) use the Serialization library from boost - specify the library linking command with the full name of the library -# e.g. --with-boost-serialization=-lboost_serialization-gcc-mt-d-1_33_1])], -# [AC_DEFINE([HAVE_BOOST_SERIALIZATION], [""], [boost serialization flag]) -# -#AC_SUBST([boost_serialization]) - AC_OUTPUT diff --git a/examples/Makefile.am b/examples/Makefile.am index 9a3770838..52f6c8dc2 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -22,12 +22,12 @@ AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CPPFLAGS = -I$(boost) -I$(top_srcdir)/.. LDADD = ../libgtsam.la AM_DEFAULT_SOURCE_EXT = .cpp -if USE_LDL -LDADD += libldl.la -endif # rule to run an executable %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ #---------------------------------------------------------------------------------------------------- diff --git a/geometry/Makefile.am b/geometry/Makefile.am index fc020965e..5638aa4d0 100644 --- a/geometry/Makefile.am +++ b/geometry/Makefile.am @@ -50,12 +50,12 @@ TESTS = $(check_PROGRAMS) AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization) LDADD = libgeometry.la ../base/libbase.la ../CppUnitLite/libCppUnitLite.a AM_DEFAULT_SOURCE_EXT = .cpp -if USE_LDL -LDADD += libldl.la -endif # rule to run an executable %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ #---------------------------------------------------------------------------------------------------- diff --git a/inference/Makefile.am b/inference/Makefile.am index e6551b7ca..843a5b144 100644 --- a/inference/Makefile.am +++ b/inference/Makefile.am @@ -69,14 +69,15 @@ AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization) LDADD = libinference.la ../base/libbase.la LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la AM_DEFAULT_SOURCE_EXT = .cpp -if USE_LDL -LDADD += libldl.la -endif # rule to run an executable %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ + if USE_LAPACK AM_CXXFLAGS += -DGT_USE_LAPACK LDADD += ../spqr_mini/libspqr_mini.la diff --git a/linear/Makefile.am b/linear/Makefile.am index 607d110ab..3b5f8ace4 100644 --- a/linear/Makefile.am +++ b/linear/Makefile.am @@ -57,14 +57,15 @@ AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization) LDADD = liblinear.la ../inference/libinference.la ../base/libbase.la LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la AM_DEFAULT_SOURCE_EXT = .cpp -if USE_LDL -LDADD += libldl.la -endif # rule to run an executable %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ + if USE_LAPACK AM_CXXFLAGS += -DGT_USE_LAPACK LDADD += ../spqr_mini/libspqr_mini.la diff --git a/nonlinear/ConstraintOptimizer.cpp b/nonlinear/ConstraintOptimizer.cpp deleted file mode 100644 index 0dea97c25..000000000 --- a/nonlinear/ConstraintOptimizer.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/** - * @file ConstraintOptimizer.cpp - * @author Alex Cunningham - */ - -/** IMPORTANT NOTE: this file is only compiled when LDL is available */ - -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -void gtsam::BFGSEstimator::update(const Vector& dfx, const boost::optional step) { - if (step) { - Vector Bis = B_ * *step, - y = dfx - prev_dfx_; - B_ = B_ + outer_prod(y, y) / inner_prod(y, *step) - - outer_prod(Bis, Bis) / inner_prod(*step, Bis); - } - prev_dfx_ = dfx; -} - -/* ************************************************************************* */ -pair gtsam::solveCQP(const Matrix& B, const Matrix& A, - const Vector& g, const Vector& h) { - // find the dimensions - size_t n = B.size1(), p = A.size2(); - - // verify matrices - if (n != B.size2()) - throw invalid_argument("solveCQP: B matrix is not square!"); - if (A.size1() != n) - throw invalid_argument("solveCQP: A matrix needs m = B.size1()"); - - // construct G matrix - Matrix G = zeros(n+p, n+p); - insertSub(G, B, 0, 0); - insertSub(G, A, 0, n); - insertSub(G, trans(A), n, 0); - - Vector rhs = zero(n+p); - subInsert(rhs, -1.0*g, 0); - subInsert(rhs, -1.0*h, n); - - // solve the system with the LDL solver - Vector fullResult = solve_ldl(G, rhs); - - return make_pair(sub(fullResult, 0, n), sub(fullResult, n, n+p)); -} - -/* ************************************************************************* */ -Vector gtsam::linesearch(const Vector& x0, const Vector& delta, - double (*penalty)(const Vector&), size_t maxIt) { - - // calculate the initial error - double init_error = penalty(x0); - Vector step = delta; - for (size_t i=0; i -#include - -namespace gtsam { - - /** - * BFGS Hessian estimator - * Maintains an estimate for a hessian matrix using - * only derivatives and the step - */ - class BFGSEstimator { - protected: - size_t n_; // dimension of square matrix - Matrix B_; // current estimate - Vector prev_dfx_; // previous gradient value - public: - - /** - * Creates an estimator of a particular dimension - */ - BFGSEstimator(size_t n) : n_(n), B_(eye(n,n)) {} - - ~BFGSEstimator() {} - - /** - * Direct vector interface - useful for small problems - * - * Update will set the previous gradient and update the - * estimate for the Hessian. When specified without - * the step, this is the initialization phase, and will not - * change the Hessian estimate - * @param df is the gradient of the function - * @param step is the step vector applied at the last iteration - */ - void update(const Vector& df, const boost::optional step = boost::none); - - // access functions - const Matrix& getB() const { return B_; } - size_t dim() const { return n_; } - - }; - - - /** - * Basic function that uses LDL factorization to solve a - * KKT system (state and lagrange multipliers) of the form: - * Gd=b, where - * G = [B A] d = [ x ] b = - [g] - * [A' 0] [lam] [h] - * B = Hessian of Lagragian function - * A = Gradient of constraints - * x = state - * lam = vector of lagrange mulipliers - * g = gradient of f(x) evaluated a point - * h = current value of c(x) - * - * @return pair of state and lambas - */ - std::pair solveCQP(const Matrix& B, const Matrix& A, - const Vector& g, const Vector& h); - - /** - * Simple line search function using an externally specified - * penalty function - */ - Vector linesearch(const Vector& x, const Vector& delta, - double (*penalty)(const Vector&), size_t maxIt = 10); - -} // \namespace gtsam - diff --git a/nonlinear/FusionTupleConfig.h b/nonlinear/FusionTupleConfig.h deleted file mode 100644 index d07b61f59..000000000 --- a/nonlinear/FusionTupleConfig.h +++ /dev/null @@ -1,393 +0,0 @@ -/** - * @file FusionTupleConfig.h - * @brief Experimental tuple config using boost.Fusion - * @author Alex Cunningham - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace gtsam { - -/** - * This config uses a real tuple to store types - * The template parameter should be a boost fusion structure, like - * set - */ -template -class FusionTupleConfig : public Testable >{ - -public: - /** useful types */ - typedef Configs BaseTuple; - -protected: - /** the underlying tuple storing everything */ - Configs base_tuple_; - -private: - /** helper structs to make use of fusion algorithms */ - struct size_helper { - typedef size_t result_type; - template - size_t operator()(const T& t, const size_t& s) const { - return s + t.size(); - } - }; - - struct dim_helper { - typedef size_t result_type; - template - size_t operator()(const T& t, const size_t& s) const { - return s + t.dim(); - } - }; - - struct zero_helper { - typedef VectorConfig result_type; - template - result_type operator()(const T& t, const result_type& s) const { - result_type new_s(s); - new_s.insert(t.zero()); - return new_s; - } - }; - - struct update_helper { - typedef Configs result_type; - template - result_type operator()(const T& t, const result_type& s) const { - result_type new_s(s); - boost::fusion::at_key(new_s).update(t); - return new_s; - } - }; - - struct expmap_helper { - typedef FusionTupleConfig result_type; - VectorConfig delta; - expmap_helper(const VectorConfig& d) : delta(d) {} - template - result_type operator()(const T& t, const result_type& s) const { - result_type new_s(s); - boost::fusion::at_key(new_s.base_tuple_) = T(gtsam::expmap(t, delta)); - return new_s; - } - }; - - struct logmap_helper { - typedef VectorConfig result_type; - template - result_type operator()(const T& t, const result_type& s) const { - result_type new_s(s); - new_s.insert(gtsam::logmap(boost::fusion::at_c<0>(t), boost::fusion::at_c<1>(t))); - return new_s; - } - }; - - struct empty_helper { - template - bool operator()(T t) const { - return t.empty(); - } - }; - - struct print_helper { - template - void operator()(T t) const { - t.print(); - } - }; - - struct equals_helper { - double tol; - equals_helper(double t) : tol(t) {} - template - bool operator()(T t) const { - return boost::fusion::at_c<0>(t).equals(boost::fusion::at_c<1>(t), tol); - } - }; - - /** two separate function objects for arbitrary copy construction */ - template - struct assign_inner { - typedef Ret result_type; - - Config config; - assign_inner(const Config& cfg) : config(cfg) {} - - template - result_type operator()(const T& t, const result_type& s) const { - result_type new_s(s); - T new_cfg(config); - if (!new_cfg.empty()) boost::fusion::at_key(new_s) = new_cfg; - return new_s; - } - }; - - template - struct assign_outer { - typedef Ret result_type; - - template // T is the config from the "other" config - Ret operator()(const T& t, const Ret& s) const { - assign_inner helper(t); - return boost::fusion::fold(s, s, helper); // loop over the "self" config - } - }; - -public: - /** create an empty config */ - FusionTupleConfig() {} - - /** copy constructor */ - FusionTupleConfig(const FusionTupleConfig& other) : base_tuple_(other.base_tuple_) {} - - /** direct initialization of the underlying structure */ - FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {} - - /** initialization from arbitrary other configs */ - template - FusionTupleConfig(const FusionTupleConfig& other) - : base_tuple_(boost::fusion::fold(other.base_tuple(), Configs(), assign_outer())) - { - } - - virtual ~FusionTupleConfig() {} - - /** insertion */ - template - void insert(const Key& j, const Value& x) { - config_ >().insert(j,x); - } - - /** insert a full config at a time */ - template - void insertSub(const Config& config) { - config_().insert(config); - } - - /** - * Update function for whole configs - this will change existing values - * @param config is a config to add - */ - void update(const FusionTupleConfig& config) { - base_tuple_ = boost::fusion::accumulate( - config.base_tuple_, base_tuple_, - update_helper()); - } - - /** - * Update function for whole subconfigs - this will change existing values - * @param config is a config to add - */ - template - void update(const Config& config) { - config_().update(config); - } - - /** - * Update function for single key/value pairs - will change existing values - * @param key is the variable identifier - * @param value is the variable value to update - */ - template - void update(const Key& key, const Value& value) { - config_ >().update(key,value); - } - - /** check if a given element exists */ - template - bool exists(const Key& j) const { - return config >().exists(j); - } - - /** a variant of exists */ - template - boost::optional exists_(const Key& j) const { - return config >().exists_(j); - } - - /** retrieve a point */ - template - const typename Key::Value_t & at(const Key& j) const { - return config >().at(j); - } - - /** access operator */ - template - const typename Key::Value_t & operator[](const Key& j) const { return at(j); } - - /** retrieve a reference to a full subconfig */ - template - const Config & config() const { - return boost::fusion::at_key(base_tuple_); - } - - /** size of the config - sum all sizes from subconfigs */ - size_t size() const { - return boost::fusion::accumulate(base_tuple_, 0, - size_helper()); - } - - /** combined dimension of the subconfigs */ - size_t dim() const { - return boost::fusion::accumulate(base_tuple_, 0, - dim_helper()); - } - - /** number of configs in the config */ - size_t nrConfigs() const { - return boost::fusion::size(base_tuple_); - } - - /** erases a specific key */ - template - void erase(const Key& j) { - config_ >().erase(j); - } - - /** clears the config */ - void clear() { - base_tuple_ = Configs(); - } - - /** returns true if the config is empty */ - bool empty() const { - return boost::fusion::all(base_tuple_, - empty_helper()); - } - - /** print */ - void print(const std::string& s="") const { - std::cout << "FusionTupleConfig " << s << ":" << std::endl; - boost::fusion::for_each(base_tuple_, print_helper()); - } - - /** equals */ - bool equals(const FusionTupleConfig& other, double tol=1e-9) const { - equals_helper helper(tol); - return boost::fusion::all( - boost::fusion::zip(base_tuple_,other.base_tuple_), helper); - } - - /** zero: create VectorConfig of appropriate structure */ - VectorConfig zero() const { - return boost::fusion::accumulate(base_tuple_, VectorConfig(), - zero_helper()); - } - - FusionTupleConfig expmap(const VectorConfig& delta) const { - return boost::fusion::accumulate(base_tuple_, base_tuple_, - expmap_helper(delta)); - } - - VectorConfig logmap(const FusionTupleConfig& cp) const { - return boost::fusion::accumulate(boost::fusion::zip(base_tuple_, cp.base_tuple_), - VectorConfig(), logmap_helper()); - } - - /** - * direct access to the underlying fusion set - don't use this normally - * TODO: make this a friend function or similar - */ - const BaseTuple & base_tuple() const { return base_tuple_; } - -private: - - /** retrieve a non-const reference to a full subconfig */ - template - Config & config_() { - return boost::fusion::at_key(base_tuple_); - } - - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(base_tuple_); - } - -}; - -/** Exmap static functions */ -template -inline FusionTupleConfig expmap(const FusionTupleConfig& c, const VectorConfig& delta) { - return c.expmap(delta); -} - -/** logmap static functions */ -template -inline VectorConfig logmap(const FusionTupleConfig& c0, const FusionTupleConfig& cp) { - return c0.logmap(cp); -} - -/** - * Easy-to-use versions of FusionTupleConfig - * These versions provide a simpler interface more like - * the existing TupleConfig. Each version has a number, and - * takes explicit template arguments for config types. - */ -template -struct FusionTupleConfig1 : public FusionTupleConfig > { - typedef FusionTupleConfig > Base; - typedef FusionTupleConfig1 This; - - typedef C1 Config1; - - FusionTupleConfig1() {} - FusionTupleConfig1(const Base& c) : Base(c) {} - FusionTupleConfig1(const C1& c1) : Base(boost::fusion::make_set(c1)) {} - - const Config1& first() const { return boost::fusion::at_key(this->base_tuple_); } -}; - -template -struct FusionTupleConfig2 : public FusionTupleConfig > { - typedef FusionTupleConfig > Base; - typedef FusionTupleConfig2 This; - - typedef C1 Config1; - typedef C2 Config2; - - FusionTupleConfig2() {} - FusionTupleConfig2(const Base& c) : Base(c) {} - FusionTupleConfig2(const C1& c1, const C2& c2) : Base(boost::fusion::make_set(c1, c2)) {} - - const Config1& first() const { return boost::fusion::at_key(this->base_tuple_); } - const Config2& second() const { return boost::fusion::at_key(this->base_tuple_); } -}; - -template -struct FusionTupleConfig3 : public FusionTupleConfig > { - typedef FusionTupleConfig > Base; - - typedef C1 Config1; - typedef C2 Config2; - typedef C3 Config3; - - FusionTupleConfig3() {} - FusionTupleConfig3(const Base& c) : Base(c) {} - FusionTupleConfig3(const C1& c1, const C2& c2, const C3& c3) - : Base(boost::fusion::make_set(c1, c2, c3)) {} - - const Config1& first() const { return boost::fusion::at_key(this->base_tuple_); } - const Config2& second() const { return boost::fusion::at_key(this->base_tuple_); } - const Config3& third() const { return boost::fusion::at_key(this->base_tuple_); } -}; - - -} // \namespace gtsam - diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index 329021ffd..1ecf03433 100644 --- a/nonlinear/Makefile.am +++ b/nonlinear/Makefile.am @@ -17,7 +17,6 @@ check_PROGRAMS = # Lie Groups headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h -headers += FusionTupleConfig.h check_PROGRAMS += tests/testLieConfig # Nonlinear nonlinear @@ -30,12 +29,6 @@ sources += NonlinearOptimizer.cpp headers += NonlinearConstraint.h headers += NonlinearEquality.h -# SQP -if USE_LDL -sources += ConstraintOptimizer.cpp -check_PROGRAMS += tests/testConstraintOptimizer -endif - #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -57,14 +50,15 @@ AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization) LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la AM_DEFAULT_SOURCE_EXT = .cpp -if USE_LDL -LDADD += libldl.la -endif # rule to run an executable %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ + if USE_LAPACK AM_CXXFLAGS += -DGT_USE_LAPACK LDADD += ../spqr_mini/libspqr_mini.la diff --git a/slam/Makefile.am b/slam/Makefile.am index 73a20710e..6f6964daa 100644 --- a/slam/Makefile.am +++ b/slam/Makefile.am @@ -78,9 +78,6 @@ AM_DEFAULT_SOURCE_EXT = .cpp AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization) LDADD = libslam.la ../geometry/libgeometry.la ../nonlinear/libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la -if USE_LDL -LDADD += libldl.la -endif if USE_LAPACK LDADD += ../spqr_mini/libspqr_mini.la @@ -90,3 +87,6 @@ endif %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ diff --git a/tests/Makefile.am b/tests/Makefile.am index bbf582e8f..3e34b2fec 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -22,13 +22,6 @@ if ENABLE_SERIALIZATION check_PROGRAMS += testSerialization endif -# experimental -#check_PROGRAMS += testFusionTupleConfig # Doesn't work on Macs - -if USE_LDL -check_PROGRAMS += testConstraintOptimizer -endif - # Timing tests noinst_PROGRAMS = timeGaussianFactorGraph timeFactorOverhead @@ -51,3 +44,6 @@ AM_DEFAULT_SOURCE_EXT = .cpp %.run: % $(LDADD) ./$^ +# rule to run executable with valgrind +%.valgrind: % $(LDADD) + valgrind ./$^ diff --git a/tests/testConstraintOptimizer.cpp b/tests/testConstraintOptimizer.cpp deleted file mode 100644 index fa85ef8b9..000000000 --- a/tests/testConstraintOptimizer.cpp +++ /dev/null @@ -1,318 +0,0 @@ -/** - * @file testConstraintOptimizer.cpp - * @brief Tests the optimization engine for SQP and BFGS Quadratic programming techniques - * @author Alex Cunningham - */ - -#include -#include - -#include -#include - -#include - -#include -#include - -#define GTSAM_MAGIC_KEY - -#include // for operator += -using namespace boost::assign; - -using namespace std; -using namespace gtsam; - -#include -using namespace example; - -/* ************************************************************************* */ -TEST( matrix, unconstrained_fg_ata ) { - // create a graph - GaussianFactorGraph fg = createGaussianFactorGraph(); - - Matrix A; Vector b; - Ordering ordering; - ordering += Symbol('l', 1), Symbol('x', 1), Symbol('x', 2); - boost::tie(A, b) = fg.matrix(ordering); - Matrix B_ata = prod(trans(A), A); - - // solve subproblem - Vector actual = solve_ldl(B_ata, prod(trans(A), b)); - - // verify - Vector expected = createCorrectDelta().vector(); - CHECK(assert_equal(expected,actual)); -} - -///* ************************************************************************* */ -//TEST( matrix, unconstrained_fg ) { -// // create a graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// Matrix A; Vector b; -// Ordering ordering; -// ordering += Symbol('l', 1), Symbol('x', 1), Symbol('x', 2); -// boost::tie(A, b) = fg.matrix(ordering); -// Matrix B_ata = prod(trans(A), A); -//// print(B_ata, "B_ata"); -//// print(b, " b"); -// -// // parameters -// size_t maxIt = 50; -// double stepsize = 0.1; -// -// // iterate to solve -// VectorConfig x = createZeroDelta(); -// BFGSEstimator B(x.dim()); -// -// Vector step; -// -// for (size_t i=0; i0) { -// B.update(dfx, step); -// } else { -// B.update(dfx); -// } -// -// // solve subproblem -//// print(B.getB(), " B_bfgs"); -// Vector delta = solve_ldl(B.getB(), -dfx); -//// Vector delta = solve_ldl(B_ata, -dfx); -// -//// print(delta, " delta"); -// -// // update -// step = stepsize * delta; -//// step = linesearch(x, delta, penalty); // TODO: switch here -// x = expmap(x, step); -//// print(step, " step"); -// } -// -// // verify -// VectorConfig expected = createCorrectDelta(); -// CHECK(assert_equal(expected,x, 1e-4)); -//} - -SharedDiagonal probModel1 = sharedSigma(1,1.0); -SharedDiagonal probModel2 = sharedSigma(2,1.0); -SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); - -/* ********************************************************************* - * This example uses a nonlinear objective function and - * nonlinear equality constraint. The formulation is actually - * the Cholesky form that creates the full Hessian explicitly, - * which should really be avoided with our QR-based machinery. - * - * Note: the update equation used here has a fixed step size - * and gain that is rather arbitrarily chosen, and as such, - * will take a silly number of iterations. - */ -TEST (SQP, problem1_cholesky ) { - bool verbose = false; - // use a nonlinear function of f(x) = x^2+y^2 - // nonlinear equality constraint: g(x) = x^2-5-y=0 - // Lagrangian: f(x) + \lambda*g(x) - - // Symbols - Symbol x1("x1"), y1("y1"), L1("L1"); - - // state structure: [x y \lambda] - VectorConfig init, state; - init.insert(x1, Vector_(1, 1.0)); - init.insert(y1, Vector_(1, 1.0)); - init.insert(L1, Vector_(1, 1.0)); - state = init; - - if (verbose) init.print("Initial State"); - - // loop until convergence - int maxIt = 10; - for (int i = 0; i ||Ax-b||^2 - * where: - * h(x) simply returns the inputs - * z zeros(2) - * A identity - * b linearization point - */ - Matrix A = eye(2); - Vector b = Vector_(2, x, y); - GaussianFactor::shared_ptr f1( - new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1) - y1, sub(A, 0,2, 1,2), // A(:,2) - b, // rhs of f(x) - probModel2)); // arbitrary sigma - - /** create the constraint-linear factor - * Provides a mechanism to use variable gain to force the constraint - * \lambda*gradG*dx + d\lambda = zero - * formulated in matrix form as: - * [\lambda*gradG eye(1)] [dx; d\lambda] = zero - */ - Matrix gradG = Matrix_(1, 2,2*x, -1.0); - GaussianFactor::shared_ptr f2( - new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) - y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) - L1, eye(1), // dlambda term - Vector_(1, 0.0), // rhs is zero - probModel1)); // arbitrary sigma - - // create the actual constraint - // [gradG] [x; y] - g = 0 - Vector g = Vector_(1,x*x-y-5); - GaussianFactor::shared_ptr c1( - new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG - y1, sub(gradG, 0,1, 1,2), // slice second part of gradG - g, // value of constraint function - constraintModel1)); // force to constraint - - // construct graph - GaussianFactorGraph fg; - fg.push_back(f1); - fg.push_back(f2); - fg.push_back(c1); - if (verbose) fg.print("Graph"); - - // solve - Ordering ord; - ord += x1, y1, L1; - VectorConfig delta = fg.optimize(ord); - if (verbose) delta.print("Delta"); - - // update initial estimate - VectorConfig newState = expmap(state, delta.scale(-1.0)); - - // set the state to the updated state - state = newState; - - if (verbose) state.print("Updated State"); - } - - // verify that it converges to the nearest optimal point - VectorConfig expected; - expected.insert(x1, Vector_(1, 2.12)); - expected.insert(y1, Vector_(1, -0.5)); - CHECK(assert_equal(state[x1], expected[x1], 1e-2)); - CHECK(assert_equal(state[y1], expected[y1], 1e-2)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/tests/testFusionTupleConfig.cpp b/tests/testFusionTupleConfig.cpp deleted file mode 100644 index 3bb73ccad..000000000 --- a/tests/testFusionTupleConfig.cpp +++ /dev/null @@ -1,518 +0,0 @@ -/** - * @file testFusionTupleConfig.cpp - * @author Alex Cunningham - */ - -#include - -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -using namespace boost; -using namespace gtsam; -using namespace std; - -static const double tol = 1e-5; - -typedef TypedSymbol PoseKey; -typedef TypedSymbol PointKey; -typedef LieConfig PoseConfig; -typedef LieConfig PointConfig; - -// some generic poses, points and keys -PoseKey x1(1), x2(2); -Pose2 x1_val(1.0, 2.0, 0.3), x2_val(2.0, 3.0, 0.4); -PointKey l1(1), l2(2); -Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0); - -typedef FusionTupleConfig > TupPointConfig; -typedef FusionTupleConfig > TupPoseConfig; -typedef FusionTupleConfig > TupPairConfig; - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, basic_config_operations ) { - - // some initialized configs to start with - PoseConfig poseConfig1; - poseConfig1.insert(x1, x1_val); - PointConfig pointConfig1; - pointConfig1.insert(l1, l1_val); - - // basic initialization - TupPointConfig cfg1A, cfg2A(pointConfig1); - EXPECT(cfg1A.size() == 0); - EXPECT(cfg2A.size() == 1); - EXPECT(cfg1A.empty()); - EXPECT(!cfg2A.empty()); - - TupPoseConfig cfg1B, cfg2B(poseConfig1); - EXPECT(cfg1B.size() == 0); - EXPECT(cfg2B.size() == 1); - EXPECT(cfg1B.empty()); - EXPECT(!cfg2B.empty()); - - TupPairConfig cfg1, cfg2(fusion::make_set(poseConfig1, pointConfig1)); - EXPECT(cfg1.size() == 0); - EXPECT(cfg1.empty()); - EXPECT(cfg2.size() == 2); - EXPECT(!cfg2.empty()); - - // insertion - cfg1A.insert(l1, l1_val); - EXPECT(cfg1A.size() == 1); - cfg1A.insert(l2, l2_val); - EXPECT(cfg1A.size() == 2); - cfg2A.insert(l2, l2_val); - EXPECT(cfg2A.size() == 2); - - cfg1B.insert(x1, x1_val); - EXPECT(cfg1B.size() == 1); - cfg1B.insert(x2, x2_val); - EXPECT(cfg1B.size() == 2); - cfg2B.insert(x2, x2_val); - EXPECT(cfg2B.size() == 2); - - cfg1.insert(x1, x1_val); - cfg1.insert(x2, x2_val); - cfg1.insert(l1, l1_val); - cfg1.insert(l2, l2_val); - EXPECT(cfg1.size() == 4); - - // exists - EXPECT(cfg1.exists(x1)); - EXPECT(cfg1.exists(x2)); - EXPECT(cfg1.exists(l1)); - EXPECT(cfg1.exists(l2)); - - // retrieval of elements - EXPECT(assert_equal(l1_val, cfg1A.at(l1))); - EXPECT(assert_equal(l2_val, cfg1A.at(l2))); - - EXPECT(assert_equal(x1_val, cfg1B.at(x1))); - EXPECT(assert_equal(x2_val, cfg1B.at(x2))); - - EXPECT(assert_equal(l1_val, cfg1.at(l1))); - EXPECT(assert_equal(l2_val, cfg1.at(l2))); - EXPECT(assert_equal(x1_val, cfg1.at(x1))); - EXPECT(assert_equal(x2_val, cfg1.at(x2))); - - // config extraction - PointConfig expPointConfig; - expPointConfig.insert(l1, l1_val); - expPointConfig.insert(l2, l2_val); - - PoseConfig expPoseConfig; - expPoseConfig.insert(x1, x1_val); - expPoseConfig.insert(x2, x2_val); - - EXPECT(assert_equal(expPointConfig, cfg1A.config())); - EXPECT(assert_equal(expPoseConfig, cfg1B.config())); - - EXPECT(assert_equal(expPointConfig, cfg1.config())); - EXPECT(assert_equal(expPoseConfig, cfg1.config())); - - // getting sizes of configs - EXPECT(cfg1A.nrConfigs() == 1); - EXPECT(cfg1B.nrConfigs() == 1); - EXPECT(cfg1.nrConfigs() == 2); - - // erase - cfg1.erase(x1); - EXPECT(cfg1.size() == 3); - EXPECT(!cfg1.exists(x1)); - cfg1.erase(l1); - EXPECT(cfg1.size() == 2); - EXPECT(!cfg1.exists(l1)); - - // clear - cfg1.clear(); - cfg1A.clear(); cfg1B.clear(); - EXPECT(cfg1.size() == 0); - EXPECT(cfg1.empty()); - EXPECT(cfg1A.size() == 0); - EXPECT(cfg1A.empty()); - EXPECT(cfg1B.size() == 0); - EXPECT(cfg1B.empty()); - - cfg2.clear(); - cfg2A.clear(); cfg2B.clear(); - EXPECT(cfg2.size() == 0); - EXPECT(cfg2.empty()); - EXPECT(cfg2A.size() == 0); - EXPECT(cfg2A.empty()); - EXPECT(cfg2B.size() == 0); - EXPECT(cfg2B.empty()); - -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, slicing ) { - TupPairConfig cfg_pair; - cfg_pair.insert(x1, x1_val); - cfg_pair.insert(x2, x2_val); - cfg_pair.insert(l1, l1_val); - cfg_pair.insert(l2, l2_val); - - // initialize configs by slicing a larger config - TupPoseConfig cfg_pose(cfg_pair); - TupPointConfig cfg_point(cfg_pair); - - PointConfig expPointConfig; - expPointConfig.insert(l1, l1_val); - expPointConfig.insert(l2, l2_val); - - PoseConfig expPoseConfig; - expPoseConfig.insert(x1, x1_val); - expPoseConfig.insert(x2, x2_val); - - // verify - EXPECT(assert_equal(expPointConfig, cfg_point.config())); - EXPECT(assert_equal(expPoseConfig, cfg_pose.config())); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, upgrading ) { - TupPoseConfig small; - small.insert(x1, x1_val); - - TupPairConfig actual(small); - EXPECT(actual.size() == 1); - EXPECT(assert_equal(x1_val, actual.at(x1), tol)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, equals ) { - TupPairConfig c1, c2, c3, c4, c5, c6; - c1.insert(l1, l1_val); - c1.insert(l2, l2_val); - c1.insert(x1, x1_val); - c1.insert(x2, x2_val); - c4 = c1; - c2.insert(x1, x1_val); - c2.insert(x2, x2_val); - - EXPECT(assert_equal(c1, c1, tol)); - EXPECT(assert_equal(c4, c1, tol)); - EXPECT(assert_equal(c4, c4, tol)); - EXPECT(!c1.equals(c2, tol)); - EXPECT(!c2.equals(c1, tol)); - EXPECT(!c1.equals(c3, tol)); - EXPECT(!c2.equals(c3, tol)); - EXPECT(assert_equal(c3, c3)); - - c5.insert(l1, l1_val); - c6.insert(l1, l1_val + Point2(1e-6, 1e-6)); - EXPECT(assert_equal(c5, c6, 1e-5)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, insert_equals1 ) -{ - TupPairConfig expected; - expected.insert(PoseKey(1), x1_val); - expected.insert(PoseKey(2), x2_val); - expected.insert(PointKey(1), l1_val); - expected.insert(PointKey(2), l2_val); - - TupPairConfig actual; - actual.insert(PoseKey(1), x1_val); - actual.insert(PoseKey(2), x2_val); - actual.insert(PointKey(1), l1_val); - actual.insert(PointKey(2), l2_val); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, insert_equals2 ) -{ - TupPairConfig config1; - config1.insert(PoseKey(1), x1_val); - config1.insert(PoseKey(2), x2_val); - config1.insert(PointKey(1), l1_val); - config1.insert(PointKey(2), l2_val); - - TupPairConfig config2; - config2.insert(PoseKey(1), x1_val); - config2.insert(PoseKey(2), x2_val); - config2.insert(PointKey(1), l1_val); - - EXPECT(!config1.equals(config2)); - - config2.insert(PointKey(2), Point2(9,11)); - - EXPECT(!config1.equals(config2)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, insert_duplicate ) -{ - TupPairConfig config1; - config1.insert(x1, x1_val); // 3 - config1.insert(x2, x2_val); // 6 - config1.insert(l1, l1_val); // 8 - config1.insert(l2, l2_val); // 10 - config1.insert(l2, l1_val); // still 10 !!!! - - EXPECT(assert_equal(l2_val, config1[PointKey(2)])); - LONGS_EQUAL(4,config1.size()); - LONGS_EQUAL(10,config1.dim()); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, size_dim ) -{ - TupPairConfig config1; - config1.insert(PoseKey(1), x1_val); - config1.insert(PoseKey(2), x2_val); - config1.insert(PointKey(1), l1_val); - config1.insert(PointKey(2), l2_val); - - EXPECT(config1.size() == 4); - EXPECT(config1.dim() == 10); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, at) -{ - TupPairConfig config1; - config1.insert(PoseKey(1), x1_val); - config1.insert(PoseKey(2), x2_val); - config1.insert(PointKey(1), l1_val); - config1.insert(PointKey(2), l2_val); - - EXPECT(assert_equal(x1_val, config1[PoseKey(1)])); - EXPECT(assert_equal(x2_val, config1[PoseKey(2)])); - EXPECT(assert_equal(l1_val, config1[PointKey(1)])); - EXPECT(assert_equal(l2_val, config1[PointKey(2)])); - - CHECK_EXCEPTION(config1[PoseKey(3)], std::invalid_argument); - CHECK_EXCEPTION(config1[PointKey(3)], std::invalid_argument); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, zero_expmap_logmap) -{ - Pose2 xA1(1,2,3), xA2(6,7,8); - Point2 lA1(4,5), lA2(9,10); - - TupPairConfig config1; - config1.insert(PoseKey(1), xA1); - config1.insert(PoseKey(2), xA2); - config1.insert(PointKey(1), lA1); - config1.insert(PointKey(2), lA2); - - VectorConfig expected_zero; - expected_zero.insert("x1", zero(3)); - expected_zero.insert("x2", zero(3)); - expected_zero.insert("l1", zero(2)); - expected_zero.insert("l2", zero(2)); - - EXPECT(assert_equal(expected_zero, config1.zero())); - - VectorConfig delta; - delta.insert("x1", Vector_(3, 1.0, 1.1, 1.2)); - delta.insert("x2", Vector_(3, 1.3, 1.4, 1.5)); - delta.insert("l1", Vector_(2, 1.0, 1.1)); - delta.insert("l2", Vector_(2, 1.3, 1.4)); - - TupPairConfig expected; - expected.insert(PoseKey(1), expmap(xA1, Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(PoseKey(2), expmap(xA2, Vector_(3, 1.3, 1.4, 1.5))); - expected.insert(PointKey(1), Point2(5.0, 6.1)); - expected.insert(PointKey(2), Point2(10.3, 11.4)); - - TupPairConfig actual = expmap(config1, delta); - EXPECT(assert_equal(expected, actual)); - - // Check log - VectorConfig expected_log = delta; - VectorConfig actual_log = logmap(config1,actual); - EXPECT(assert_equal(expected_log, actual_log)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, partial_insert) { - TupPairConfig init, expected; - - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - Point2 point1(2.0, 3.0), point2(5.0, 6.0); - - init.insert(x1, pose1); - init.insert(l1, point1); - - PoseConfig cfg1; - cfg1.insert(x2, pose2); - - init.insertSub(cfg1); - - expected.insert(x1, pose1); - expected.insert(l1, point1); - expected.insert(x2, pose2); - - CHECK(assert_equal(expected, init)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, update) { - TupPairConfig init, superset, expected; - - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - Point2 point1(2.0, 3.0), point2(5.0, 6.0); - - init.insert(x1, pose1); - init.insert(l1, point1); - - Pose2 pose1_(1.0, 2.0, 0.4); - Point2 point1_(2.0, 4.0); - superset.insert(x1, pose1_); - superset.insert(l1, point1_); - superset.insert(x2, pose2); - superset.insert(l2, point2); - init.update(superset); - - expected.insert(x1, pose1_); - expected.insert(l1, point1_); - - CHECK(assert_equal(expected, init)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, update_element ) -{ - TupPairConfig cfg; - PoseKey xk(1); - PointKey lk(1); - - cfg.insert(xk, x1_val); - EXPECT(cfg.size() == 1); - EXPECT(assert_equal(x1_val, cfg.at(xk))); - - cfg.update(xk, x2_val); - EXPECT(cfg.size() == 1); - EXPECT(assert_equal(x2_val, cfg.at(xk))); - - cfg.insert(lk, l1_val); - EXPECT(cfg.size() == 2); - EXPECT(assert_equal(l1_val, cfg.at(lk))); - - cfg.update(lk, l2_val); - EXPECT(cfg.size() == 2); - EXPECT(assert_equal(l2_val, cfg.at(lk))); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, expmap) -{ - Pose2 xA1(1,2,3), xA2(6,7,8); - PoseKey x1k(1), x2k(2); - Point2 lA1(4,5), lA2(9,10); - PointKey l1k(1), l2k(2); - - TupPairConfig config1; - config1.insert(x1k, xA1); - config1.insert(x2k, xA2); - config1.insert(l1k, lA1); - config1.insert(l2k, lA2); - - VectorConfig delta; - delta.insert("x1", Vector_(3, 1.0, 1.1, 1.2)); - delta.insert("x2", Vector_(3, 1.3, 1.4, 1.5)); - delta.insert("l1", Vector_(2, 1.0, 1.1)); - delta.insert("l2", Vector_(2, 1.3, 1.4)); - - TupPairConfig expected; - expected.insert(x1k, expmap(xA1, Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, expmap(xA2, Vector_(3, 1.3, 1.4, 1.5))); - expected.insert(l1k, Point2(5.0, 6.1)); - expected.insert(l2k, Point2(10.3, 11.4)); - - CHECK(assert_equal(expected, expmap(config1, delta))); - CHECK(assert_equal(delta, logmap(config1, expected))); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, configN) -{ - typedef FusionTupleConfig2 ConfigA; - PointConfig expPointConfig; - expPointConfig.insert(l1, l1_val); - expPointConfig.insert(l2, l2_val); - - PoseConfig expPoseConfig; - expPoseConfig.insert(x1, x1_val); - expPoseConfig.insert(x2, x2_val); - - ConfigA cfg1; - EXPECT(cfg1.empty()); - - ConfigA cfg2(expPoseConfig, expPointConfig); - - EXPECT(assert_equal(expPoseConfig, cfg2.config())); - EXPECT(assert_equal(expPointConfig, cfg2.config())); - - EXPECT(assert_equal(expPoseConfig, cfg2.first())); - EXPECT(assert_equal(expPointConfig, cfg2.second())); - - ConfigA cfg3(cfg2); - EXPECT(assert_equal(cfg2, cfg3)); -} - -/* ************************************************************************* */ -TEST( testFusionTupleConfig, basic_factor) -{ - // planar example system - typedef FusionTupleConfig2 Config; // pair config - typedef FusionTupleConfig1 TestPoseConfig; - typedef NonlinearFactorGraph Graph; - typedef NonlinearOptimizer Optimizer; - - // Factors -// typedef PriorFactor Prior; // fails to add to graph - typedef PriorFactor Prior; - typedef BetweenFactor Odometry; - typedef BearingRangeFactor BearingRange; - - PoseKey pose1k(1), pose2k(2), pose3k(3); - Pose2 pose1, pose2(2.0, 0.0, 0.0), pose3(4.0, 0.0, 0.0); - SharedDiagonal prior_model = noiseModel::Isotropic::Sigma(3, 0.1); - SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - - Graph graph; - graph.add(Prior(pose1k, pose1, prior_model)); - graph.add(Odometry(pose1k, pose2k, between(pose1, pose2), odom_model)); - graph.add(Odometry(pose2k, pose3k, between(pose2, pose3), odom_model)); - - Config init; - init.insert(pose1k, Pose2(0.2, 0.4, 0.0)); - init.insert(pose2k, Pose2(1.8,-0.4, 0.3)); - init.insert(pose3k, Pose2(4.1, 0.0,-0.2)); - - Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init); - - Config expected; - expected.insert(pose1k, pose1); - expected.insert(pose2k, pose2); - expected.insert(pose3k, pose3); - - EXPECT(assert_equal(expected, *actual, tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */