From 200ac4e8621f375b763456ad823a6a2010e47d97 Mon Sep 17 00:00:00 2001 From: Rahul Sawhney Date: Thu, 21 Oct 2010 16:32:11 +0000 Subject: [PATCH] some cleanup / cleanup flags, capitalization --- base/BTree.h | 54 +++++++------- base/DSF.h | 44 +++++------ base/DenseQRUtil.cpp | 17 +---- base/DenseQRUtil.h | 1 + base/FastMap.h | 4 +- base/FastSet.h | 4 +- base/LieVector.h | 1 - base/Matrix-inl.h | 20 ++--- base/Matrix.cpp | 147 ++++--------------------------------- base/Matrix.h | 3 +- base/Vector.cpp | 2 +- base/blockMatrices.h | 94 +++++++++++++----------- base/numericalDerivative.h | 2 +- base/svdcmp.cpp | 11 --- 14 files changed, 138 insertions(+), 266 deletions(-) diff --git a/base/BTree.h b/base/BTree.h index 8113d7dbd..130ecb48a 100644 --- a/base/BTree.h +++ b/base/BTree.h @@ -26,12 +26,12 @@ namespace gtsam { /** * @brief Binary tree */ - template + template class BTree { public: - typedef std::pair value_type; + typedef std::pair value_type; private: @@ -63,8 +63,8 @@ namespace gtsam { keyValue_(keyValue), left(l), right(r) { } - inline const Key& key() const { return keyValue_.first;} - inline const Value& value() const { return keyValue_.second;} + inline const KEY& key() const { return keyValue_.first;} + inline const VALUE& value() const { return keyValue_.second;} }; // Node @@ -74,8 +74,8 @@ namespace gtsam { sharedNode root_; inline const value_type& keyValue() const { return root_->keyValue_;} - inline const Key& key() const { return root_->key(); } - inline const Value& value() const { return root_->value(); } + inline const KEY& key() const { return root_->key(); } + inline const VALUE& value() const { return root_->value(); } inline const BTree& left() const { return root_->left; } inline const BTree& right() const { return root_->right; } @@ -133,7 +133,7 @@ namespace gtsam { /** add a key-value pair */ BTree add(const value_type& xd) const { if (empty()) return BTree(xd); - const Key& x = xd.first; + const KEY& x = xd.first; if (x == key()) return BTree(left(), xd, right()); else if (x < key()) @@ -143,12 +143,12 @@ namespace gtsam { } /** add a key-value pair */ - BTree add(const Key& x, const Value& d) const { + BTree add(const KEY& x, const VALUE& d) const { return add(make_pair(x, d)); } /** member predicate */ - bool mem(const Key& x) const { + bool mem(const KEY& x) const { if (!root_) return false; if (x == key()) return true; if (x < key()) @@ -202,7 +202,7 @@ namespace gtsam { } /** remove a key-value pair */ - BTree remove(const Key& x) const { + BTree remove(const KEY& x) const { if (!root_) return BTree(); if (x == key()) return merge(left(), right()); @@ -227,22 +227,22 @@ namespace gtsam { * find a value given a key, throws exception when not found * Optimized non-recursive version as [find] is crucial for speed */ - const Value& find(const Key& k) const { + const VALUE& find(const KEY& k) const { const Node* node = root_.get(); while (node) { - const Key& key = node->key(); + const KEY& key = node->key(); if (k < key) node = node->left.root_.get(); else if (key < k) node = node->right.root_.get(); - else /* (key() == k) */ return node->value(); + else return node->value(); } - //throw std::invalid_argument("BTree::find: key '" + (std::string) k + "' not found"); + throw std::invalid_argument("BTree::find: key not found"); } /** print in-order */ void print(const std::string& s = "") const { if (empty()) return; - Key k = key(); + KEY k = key(); std::stringstream ss; ss << height(); k.print(s + ss.str() + " "); @@ -251,7 +251,7 @@ namespace gtsam { } /** iterate over tree */ - void iter(boost::function f) const { + void iter(boost::function f) const { if (!root_) return; left().iter(f); f(key(), value()); @@ -259,11 +259,11 @@ namespace gtsam { } /** map key-values in tree over function f that computes a new value */ - template - BTree map(boost::function f) const { - if (empty()) return BTree (); - std::pair xd(key(), f(key(), value())); - return BTree (left().map(f), xd, right().map(f)); + template + BTree map(boost::function f) const { + if (empty()) return BTree (); + std::pair xd(key(), f(key(), value())); + return BTree (left().map(f), xd, right().map(f)); } /** @@ -272,12 +272,12 @@ namespace gtsam { * and [d1 ... dN] are the associated data. * The associated values are passed to [f] in reverse sort order */ - template - Acc fold(boost::function f, - const Acc& a) const { + template + ACC fold(boost::function f, + const ACC& a) const { if (!root_) return a; - Acc ar = right().fold(f, a); // fold over right subtree - Acc am = f(key(), value(), ar); // apply f with current value + ACC ar = right().fold(f, a); // fold over right subtree + ACC am = f(key(), value(), ar); // apply f with current value return left().fold(f, am); // fold over left subtree } @@ -330,7 +330,7 @@ namespace gtsam { // traits for playing nice with STL typedef ptrdiff_t difference_type; // correct ? typedef std::forward_iterator_tag iterator_category; - typedef std::pair value_type; + typedef std::pair value_type; typedef const value_type* pointer; typedef const value_type& reference; diff --git a/base/DSF.h b/base/DSF.h index 2d69b530f..09b8fa332 100644 --- a/base/DSF.h +++ b/base/DSF.h @@ -31,15 +31,15 @@ namespace gtsam { - template - class DSF : protected BTree { + template + class DSF : protected BTree { public: - typedef Key Label; // label can be different from key, but for now they are same - typedef DSF Self; - typedef std::set Set; - typedef BTree Tree; - typedef std::pair KeyLabel; + typedef KEY Label; // label can be different from key, but for now they are same + typedef DSF Self; + typedef std::set Set; + typedef BTree Tree; + typedef std::pair KeyLabel; // constructor DSF() : Tree() { } @@ -48,29 +48,29 @@ namespace gtsam { DSF(const Tree& tree) : Tree(tree) {} // constructor with a list of unconnected keys - DSF(const std::list& keys) : Tree() { BOOST_FOREACH(const Key& key, keys) *this = this->add(key, key); } + DSF(const std::list& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); } // create a new singleton, does nothing if already exists - Self makeSet(const Key& key) const { if (mem(key)) return *this; else return this->add(key, key); } + Self makeSet(const KEY& key) const { if (mem(key)) return *this; else return this->add(key, key); } // find the label of the set in which {key} lives - Label findSet(const Key& key) const { - Key parent = this->find(key); + Label findSet(const KEY& key) const { + KEY parent = this->find(key); return parent == key ? key : findSet(parent); } // return a new DSF where x and y are in the same set. Kai: the caml implementation is not const, and I followed - Self makeUnion(const Key& key1, const Key& key2) { return this->add(findSet_(key2), findSet_(key1)); } + Self makeUnion(const KEY& key1, const KEY& key2) { return this->add(findSet_(key2), findSet_(key1)); } // the in-place version of makeUnion - void makeUnionInPlace(const Key& key1, const Key& key2) { *this = this->add(findSet_(key2), findSet_(key1)); } + void makeUnionInPlace(const KEY& key1, const KEY& key2) { *this = this->add(findSet_(key2), findSet_(key1)); } // create a new singleton with two connected keys - Self makePair(const Key& key1, const Key& key2) const { return makeSet(key1).makeSet(key2).makeUnion(key1, key2); } + Self makePair(const KEY& key1, const KEY& key2) const { return makeSet(key1).makeSet(key2).makeUnion(key1, key2); } // create a new singleton with a list of fully connected keys - Self makeList(const std::list& keys) const { + Self makeList(const std::list& keys) const { Self t = *this; - BOOST_FOREACH(const Key& key, keys) + BOOST_FOREACH(const KEY& key, keys) t = t.makePair(key, keys.front()); return t; } @@ -84,7 +84,7 @@ namespace gtsam { } // maps f over all keys, must be invertible - DSF map(boost::function func) const { + DSF map(boost::function func) const { DSF t; BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) t = t.add(func(pair.first), func(pair.second)); @@ -111,9 +111,9 @@ namespace gtsam { } // return a partition of the given elements {keys} - std::map partition(const std::list& keys) const { + std::map partition(const std::list& keys) const { std::map partitions; - BOOST_FOREACH(const Key& key, keys) + BOOST_FOREACH(const KEY& key, keys) partitions[findSet(key)].insert(key); return partitions; } @@ -147,12 +147,12 @@ namespace gtsam { * same as findSet except with path compression: After we have traversed the path to * the root, each parent pointer is made to directly point to it */ - Key findSet_(const Key& key) { - Key parent = this->find(key); + KEY findSet_(const KEY& key) { + KEY parent = this->find(key); if (parent == key) return parent; else { - Key label = findSet_(parent); + KEY label = findSet_(parent); *this = this->add(key, label); return label; } diff --git a/base/DenseQRUtil.cpp b/base/DenseQRUtil.cpp index 9f142b2b3..108edcd5f 100644 --- a/base/DenseQRUtil.cpp +++ b/base/DenseQRUtil.cpp @@ -133,15 +133,13 @@ namespace gtsam { double wscale = 0; double wssq = 0; -// cholmod_common cc; -// cholmod_l_start(&cc); // todo: do something with the rank long rank = DenseQR(m, n, npiv, tol, ntol, fchunk, a, Stair, Rdead, Tau, W, &wscale, &wssq); toc("householder_denseqr: denseqr_front"); -//#ifndef NDEBUG + for(long j=0; jrow"); long k0 = 0; @@ -170,14 +167,11 @@ namespace gtsam { A(i,j) = a[k]; } -// ublas::matrix_range > Acolsub( -// ublas::project(Acolwise, ublas::range(0, min(m,n)), ublas::range(0,n))); -// ublas::matrix_range Asub(ublas::project(A, ublas::range(0, min(m,n)), ublas::range(0,n))); -// ublas::noalias(Asub) = ublas::triangular_adaptor(Acolsub); + toc("householder_denseqr: col->row"); -// cholmod_l_finish(&cc); + if(allocedStair) delete[] Stair; @@ -203,15 +197,12 @@ namespace gtsam { double wscale = 0; double wssq = 0; -// cholmod_common cc; -// cholmod_l_start(&cc); // todo: do something with the rank long rank = DenseQR(m, n, npiv, tol, ntol, fchunk, A.data().begin(), Stair, Rdead, Tau, W, &wscale, &wssq); toc("householder_denseqr: denseqr_front"); -//#ifndef NDEBUG for(long j=0; j& A, long *Stair); } #endif diff --git a/base/FastMap.h b/base/FastMap.h index 881273af6..965eeba38 100644 --- a/base/FastMap.h +++ b/base/FastMap.h @@ -41,8 +41,8 @@ public: FastMap() {} /** Constructor from a range, passes through to base class */ - template - FastMap(InputIterator first, InputIterator last) : Base(first, last) {} + template + FastMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} /** Copy constructor from another FastMap */ FastMap(const FastMap& x) : Base(x) {} diff --git a/base/FastSet.h b/base/FastSet.h index 133ac5636..58d69ce9c 100644 --- a/base/FastSet.h +++ b/base/FastSet.h @@ -38,8 +38,8 @@ public: typedef std::set, boost::fast_pool_allocator > Base; /** Constructor from a range, passes through to base class */ - template - FastSet(InputIterator first, InputIterator last) : Base(first, last) {} + template + FastSet(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} /** Copy constructor from another FastMap */ FastSet(const FastSet& x) : Base(x) {} diff --git a/base/LieVector.h b/base/LieVector.h index c11f39ff5..2570283fd 100644 --- a/base/LieVector.h +++ b/base/LieVector.h @@ -77,7 +77,6 @@ namespace gtsam { * Default Implementation calls global binary function */ inline Vector logmap(const LieVector& lp) const { -// return Logmap(between(lp)); // works return lp.vector() - vector(); } diff --git a/base/Matrix-inl.h b/base/Matrix-inl.h index 6fb4c8e4a..1e1c5dfe4 100644 --- a/base/Matrix-inl.h +++ b/base/Matrix-inl.h @@ -33,11 +33,11 @@ using namespace std; * @param b an RHS vector * @return the solution x of U*x=b */ -template -Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression& _U, - const boost::numeric::ublas::vector_expression& _b, bool unit=false) { - const MatrixAE& U((const MatrixAE&)_U); - const VectorAE& b((const VectorAE&)_b); +template +Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression& _U, + const boost::numeric::ublas::vector_expression& _b, bool unit=false) { + const MATRIXAE& U((const MATRIXAE&)_U); + const VECTORAE& b((const VECTORAE&)_b); size_t n = U.size2(); #ifndef NDEBUG size_t m = U.size1(); @@ -74,11 +74,11 @@ Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression -Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression& _b, - const boost::numeric::ublas::matrix_expression& _U, bool unit=false) { - const VectorAE& b((const VectorAE&)_b); - const MatrixAE& U((const MatrixAE&)_U); +template +Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression& _b, + const boost::numeric::ublas::matrix_expression& _U, bool unit=false) { + const VECTORAE& b((const VECTORAE&)_b); + const MATRIXAE& U((const MATRIXAE&)_U); size_t n = U.size2(); #ifndef NDEBUG size_t m = U.size1(); diff --git a/base/Matrix.cpp b/base/Matrix.cpp index 9295cbbf6..819d9bda7 100644 --- a/base/Matrix.cpp +++ b/base/Matrix.cpp @@ -144,11 +144,7 @@ bool equal_with_abs_tol(const Matrix& A, const Matrix& B, double tol) { equal = false; } -// if(!equal) { -// cout << "not equal:" << endl; -// print(A,"expected = "); -// print(B,"actual = "); -// } + return equal; } @@ -202,8 +198,7 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) { void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { #if defined GT_USE_CBLAS - // uncomment and run tests to verify blas is enabled -// throw runtime_error("You are in multiplyAdd!"); + // get sizes const size_t m = A.size1(), n = A.size2(); @@ -221,8 +216,7 @@ void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { cblas_dgemv(CblasRowMajor, CblasNoTrans, m, n, alpha, Aptr, ida, Xptr, incx, beta, Eptr, incy); #else -// throw runtime_error("You are in multiplyAdd / unoptimized!"); - // ublas e += prod(A,x) is terribly slow + size_t m = A.size1(), n = A.size2(); double * ei = e.data().begin(); const double * aij = A.data().begin(); @@ -236,7 +230,7 @@ void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { /* ************************************************************************* */ void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { - // ublas e += prod(A,x) is terribly slow + #ifdef GT_USE_CBLAS multiplyAdd(1.0, A, x, e); #else @@ -280,7 +274,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector cblas_dgemv(CblasRowMajor, CblasTrans, m, n, alpha, Aptr, ida, Eptr, incx, beta, Xptr, incy); #else - // ublas x += prod(trans(A),e) is terribly slow + // TODO: use BLAS size_t m = A.size1(), n = A.size2(); double * xj = x.data().begin(); @@ -295,7 +289,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector /* ************************************************************************* */ void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { - // ublas x += prod(trans(A),e) is terribly slow + #ifdef GT_USE_CBLAS transposeMultiplyAdd(1.0, A, e, x); #else @@ -312,7 +306,7 @@ void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { /* ************************************************************************* */ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { - // ublas x += prod(trans(A),e) is terribly slow + // TODO: use BLAS size_t m = A.size1(), n = A.size2(); for (size_t j = 0; j < n; j++) { @@ -336,8 +330,7 @@ Vector Vector_(const Matrix& A) /* ************************************************************************* */ Vector column_(const Matrix& A, size_t j) { -// if (j>=A.size2()) -// throw invalid_argument("Column index out of bounds!"); + return column(A,j); // real boost version } @@ -376,8 +369,7 @@ void save(const Matrix& A, const string &s, const string& filename) { /* ************************************************************************* */ Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2) { - // using ublas is slower: - // Matrix B = Matrix(ublas::project(A,ublas::range(i1,i2+1),ublas::range(j1,j2+1))); + size_t m=i2-i1, n=j2-j1; Matrix B(m,n); for (size_t i=i1,k=0;i qr(const Matrix& A) { /* ************************************************************************* */ inline void householder_update_manual(Matrix &A, size_t j, double beta, const Vector& vjm) { const size_t m = A.size1(), n = A.size2(); - // w = beta*transpose(A(j:m,:))*v(j:m) + Vector w(n); for( size_t c = 0; c < n; c++) { w(c) = 0.0; // dangerous as relies on row-major scheme const double *a = &A(j,c), * const v = &vjm(0); for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n ) - // w(c) += A(r,c) * vjm(r-j) + w(c) += (*a) * v[s]; w(c) *= beta; } @@ -506,7 +498,7 @@ inline void householder_update_manual(Matrix &A, size_t j, double beta, const Ve double wc = w(c); double *a = &A(j,c); const double * const v =&vjm(0); for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n ) - // A(r,c) -= vjm(r-j) * wjn(c-j); + (*a) -= v[s] * wc; } } @@ -517,55 +509,7 @@ void householder_update(Matrix &A, size_t j, double beta, const Vector& vjm) { // CBLAS version not working, using manual approach householder_update_manual(A,j,beta,vjm); -// // straight atlas version -// const size_t m = A.size1(), n = A.size2(); -// const size_t mj = m-j; -// -// // find pointers to the data -// const double * vptr = vjm.data().begin(); // mj long -// double * Aptr = A.data().begin() + n*j; // mj x n - note that this starts at row j -// -// // first step: get w = beta*trans(A(j:m,:))*vjm -// Vector w = zero(n); -// double * wptr = w.data().begin(); -// -// // DEBUG: create a duplicate version of the problem to solve simultaneously -// Matrix aA(A); Vector avjm(vjm); -// -// // execute w generation -// cblas_dgemv(CblasRowMajor, CblasTrans, mj, n, beta, Aptr, n, vptr, 1, 0.0, wptr, 1); -// -// // Execute w generation with alternate code -// Vector aw(n); -// for( size_t c = 0; c < n; c++) { -// aw(c) = 0.0; -// // dangerous as relies on row-major scheme -// const double *a = &aA(j,c), * const v = &avjm(0); -// for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n ) -// // w(c) += A(r,c) * vjm(r-j) -// aw(c) += (*a) * v[s]; -// aw(c) *= beta; -// } -// -// print(w, "CBLAS w "); -// print(aw, "Alternate w"); -// -// // second step: rank 1 update A(j:m,:) = v(j:m)*w' + A(j:m,:) -// cblas_dger(CblasRowMajor, mj, n, 1.0, vptr, 1, wptr, 1, Aptr, n); // not correct -// -// // Execute second step using alternate code -// // rank 1 update A(j:m,:) -= v(j:m)*w' -// for( size_t c = 0 ; c < n; c++) { -// double wc = aw(c); -// double *a = &aA(j,c); -// const double * const v =&avjm(0); -// for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n ) -// // A(r,c) -= vjm(r-j) * wjn(c-j); -// (*a) -= v[s] * wc; -// } -// -// // copy in alternate results, which should be correct -// A = aA; + #else householder_update_manual(A,j,beta,vjm); @@ -584,7 +528,7 @@ inline void updateAb_manual(Matrix& A, Vector& b, size_t j, const Vector& a, b(i) -= ai * d; double *Aij = A.data().begin() + i * n + j + 1; const double *rptr = r.data().begin() + j + 1; - // A(i,j+1:end) -= ai*r(j+1:end) + for (size_t j2 = j + 1; j2 < n; j2++, Aij++, rptr++) *Aij -= ai * (*rptr); } @@ -618,12 +562,10 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { for (size_t j=0; j 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; -//} -///* ************************************************************************* */ -//Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) { -// size_t n = U.size2(); -//#ifndef NDEBUG -// size_t m = U.size1(); -// if (m!=n) -// throw invalid_argument("backSubstituteUpper: U 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 -= U(j-1,i-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; -//} - -/* ************************************************************************* */ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) { size_t n = L.size2(); #ifndef NDEBUG diff --git a/base/Matrix.h b/base/Matrix.h index 209d9c023..6ebb86623 100644 --- a/base/Matrix.h +++ b/base/Matrix.h @@ -213,8 +213,7 @@ void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j); */ Vector row_(const Matrix& A, size_t j); -// left multiply with scalar -//inline Matrix operator*(double s, const Matrix& A) { return A*s;} + /** * solve AX=B via in-place Lu factorization and backsubstitution diff --git a/base/Vector.cpp b/base/Vector.cpp index 5f66bef11..835e9402a 100644 --- a/base/Vector.cpp +++ b/base/Vector.cpp @@ -65,7 +65,7 @@ namespace gtsam { } /* ************************************************************************* */ - // copy and paste from above, as two functions can not be easily merged + void odprintf(const char *format, ...) { char buf[4096], *p = buf; diff --git a/base/blockMatrices.h b/base/blockMatrices.h index 9b013d02e..f62c493e1 100644 --- a/base/blockMatrices.h +++ b/base/blockMatrices.h @@ -29,15 +29,15 @@ namespace gtsam { * reference to the range. The stored range stores a reference to the original * matrix. */ -template -class BlockColumn : public boost::numeric::ublas::vector_expression > { +template +class BlockColumn : public boost::numeric::ublas::vector_expression > { protected: - typedef boost::numeric::ublas::matrix_range Range; + typedef boost::numeric::ublas::matrix_range Range; typedef boost::numeric::ublas::matrix_column Base; Range range_; Base base_; public: - typedef BlockColumn Self; + typedef BlockColumn Self; typedef typename Base::matrix_type matrix_type; typedef typename Base::size_type size_type; typedef typename Base::difference_type difference_type; @@ -50,7 +50,7 @@ public: typedef typename Base::iterator iterator; typedef typename Base::const_iterator const_iterator; - BlockColumn(const boost::numeric::ublas::matrix_range& block, size_t column) : + BlockColumn(const boost::numeric::ublas::matrix_range& block, size_t column) : range_(block), base_(range_, column) {} BlockColumn(const BlockColumn& rhs) : range_(rhs.range_), base_(rhs.base_) {} @@ -83,14 +83,14 @@ public: * This class also has three parameters that can be changed after construction * that change the */ -template +template class VerticalBlockView { public: - typedef Matrix matrix_type; - typedef typename boost::numeric::ublas::matrix_range block_type; - typedef typename boost::numeric::ublas::matrix_range const_block_type; - typedef BlockColumn column_type; - typedef BlockColumn const_column_type; + typedef MATRIX matrix_type; + typedef typename boost::numeric::ublas::matrix_range block_type; + typedef typename boost::numeric::ublas::matrix_range const_block_type; + typedef BlockColumn column_type; + typedef BlockColumn const_column_type; protected: matrix_type& matrix_; @@ -104,20 +104,30 @@ public: VerticalBlockView(matrix_type& matrix); /** Construct from iterators over the sizes of each vertical block */ - template - VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim); + template + VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim); /** Construct from a vector of the sizes of each vertical block, resize the * matrix so that its height is matrixNewHeight and its width fits the given * block dimensions. */ - template - VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight); - + template + VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight); + + /** Row size + */ size_t size1() const { assertInvariants(); return rowEnd_ - rowStart_; } + + /** Column size + */ size_t size2() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + + + /** Block count + */ size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + block_type operator()(size_t block) { return range(block, block+1); } @@ -183,8 +193,8 @@ public: /** Copy the block structure and resize the underlying matrix, but do not * copy the matrix data. */ - template - void copyStructureFrom(const VerticalBlockView& rhs); + template + void copyStructureFrom(const VerticalBlockView& rhs); /** Copy the block struture and matrix data, resizing the underlying matrix * in the process. This can deal with assigning between different types of @@ -192,13 +202,13 @@ public: * To avoid creating a temporary matrix this assumes no aliasing, i.e. that * no part of the underlying matrices refer to the same memory! */ - template - VerticalBlockView& assignNoalias(const VerticalBlockView& rhs); + template + VerticalBlockView& assignNoalias(const VerticalBlockView& rhs); /** Swap the contents of the underlying matrix and the block information with * another VerticalBlockView. */ - void swap(VerticalBlockView& other); + void swap(VerticalBlockView& other); protected: void assertInvariants() const { @@ -215,42 +225,42 @@ protected: assert(variableColOffsets_[block] < matrix_.size2() && variableColOffsets_[block+1] <= matrix_.size2()); } - template - void fillOffsets(Iterator firstBlockDim, Iterator lastBlockDim) { + template + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); variableColOffsets_[0] = 0; size_t j=0; - for(Iterator dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; ++ j; } } - template - friend class VerticalBlockView; + template + friend class VerticalBlockView; }; /* ************************************************************************* */ -template -VerticalBlockView::VerticalBlockView(matrix_type& matrix) : +template +VerticalBlockView::VerticalBlockView(matrix_type& matrix) : matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { fillOffsets((size_t*)0, (size_t*)0); assertInvariants(); } /* ************************************************************************* */ -template -template -VerticalBlockView::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim) : +template +template +VerticalBlockView::VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) : matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim); assertInvariants(); } /* ************************************************************************* */ -template -template -VerticalBlockView::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight) : +template +template +VerticalBlockView::VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight) : matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim); matrix_.resize(matrixNewHeight, variableColOffsets_.back(), false); @@ -258,9 +268,9 @@ matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) { } /* ************************************************************************* */ -template -template -void VerticalBlockView::copyStructureFrom(const VerticalBlockView& rhs) { +template +template +void VerticalBlockView::copyStructureFrom(const VerticalBlockView& rhs) { matrix_.resize(rhs.rowEnd() - rhs.rowStart(), rhs.range(0, rhs.nBlocks()).size2(), false); if(rhs.blockStart_ == 0) variableColOffsets_ = rhs.variableColOffsets_; @@ -281,17 +291,17 @@ void VerticalBlockView::copyStructureFrom(const VerticalBlockView -template -VerticalBlockView& VerticalBlockView::assignNoalias(const VerticalBlockView& rhs) { +template +template +VerticalBlockView& VerticalBlockView::assignNoalias(const VerticalBlockView& rhs) { copyStructureFrom(rhs); boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks()); return *this; } /* ************************************************************************* */ -template -void VerticalBlockView::swap(VerticalBlockView& other) { +template +void VerticalBlockView::swap(VerticalBlockView& other) { matrix_.swap(other.matrix_); variableColOffsets_.swap(other.variableColOffsets_); std::swap(rowStart_, other.rowStart_); diff --git a/base/numericalDerivative.h b/base/numericalDerivative.h index 2fd8a161f..e699f7cb8 100644 --- a/base/numericalDerivative.h +++ b/base/numericalDerivative.h @@ -26,7 +26,7 @@ #include #include -//#define LINEARIZE_AT_IDENTITY + namespace gtsam { diff --git a/base/svdcmp.cpp b/base/svdcmp.cpp index d174d1919..fd793ad0f 100644 --- a/base/svdcmp.cpp +++ b/base/svdcmp.cpp @@ -33,17 +33,6 @@ static int iminarg1, iminarg2; #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\ (iminarg1) : (iminarg2)) -/* ************************************************************************* */ -/* - double pythag(double a, double b) - { - double absa = 0.0, absb = 0.0; - absa=fabs(a); - absb=fabs(b); - if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa)); - else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb))); - } - */ /* ************************************************************************* */ double pythag(double a, double b) {