From 79c09708e89392864afe006008bcdab8a47aa2a6 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 2 Jun 2011 20:35:02 +0000 Subject: [PATCH] Assorted cleanup to remove ublas references, switch more Vector/Matrix utility functions to use Eigen in templates, reimplemented backsubstitution with Eigen --- gtsam/3rdparty/Eigen/Makefile.am | 1 - gtsam/base/Matrix.cpp | 62 ++++--------------- gtsam/base/Matrix.h | 8 +-- gtsam/base/Vector.cpp | 25 +------- gtsam/base/Vector.h | 40 ++++++++---- gtsam/base/blockMatrices.h | 6 +- gtsam/base/tests/testCholesky.cpp | 52 ---------------- gtsam/base/tests/testVector.cpp | 9 ++- gtsam/base/tests/timeMatrix.cpp | 2 - gtsam/inference/Conditional.h | 2 - gtsam/inference/Factor.h | 3 - gtsam/linear/Errors.cpp | 3 +- gtsam/linear/Errors.h | 3 +- gtsam/linear/GaussianConditional.cpp | 1 - gtsam/linear/GaussianConditional.h | 1 - gtsam/linear/GaussianFactorGraph.cpp | 2 +- gtsam/linear/GaussianFactorGraph.h | 3 - gtsam/linear/GaussianISAM.cpp | 2 - gtsam/linear/GaussianMultifrontalSolver.cpp | 1 - gtsam/linear/GaussianSequentialSolver.cpp | 1 - gtsam/linear/HessianFactor.cpp | 7 --- gtsam/linear/HessianFactor.h | 8 +-- gtsam/linear/JacobianFactor.cpp | 1 - gtsam/linear/JacobianFactor.h | 14 +---- gtsam/linear/Makefile.am | 2 +- gtsam/linear/SubgraphPreconditioner.cpp | 2 +- gtsam/linear/tests/testGaussianFactor.cpp | 2 +- .../linear/tests/testGaussianFactorGraph.cpp | 37 +++++------ gtsam/linear/tests/testHessianFactor.cpp | 2 +- gtsam/linear/tests/testNoiseModel.cpp | 6 +- gtsam/linear/tests/testVectorBTree.cpp | 1 - gtsam/nonlinear/NonlinearFactor.h | 8 --- 32 files changed, 81 insertions(+), 236 deletions(-) diff --git a/gtsam/3rdparty/Eigen/Makefile.am b/gtsam/3rdparty/Eigen/Makefile.am index 1902d0144..d99a72e02 100644 --- a/gtsam/3rdparty/Eigen/Makefile.am +++ b/gtsam/3rdparty/Eigen/Makefile.am @@ -4,7 +4,6 @@ SUBDIRS = src # And the corresponding libraries produced -# TODO add sublibs when something in 3rdparty actually needs compilation SUBLIBS = # use nostdinc to turn off -I. and -I.., we do not need them because diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 6340806da..d44701448 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -522,67 +522,27 @@ void householder(MatrixColMajor& A, size_t k) { /* ************************************************************************* */ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) { - size_t n = L.cols(); -#ifndef NDEBUG - size_t m = L.rows(); - if (m!=n) - throw invalid_argument("backSubstituteLower: L must be square"); -#endif - - Vector result(n); - for (size_t i = 1; i <= n; i++) { - double zi = b(i-1); - for (size_t j = 1; j < i; j++) - zi -= L(i-1,j-1) * result(j-1); -#ifndef NDEBUG - if(!unit && fabs(L(i-1,i-1)) <= numeric_limits::epsilon()) { - stringstream ss; - ss << "backSubstituteUpper: L is singular,\n"; - print(L, "L: ", ss); - throw invalid_argument(ss.str()); - } -#endif - if (!unit) zi /= L(i-1,i-1); - result(i-1) = zi; - } - - return result; + // @return the solution x of L*x=b + assert(L.rows() == L.cols()); + if (unit) + return L.triangularView().solve(b); + else + return L.triangularView().solve(b); } /* ************************************************************************* */ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) { // @return the solution x of U*x=b - size_t n = U.cols(); -#ifndef NDEBUG - size_t m = U.rows(); - if (m!=n) - throw invalid_argument("backSubstituteUpper: U must be square"); -#endif - - Vector result(n); - for (size_t i = n; i > 0; i--) { - double zi = b(i-1); - for (size_t j = i+1; j <= n; j++) - zi -= U(i-1,j-1) * result(j-1); -#ifndef NDEBUG - if(!unit && fabs(U(i-1,i-1)) <= numeric_limits::epsilon()) { - stringstream ss; - ss << "backSubstituteUpper: U is singular,\n"; - print(U, "U: ", ss); - throw invalid_argument(ss.str()); - } -#endif - if (!unit) zi /= U(i-1,i-1); - result(i-1) = zi; - } - - return result; + assert(U.rows() == U.cols()); + if (unit) + return U.triangularView().solve(b); + else + return U.triangularView().solve(b); } /* ************************************************************************* */ Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) { // @return the solution x of x'*U=b' - size_t n = U.cols(); #ifndef NDEBUG size_t m = U.rows(); diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 438099b88..972cd6339 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -30,16 +30,9 @@ /** * Matrix is a *global* typedef - * wrap-matlab does this typedef as well * we use the default < double,row_major,unbounded_array > */ -// FIXME: replace to handle matlab wrapper -//#if ! defined (MEX_H) -//typedef boost::numeric::ublas::matrix Matrix; -//typedef boost::numeric::ublas::matrix MatrixColMajor; -//#endif - typedef Eigen::Matrix Matrix; typedef Eigen::Matrix MatrixColMajor; @@ -373,6 +366,7 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false); * @return the solution x of x'*U=b' */ //FIXME: add back expression form +//TODO: is this function necessary? it isn't used //template //Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false); Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false); diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 625d767ba..d5046d81a 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -11,7 +11,7 @@ /** * @file Vector.cpp - * @brief typedef and functions to augment Boost's ublas::vector + * @brief typedef and functions to augment Eigen's Vectors * @author Kai Ni * @author Frank Dellaert */ @@ -312,29 +312,6 @@ double max(const Vector &a) { return a.maxCoeff(); } -/* ************************************************************************* */ -double dot(const Vector& a, const Vector& b) { - assert (b.size()==a.size()); - return a.dot(b); -} - -/* ************************************************************************* */ -void scal(double alpha, Vector& x) { - x *= alpha; -} - -/* ************************************************************************* */ -void axpy(double alpha, const Vector& x, Vector& y) { - assert (y.size()==x.size()); - y += alpha * x; -} - -/* ************************************************************************* */ -void axpy(double alpha, const Vector& x, SubVector y) { - assert (y.size()==x.size()); - y += alpha * x; -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index aba488e04..97c34f499 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -11,7 +11,7 @@ /** * @file Vector.h - * @brief typedef and functions to augment Boost's ublas::vector + * @brief typedef and functions to augment Eigen's VectorXd * @author Kai Ni * @author Frank Dellaert */ @@ -26,11 +26,6 @@ #include // Vector is a *global* typedef -// wrap-matlab does this typedef as well -// TODO: fix matlab wrapper -//#if ! defined (MEX_H) -//typedef boost::numeric::ublas::vector Vector; -//#endif // Typedef arbitary length vector typedef Eigen::VectorXd Vector; @@ -263,20 +258,41 @@ Vector abs(const Vector& v); */ double max(const Vector &a); -/** Dot product */ -double dot(const Vector &a, const Vector& b); +/** + * Dot product + */ +template +inline double dot(const V1 &a, const V2& b) { + assert (b.size()==a.size()); + return a.dot(b); +} + +/** compatibility version for ublas' inner_prod() */ +template +inline double inner_prod(const V1 &a, const V2& b) { + assert (b.size()==a.size()); + return a.dot(b); +} -// TODO: remove simple blas functions - these are one-liners with Eigen /** * BLAS Level 1 scal: x <- alpha*x + * @DEPRECIATED: use operators instead */ -void scal(double alpha, Vector& x); +inline void scal(double alpha, Vector& x) { x *= alpha; } /** * BLAS Level 1 axpy: y <- alpha*x + y + * @DEPRECIATED: use operators instead */ -void axpy(double alpha, const Vector& x, Vector& y); -void axpy(double alpha, const Vector& x, SubVector y); +template +inline void axpy(double alpha, const V1& x, V2& y) { + assert (y.size()==x.size()); + y += alpha * x; +} +inline void axpy(double alpha, const Vector& x, SubVector y) { + assert (y.size()==x.size()); + y += alpha * x; +} /** * house(x,j) computes HouseHolder vector v and scaling factor beta diff --git a/gtsam/base/blockMatrices.h b/gtsam/base/blockMatrices.h index 377b4db40..e1a8fdb53 100644 --- a/gtsam/base/blockMatrices.h +++ b/gtsam/base/blockMatrices.h @@ -243,8 +243,7 @@ public: template VerticalBlockView& assignNoalias(const RHS& rhs) { copyStructureFrom(rhs); -// boost::numeric::ublas::noalias(matrix_) = rhs.full(); - matrix_ = rhs.full(); // FIXME: check if noalias is necessary here + matrix_.noalias() = rhs.full(); return *this; } @@ -530,8 +529,7 @@ public: template SymmetricBlockView& assignNoalias(const SymmetricBlockView& rhs) { copyStructureFrom(rhs); -// boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks(), 0, rhs.nBlocks()); - matrix_ = rhs.matrix_.block(0, 0, rhs.nBlocks(), rhs.nBlocks()); + matrix_.noalias() = rhs.matrix_.block(0, 0, rhs.nBlocks(), rhs.nBlocks()); return *this; } diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index a07e6a08d..c095d7c71 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -20,55 +20,7 @@ #include #include -//#include -//#include -//#include - using namespace gtsam; -//namespace ublas = boost::numeric::ublas; - -///* ************************************************************************* */ -//TEST(cholesky, cholesky_inplace) { -// Matrix I = Matrix_(5,5, -// 1.739214152118470, 0.714164147286383, 1.893437990040579, 1.469485110232169, 1.549910412077052, -// 0.714164147286383, 0.587767128354794, 0.738548568260523, 0.588775778746414, 0.645015863153235, -// 1.893437990040579, 0.738548568260523, 2.165530165155413, 1.631203698494913, 1.617901992621506, -// 1.469485110232169, 0.588775778746414, 1.631203698494913, 1.591363675348797, 1.584779118350080, -// 1.549910412077052, 0.645015863153235, 1.617901992621506, 1.584779118350080, 1.928887183904716); -// -// Matrix expected = Matrix_(5,5, -// 1.318792687316119, 0.541528743793524, 1.435735888021887, 1.114265437142152, 1.175249473995251, -// 0.0, 0.542691208699940, -0.071760299365346, -0.026960052875681, 0.015818372803848, -// 0.0, 0.0, 0.314711112667495, 0.093667363355578, -0.217058504307151, -// 0.0, 0.0, 0.0, 0.583331630832890, 0.507424929100112, -// 0.0, 0.0, 0.0, 0.0, 0.492779041656733); -// -// MatrixColMajor actualColMaj = I; -// cholesky_inplace(actualColMaj); -// Matrix actual = ublas::triangular_adaptor(actualColMaj); -// CHECK(assert_equal(expected, actual, 1e-7)); -//} - -///* ************************************************************************* */ -//TEST(cholesky, choleskyFactorUnderdetermined) { -// -// Matrix Ab = Matrix_(3,7, -// 0.0357, 0.6787, 0.3922, 0.7060, 0.0462, 0.6948, 0.0344, -// 0.8491, 0.7577, 0.6555, 0.0318, 0.0971, 0.3171, 0.4387, -// 0.9340, 0.7431, 0.1712, 0.2769, 0.8235, 0.9502, 0.3816); -// -// Matrix expected = Matrix_(3,7, -// 1.2628, 1.0784, 0.5785, 0.2462, 0.6757, 0.9357, 0.5782, -// 0.0, 0.6513, 0.4089, 0.6811, -0.0180, 0.6280, 0.0244, -// 0.0, 0.0, 0.3332, -0.2273, -0.4825, -0.4652, 0.0660); -// -// MatrixColMajor actualColmaj = Ab; -// choleskyFactorUnderdetermined(actualColmaj, 3); -// -// Matrix actual = ublas::triangular_adaptor(actualColmaj); -// -// CHECK(assert_equal(expected, actual, 1e-3)); -//} /* ************************************************************************* */ TEST(cholesky, choleskyPartial) { @@ -91,12 +43,8 @@ TEST(cholesky, choleskyPartial) { // See the function comment for choleskyPartial, this decomposition should hold. MatrixColMajor R1 = RSL.transpose(); MatrixColMajor R2 = RSL; -// ublas::project(R1, ublas::range(3,7), ublas::range(3,7)) = eye(4,4); R1.block(3, 3, 4, 4).setIdentity(); -// ublas::project(R2, ublas::range(3,7), ublas::range(3,7)) = -// ublas::symmetric_adaptor,ublas::upper>( -// ublas::project(R2, ublas::range(3,7), ublas::range(3,7))); R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView(); MatrixColMajor actual = R1 * R2; diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 86c27ce1b..7506043c3 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -239,10 +239,13 @@ TEST( TestVector, dot ) TEST( TestVector, axpy ) { Vector x = Vector_(3,10.,20.,30.); - Vector y = Vector_(3,2.0,5.0,6.0); - axpy(0.1,x,y); + Vector y0 = Vector_(3,2.0,5.0,6.0); + Vector y1 = y0, y2 = y0; + axpy(0.1,x,y1); + axpy(0.1,x,y2.head(3)); Vector expected = Vector_(3,3.0,7.0,9.0); - EXPECT(assert_equal(expected,y)); + EXPECT(assert_equal(expected,y1)); + EXPECT(assert_equal(expected,Vector(y2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/timeMatrix.cpp b/gtsam/base/tests/timeMatrix.cpp index 29731bdd4..b85183854 100644 --- a/gtsam/base/tests/timeMatrix.cpp +++ b/gtsam/base/tests/timeMatrix.cpp @@ -16,13 +16,11 @@ */ #include -#include #include #include using namespace std; using namespace gtsam; -namespace ublas = boost::numeric::ublas; /* * Results: diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index bc4e4051c..fa870537b 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -34,8 +34,6 @@ namespace gtsam { * provides storage for the keys involved in a conditional, and iterators and * access to the frontal and separator keys. * - * todo: Move permutation functions to IndexConditional. - * * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer * to the associated factor type and shared_ptr type of the derived class. See * IndexConditional and GaussianConditional for examples. diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 2e8b42603..ec2387bf7 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -40,9 +40,6 @@ template class Conditional; * class, using Index keys. This class does not store any data other than its * keys. Derived classes store data such as matrices and probability tables. * - * todo: Make NonlinearFactor derive from this too, which requires moving - * Combine, eliminate*, permute* and the sorted key invariant to IndexFactor. - * * Note that derived classes *must* redefine the ConditionalType and shared_ptr * typedefs to refer to the associated conditional and shared_ptr types of the * derived class. See IndexFactor, JacobianFactor, etc. for examples. diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index 911f78b1d..71d0cc003 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -103,7 +103,8 @@ double dot(const Errors& a, const Errors& b) { } /* ************************************************************************* */ -void axpy(double alpha, const Errors& x, Errors& y) { +template<> +void axpy(double alpha, const Errors& x, Errors& y) { Errors::const_iterator it = x.begin(); BOOST_FOREACH(Vector& yi, y) axpy(alpha,*(it++),yi); diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index f683e91f0..76b756b00 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -60,7 +60,8 @@ namespace gtsam { /** * BLAS level 2 style */ - void axpy(double alpha, const Errors& x, Errors& y); + template <> + void axpy(double alpha, const Errors& x, Errors& y); /** print with optional string */ void print(const Errors& a, const std::string& s = "Error"); diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 07baaa05f..d131a8618 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -148,7 +148,6 @@ Vector GaussianConditional::solve(const VectorValues& x) const { Vector rhs(get_d()); for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { rhs += -get_S(parent) * x[*parent]; -// ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false); } if(debug) gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on "); if(debug) gtsam::print(rhs, "rhs: "); diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index b82811743..69381a5b5 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -47,7 +47,6 @@ public: typedef boost::shared_ptr shared_ptr; /** Store the conditional matrix as upper-triangular column-major */ -// typedef boost::numeric::ublas::triangular_matrix AbMatrix; typedef MatrixColMajor AbMatrix; typedef VerticalBlockView rsd_type; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 7cff84559..ff5b5f71f 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -275,7 +275,7 @@ namespace gtsam { } /* ************************************************************************* */ - //static + static FastMap findScatterAndDims (const FactorGraph& factors) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 092724919..1e3ac8fd6 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -61,9 +61,6 @@ namespace gtsam { return e; } - /** DEBUG ONLY: TODO: hide again later */ - FastMap findScatterAndDims(const FactorGraph& factors); - /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == GaussianFactor diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index 96664f9f8..a7911f2cb 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -24,8 +24,6 @@ using namespace gtsam; #include template class ISAM; -//namespace ublas = boost::numeric::ublas; - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianMultifrontalSolver.cpp b/gtsam/linear/GaussianMultifrontalSolver.cpp index 6093b6493..45b51c97f 100644 --- a/gtsam/linear/GaussianMultifrontalSolver.cpp +++ b/gtsam/linear/GaussianMultifrontalSolver.cpp @@ -72,7 +72,6 @@ std::pair GaussianMultifrontalSolver::marginalCovariance(Index j GaussianConditional::shared_ptr conditional = EliminateQR(fg,1).first->front(); Matrix R = conditional->get_R(); return make_pair(conditional->get_d(), (R.transpose() * R).inverse()); -// return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R))); } } diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp index 6fe773d2d..f2610ae80 100644 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ b/gtsam/linear/GaussianSequentialSolver.cpp @@ -88,7 +88,6 @@ std::pair GaussianSequentialSolver::marginalCovariance(Index j) GaussianConditional::shared_ptr conditional = EliminateQR(fg, 1).first->front(); Matrix R = conditional->get_R(); return make_pair(conditional->get_d(), (R.transpose() * R).inverse()); -// return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R),R))); } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f7021f531..652ab28ef 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -105,8 +105,6 @@ HessianFactor::HessianFactor(Index j1, Index j2, HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) { JacobianFactor jf(cg); info_.copyStructureFrom(jf.Ab_); - // ublas::noalias(ublas::symmetric_adaptor(matrix_)) = - // ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); matrix_.noalias() = jf.matrix_.transpose() * jf.matrix_; assertInvariants(); } @@ -175,8 +173,6 @@ void HessianFactor::print(const std::string& s) const { for(const_iterator key=this->begin(); key!=this->end(); ++key) cout << *key << "(" << this->getDim(key) << ") "; cout << "\n"; - // gtsam::print(ublas::symmetric_adaptor( - // info_.range(0,info_.nBlocks(), 0,info_.nBlocks())), "Ab^T * Ab: "); gtsam::print(MatrixColMajor(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); } @@ -186,10 +182,8 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { return false; else { MatrixColMajor thisMatrix = this->info_.full().selfadjointView(); - // MatrixColMajor thisMatrix = ublas::symmetric_adaptor(this->info_.full()); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; MatrixColMajor rhsMatrix = static_cast(lf).info_.full().selfadjointView(); - // MatrixColMajor rhsMatrix = ublas::symmetric_adaptor(static_cast(lf).info_.full()); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); } @@ -444,7 +438,6 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& key } tic(2, "construct cond"); - // const ublas::scalar_vector sigmas(varDim, 1.0); Vector sigmas = Vector::Ones(varDim); conditionals->push_back(boost::make_shared(keys.begin()+j, keys.end(), 1, Ab, sigmas)); toc(2, "construct cond"); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 39f8a5822..d5628d7d3 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -25,6 +25,8 @@ // Forward declarations for friend unit tests class ConversionConstructorHessianFactorTest; +class Constructor1HessianFactorTest; + namespace gtsam { @@ -118,11 +120,6 @@ namespace gtsam { /** returns the full linear term - g from the constructors */ constColumn linear_term() const; - /** const access to the full matrix DEBUG ONLY*/ - const InfoMatrix& raw_matrix() const { return matrix_; } - - const BlockInfo& raw_info() const { return info_; } /// DEBUG ONLY - /** * Permutes the GaussianFactor, but for efficiency requires the permutation * to already be inverted. This acts just as a change-of-name for each @@ -133,6 +130,7 @@ namespace gtsam { // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; + friend class ::Constructor1HessianFactorTest; // Friend JacobianFactor for conversion friend class JacobianFactor; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 255cb7a2d..629015a54 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -164,7 +164,6 @@ namespace gtsam { Ab_.assignNoalias(factor.info_); size_t maxrank = choleskyCareful(matrix_).first; // FIXME: replace with triangular system -// matrix_ = ublas::triangular_adaptor(matrix_); Ab_.rowEnd() = maxrank; model_ = noiseModel::Unit::Create(maxrank); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 368681a77..d5c8fde41 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -32,6 +32,7 @@ // Forward declarations of friend unit tests class Combine2GaussianFactorTest; class eliminateFrontalsGaussianFactorTest; +class constructor2GaussianFactorTest; namespace gtsam { @@ -183,18 +184,6 @@ namespace gtsam { */ Matrix matrix_augmented(bool weight = true) const; - /** - * Returns full dense matrix underying factor - * TESTING ONLY - */ - const AbMatrix& raw_matrix() const { return matrix_; } - - /** - * Returns the block indexing view - * TESTING ONLY - */ - const BlockAb& Ab() const { return Ab_; } - /** * Return vector of i, j, and s to generate an m-by-n sparse matrix * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. @@ -219,6 +208,7 @@ namespace gtsam { // Friend unit tests (see also forward declarations above) friend class ::Combine2GaussianFactorTest; friend class ::eliminateFrontalsGaussianFactorTest; + friend class ::constructor2GaussianFactorTest; /* Used by ::CombineJacobians for sorting */ struct _RowSource { diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index 85ad8e3b6..857641c81 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -38,7 +38,7 @@ headers += iterative-inl.h sources += iterative.cpp SubgraphPreconditioner.cpp headers += IterativeSolver.h IterativeOptimizationParameters.h headers += SubgraphSolver.h SubgraphSolver-inl.h -#sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp +#sources += BayesNetPreconditioner.cpp #check_PROGRAMS += tests/testBayesNetPreconditioner # Timing tests diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 7d4da0c2f..f94e07ced 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -122,7 +122,7 @@ namespace gtsam { Errors::const_iterator it = e.begin(); for ( Index i = 0 ; i < y.size() ; ++i, ++it ) { const Vector& ei = *it; - axpy(alpha,ei,y[i]) ; + axpy(alpha,ei,y[i]); } sp.transposeMultiplyAdd2(alpha,it,e.end(),y); } diff --git a/gtsam/linear/tests/testGaussianFactor.cpp b/gtsam/linear/tests/testGaussianFactor.cpp index 30e1a2123..984d3d2c6 100644 --- a/gtsam/linear/tests/testGaussianFactor.cpp +++ b/gtsam/linear/tests/testGaussianFactor.cpp @@ -59,7 +59,7 @@ TEST(GaussianFactor, constructor2) EXPECT(assert_equal(*key0, _x0_)); EXPECT(assert_equal(*key1, _x1_)); EXPECT(!actual.empty()); - EXPECT_LONGS_EQUAL(3, actual.Ab().nBlocks()); + EXPECT_LONGS_EQUAL(3, actual.Ab_.nBlocks()); Matrix actualA0 = actual.getA(key0); Matrix actualA1 = actual.getA(key1); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 98640aa81..ea90e7249 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -334,13 +334,6 @@ TEST(GaussianFactor, eliminateFrontals) // Create first factor (from pieces of Ab) list > terms1; -// terms1 += -// make_pair( 3, Matrix(ublas::project(Ab, ublas::range(0,4), ublas::range(0,2)))), -// make_pair( 5, ublas::project(Ab, ublas::range(0,4), ublas::range(2,4))), -// make_pair( 7, ublas::project(Ab, ublas::range(0,4), ublas::range(4,6))), -// make_pair( 9, ublas::project(Ab, ublas::range(0,4), ublas::range(6,8))), -// make_pair(11, ublas::project(Ab, ublas::range(0,4), ublas::range(8,10))); -// Vector b1 = ublas::project(ublas::column(Ab, 10), ublas::range(0,4)); terms1 += make_pair( 3, Matrix(Ab.block(0, 0, 4, 2))), @@ -348,33 +341,33 @@ TEST(GaussianFactor, eliminateFrontals) make_pair( 7, Matrix(Ab.block(0, 4, 4, 2))), make_pair( 9, Matrix(Ab.block(0, 6, 4, 2))), make_pair(11, Matrix(Ab.block(0, 8, 4, 2))); - Vector b1 = Ab.col(10).segment(0, 4); //ublas::project(ublas::column(Ab, 10), ublas::range(0,4)); + Vector b1 = Ab.col(10).segment(0, 4); JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, sharedSigma(4, 0.5))); // Create second factor list > terms2; terms2 += - make_pair(5, Matrix(Ab.block(4, 2, 4, 2))), //make_pair(5, ublas::project(Ab, ublas::range(4,8), ublas::range(2,4))), - make_pair(7, Matrix(Ab.block(4, 4, 4, 2))), //make_pair(7, ublas::project(Ab, ublas::range(4,8), ublas::range(4,6))), - make_pair(9, Matrix(Ab.block(4, 6, 4, 2))), //make_pair(9, ublas::project(Ab, ublas::range(4,8), ublas::range(6,8))), - make_pair(11,Matrix(Ab.block(4, 8, 4, 2))); //make_pair(11,ublas::project(Ab, ublas::range(4,8), ublas::range(8,10))); - Vector b2 = Ab.col(10).segment(4, 4); //ublas::project(ublas::column(Ab, 10), ublas::range(4,8)); + make_pair(5, Matrix(Ab.block(4, 2, 4, 2))), + make_pair(7, Matrix(Ab.block(4, 4, 4, 2))), + make_pair(9, Matrix(Ab.block(4, 6, 4, 2))), + make_pair(11,Matrix(Ab.block(4, 8, 4, 2))); + Vector b2 = Ab.col(10).segment(4, 4); JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, sharedSigma(4, 0.5))); // Create third factor list > terms3; terms3 += - make_pair(7, Matrix(Ab.block(8, 4, 4, 2))), //make_pair(7, ublas::project(Ab, ublas::range(8,12), ublas::range(4,6))), - make_pair(9, Matrix(Ab.block(8, 6, 4, 2))), //make_pair(9, ublas::project(Ab, ublas::range(8,12), ublas::range(6,8))), - make_pair(11,Matrix(Ab.block(8, 8, 4, 2))); //make_pair(11,ublas::project(Ab, ublas::range(8,12), ublas::range(8,10))); - Vector b3 = Ab.col(10).segment(8, 4); //ublas::project(ublas::column(Ab, 10), ublas::range(8,12)); + make_pair(7, Matrix(Ab.block(8, 4, 4, 2))), + make_pair(9, Matrix(Ab.block(8, 6, 4, 2))), + make_pair(11,Matrix(Ab.block(8, 8, 4, 2))); + Vector b3 = Ab.col(10).segment(8, 4); JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, sharedSigma(4, 0.5))); // Create fourth factor list > terms4; terms4 += - make_pair(11, Matrix(Ab.block(12, 8, 2, 2))); //make_pair(11, ublas::project(Ab, ublas::range(12,14), ublas::range(8,10))); - Vector b4 = Ab.col(10).segment(12, 2); //ublas::project(ublas::column(Ab, 10), ublas::range(12,14)); + make_pair(11, Matrix(Ab.block(12, 8, 2, 2))); + Vector b4 = Ab.col(10).segment(12, 2); JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, sharedSigma(2, 0.5))); // Create factor graph @@ -424,7 +417,7 @@ TEST(GaussianFactor, eliminateFrontals) make_pair(7, sub(R, 2,4, 4,6)), make_pair(9, sub(R, 2,4, 6,8)), make_pair(11,sub(R, 2,4, 8,10)); - Vector d2 = R.col(10).segment(2,2); //ublas::project(ublas::column(R, 10), ublas::range(2,4)); + Vector d2 = R.col(10).segment(2,2); GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2))); // Expected conditional on third variable from next 2 rows of R @@ -433,7 +426,7 @@ TEST(GaussianFactor, eliminateFrontals) cterms3 += make_pair(9, sub(R, 4,6, 6,8)), make_pair(11, sub(R, 4,6, 8,10)); - Vector d3 = R.col(10).segment(4,2); //ublas::project(ublas::column(R, 10), ublas::range(4,6)); + Vector d3 = R.col(10).segment(4,2); GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2))); // Create expected Bayes net fragment from three conditionals above @@ -445,7 +438,7 @@ TEST(GaussianFactor, eliminateFrontals) // Get expected matrices for remaining factor Matrix Ae1 = sub(R, 6,10, 6,8); Matrix Ae2 = sub(R, 6,10, 8,10); - Vector be = R.col(10).segment(6, 4); //blas::project(ublas::column(R, 10), ublas::range(6,10)); + Vector be = R.col(10).segment(6, 4); // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 66983bf44..4b097a81c 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -113,7 +113,7 @@ TEST(HessianFactor, Constructor1) HessianFactor factor(0, G, g, f); // extract underlying parts - MatrixColMajor info_matrix = factor.raw_info().range(0, 1, 0, 1); + MatrixColMajor info_matrix = factor.info_.range(0, 1, 0, 1); EXPECT(assert_equal(MatrixColMajor(G), info_matrix)); EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10); EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10)); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9d7f99717..178b5c4dd 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -231,12 +231,12 @@ TEST(NoiseModel, Cholesky) SharedDiagonal expected = noiseModel::Unit::Create(4); MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab, 4); // ASSERTION FAILURE: access out of bounds -// EXPECT(assert_equal(*expected,*actual)); + EXPECT(assert_equal(*expected,*actual)); // Ab was modified in place !!! -// MatrixColMajor actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView(); + MatrixColMajor actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView(); // ublas::project(ublas::triangular_adaptor(Ab), // ublas::range(0,actual->dim()), ublas::range(0, Ab.cols())); -// EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); + EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); // FIXME: enable test } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorBTree.cpp b/gtsam/linear/tests/testVectorBTree.cpp index 842f227c2..bc685767a 100644 --- a/gtsam/linear/tests/testVectorBTree.cpp +++ b/gtsam/linear/tests/testVectorBTree.cpp @@ -19,7 +19,6 @@ #include #include -#include #include using namespace boost::assign; // bring 'operator+=()' into scope diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 1a64aa42b..d2c3e20fa 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -36,14 +36,6 @@ #include -// FIXME: is this necessary? -#define INSTANTIATE_NONLINEAR_FACTOR1(C,J) \ - template class gtsam::NonlinearFactor1; -#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,J2) \ - template class gtsam::NonlinearFactor2; -#define INSTANTIATE_NONLINEAR_FACTOR3(C,J1,J2,J3) \ - template class gtsam::NonlinearFactor3; - namespace gtsam { /**