12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

some cleanup / cleanup flags, capitalization

release/4.3a0
Rahul Sawhney 2010-10-21 16:32:11 +00:00
parent e01096d257
commit 200ac4e862
14 changed files with 138 additions and 266 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
base/BTree.h
View File

@ -26,12 +26,12 @@ namespace gtsam {
/** /**
* @brief Binary tree * @brief Binary tree
*/ */
template<class Key, class Value> template<class KEY, class VALUE>
class BTree { class BTree {
public: public:
typedef std::pair<Key, Value> value_type; typedef std::pair<KEY, VALUE> value_type;
private: private:
@ -63,8 +63,8 @@ namespace gtsam {
keyValue_(keyValue), left(l), right(r) { keyValue_(keyValue), left(l), right(r) {
} }
inline const Key& key() const { return keyValue_.first;} inline const KEY& key() const { return keyValue_.first;}
inline const Value& value() const { return keyValue_.second;} inline const VALUE& value() const { return keyValue_.second;}
}; // Node }; // Node
@ -74,8 +74,8 @@ namespace gtsam {
sharedNode root_; sharedNode root_;
inline const value_type& keyValue() const { return root_->keyValue_;} inline const value_type& keyValue() const { return root_->keyValue_;}
inline const Key& key() const { return root_->key(); } inline const KEY& key() const { return root_->key(); }
inline const Value& value() const { return root_->value(); } inline const VALUE& value() const { return root_->value(); }
inline const BTree& left() const { return root_->left; } inline const BTree& left() const { return root_->left; }
inline const BTree& right() const { return root_->right; } inline const BTree& right() const { return root_->right; }
@ -133,7 +133,7 @@ namespace gtsam {
/** add a key-value pair */ /** add a key-value pair */
BTree add(const value_type& xd) const { BTree add(const value_type& xd) const {
if (empty()) return BTree(xd); if (empty()) return BTree(xd);
const Key& x = xd.first; const KEY& x = xd.first;
if (x == key()) if (x == key())
return BTree(left(), xd, right()); return BTree(left(), xd, right());
else if (x < key()) else if (x < key())
@ -143,12 +143,12 @@ namespace gtsam {
} }
/** add a key-value pair */ /** 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)); return add(make_pair(x, d));
} }
/** member predicate */ /** member predicate */
bool mem(const Key& x) const { bool mem(const KEY& x) const {
if (!root_) return false; if (!root_) return false;
if (x == key()) return true; if (x == key()) return true;
if (x < key()) if (x < key())
@ -202,7 +202,7 @@ namespace gtsam {
} }
/** remove a key-value pair */ /** remove a key-value pair */
BTree remove(const Key& x) const { BTree remove(const KEY& x) const {
if (!root_) return BTree(); if (!root_) return BTree();
if (x == key()) if (x == key())
return merge(left(), right()); return merge(left(), right());
@ -227,22 +227,22 @@ namespace gtsam {
* find a value given a key, throws exception when not found * find a value given a key, throws exception when not found
* Optimized non-recursive version as [find] is crucial for speed * 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(); const Node* node = root_.get();
while (node) { while (node) {
const Key& key = node->key(); const KEY& key = node->key();
if (k < key) node = node->left.root_.get(); if (k < key) node = node->left.root_.get();
else if (key < k) node = node->right.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"); throw std::invalid_argument("BTree::find: key not found");
} }
/** print in-order */ /** print in-order */
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
if (empty()) return; if (empty()) return;
Key k = key(); KEY k = key();
std::stringstream ss; std::stringstream ss;
ss << height(); ss << height();
k.print(s + ss.str() + " "); k.print(s + ss.str() + " ");
@ -251,7 +251,7 @@ namespace gtsam {
} }
/** iterate over tree */ /** iterate over tree */
void iter(boost::function<void(const Key&, const Value&)> f) const { void iter(boost::function<void(const KEY&, const VALUE&)> f) const {
if (!root_) return; if (!root_) return;
left().iter(f); left().iter(f);
f(key(), value()); f(key(), value());
@ -259,11 +259,11 @@ namespace gtsam {
} }
/** map key-values in tree over function f that computes a new value */ /** map key-values in tree over function f that computes a new value */
template<class To> template<class TO>
BTree<Key, To> map(boost::function<To(const Key&, const Value&)> f) const { BTree<KEY, TO> map(boost::function<TO(const KEY&, const VALUE&)> f) const {
if (empty()) return BTree<Key, To> (); if (empty()) return BTree<KEY, TO> ();
std::pair<Key, To> xd(key(), f(key(), value())); std::pair<KEY, TO> xd(key(), f(key(), value()));
return BTree<Key, To> (left().map(f), xd, right().map(f)); return BTree<KEY, TO> (left().map(f), xd, right().map(f));
} }
/** /**
@ -272,12 +272,12 @@ namespace gtsam {
* and [d1 ... dN] are the associated data. * and [d1 ... dN] are the associated data.
* The associated values are passed to [f] in reverse sort order * The associated values are passed to [f] in reverse sort order
*/ */
template<class Acc> template<class ACC>
Acc fold(boost::function<Acc(const Key&, const Value&, const Acc&)> f, ACC fold(boost::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
const Acc& a) const { const ACC& a) const {
if (!root_) return a; if (!root_) return a;
Acc ar = right().fold(f, a); // fold over right subtree ACC ar = right().fold(f, a); // fold over right subtree
Acc am = f(key(), value(), ar); // apply f with current value ACC am = f(key(), value(), ar); // apply f with current value
return left().fold(f, am); // fold over left subtree return left().fold(f, am); // fold over left subtree
} }
@ -330,7 +330,7 @@ namespace gtsam {
// traits for playing nice with STL // traits for playing nice with STL
typedef ptrdiff_t difference_type; // correct ? typedef ptrdiff_t difference_type; // correct ?
typedef std::forward_iterator_tag iterator_category; typedef std::forward_iterator_tag iterator_category;
typedef std::pair<Key, Value> value_type; typedef std::pair<KEY, VALUE> value_type;
typedef const value_type* pointer; typedef const value_type* pointer;
typedef const value_type& reference; typedef const value_type& reference;

44
base/DSF.h
View File

@ -31,15 +31,15 @@
namespace gtsam { namespace gtsam {
template <class Key> template <class KEY>
class DSF : protected BTree<Key, Key> { class DSF : protected BTree<KEY, KEY> {
public: public:
typedef Key Label; // label can be different from key, but for now they are same typedef KEY Label; // label can be different from key, but for now they are same
typedef DSF<Key> Self; typedef DSF<KEY> Self;
typedef std::set<Key> Set; typedef std::set<KEY> Set;
typedef BTree<Key, Label> Tree; typedef BTree<KEY, Label> Tree;
typedef std::pair<Key, Label> KeyLabel; typedef std::pair<KEY, Label> KeyLabel;
// constructor // constructor
DSF() : Tree() { } DSF() : Tree() { }
@ -48,29 +48,29 @@ namespace gtsam {
DSF(const Tree& tree) : Tree(tree) {} DSF(const Tree& tree) : Tree(tree) {}
// constructor with a list of unconnected keys // constructor with a list of unconnected keys
DSF(const std::list<Key>& keys) : Tree() { BOOST_FOREACH(const Key& key, keys) *this = this->add(key, key); } DSF(const std::list<KEY>& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); }
// create a new singleton, does nothing if already exists // 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 // find the label of the set in which {key} lives
Label findSet(const Key& key) const { Label findSet(const KEY& key) const {
Key parent = this->find(key); KEY parent = this->find(key);
return parent == key ? key : findSet(parent); } 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 // 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 // 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 // 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 // create a new singleton with a list of fully connected keys
Self makeList(const std::list<Key>& keys) const { Self makeList(const std::list<KEY>& keys) const {
Self t = *this; Self t = *this;
BOOST_FOREACH(const Key& key, keys) BOOST_FOREACH(const KEY& key, keys)
t = t.makePair(key, keys.front()); t = t.makePair(key, keys.front());
return t; return t;
} }
@ -84,7 +84,7 @@ namespace gtsam {
} }
// maps f over all keys, must be invertible // maps f over all keys, must be invertible
DSF map(boost::function<Key(const Key&)> func) const { DSF map(boost::function<KEY(const KEY&)> func) const {
DSF t; DSF t;
BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
t = t.add(func(pair.first), func(pair.second)); t = t.add(func(pair.first), func(pair.second));
@ -111,9 +111,9 @@ namespace gtsam {
} }
// return a partition of the given elements {keys} // return a partition of the given elements {keys}
std::map<Label, Set> partition(const std::list<Key>& keys) const { std::map<Label, Set> partition(const std::list<KEY>& keys) const {
std::map<Label, Set> partitions; std::map<Label, Set> partitions;
BOOST_FOREACH(const Key& key, keys) BOOST_FOREACH(const KEY& key, keys)
partitions[findSet(key)].insert(key); partitions[findSet(key)].insert(key);
return partitions; return partitions;
} }
@ -147,12 +147,12 @@ namespace gtsam {
* same as findSet except with path compression: After we have traversed the path to * 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 * the root, each parent pointer is made to directly point to it
*/ */
Key findSet_(const Key& key) { KEY findSet_(const KEY& key) {
Key parent = this->find(key); KEY parent = this->find(key);
if (parent == key) if (parent == key)
return parent; return parent;
else { else {
Key label = findSet_(parent); KEY label = findSet_(parent);
*this = this->add(key, label); *this = this->add(key, label);
return label; return label;
} }

17
base/DenseQRUtil.cpp
View File

@ -133,15 +133,13 @@ namespace gtsam {
double wscale = 0; double wscale = 0;
double wssq = 0; double wssq = 0;
// cholmod_common cc;
// cholmod_l_start(&cc);
// todo: do something with the rank // todo: do something with the rank
long rank = DenseQR(m, n, npiv, tol, ntol, fchunk, long rank = DenseQR(m, n, npiv, tol, ntol, fchunk,
a, Stair, Rdead, Tau, W, &wscale, &wssq); a, Stair, Rdead, Tau, W, &wscale, &wssq);
toc("householder_denseqr: denseqr_front"); toc("householder_denseqr: denseqr_front");
//#ifndef NDEBUG
for(long j=0; j<npiv; ++j) for(long j=0; j<npiv; ++j)
if(Rdead[j]) { if(Rdead[j]) {
cout << "In householder_denseqr, aborting because some columns were found to be\n" cout << "In householder_denseqr, aborting because some columns were found to be\n"
@ -156,7 +154,6 @@ namespace gtsam {
cout << endl; cout << endl;
exit(1); exit(1);
} }
//#endif
tic("householder_denseqr: col->row"); tic("householder_denseqr: col->row");
long k0 = 0; long k0 = 0;
@ -170,14 +167,11 @@ namespace gtsam {
A(i,j) = a[k]; A(i,j) = a[k];
} }
// ublas::matrix_range<ublas::matrix<double,ublas::column_major> > Acolsub(
// ublas::project(Acolwise, ublas::range(0, min(m,n)), ublas::range(0,n)));
// ublas::matrix_range<Matrix> Asub(ublas::project(A, ublas::range(0, min(m,n)), ublas::range(0,n)));
// ublas::noalias(Asub) = ublas::triangular_adaptor<typeof(Acolsub), ublas::upper>(Acolsub);
toc("householder_denseqr: col->row"); toc("householder_denseqr: col->row");
// cholmod_l_finish(&cc);
if(allocedStair) delete[] Stair; if(allocedStair) delete[] Stair;
@ -203,15 +197,12 @@ namespace gtsam {
double wscale = 0; double wscale = 0;
double wssq = 0; double wssq = 0;
// cholmod_common cc;
// cholmod_l_start(&cc);
// todo: do something with the rank // todo: do something with the rank
long rank = DenseQR(m, n, npiv, tol, ntol, fchunk, long rank = DenseQR(m, n, npiv, tol, ntol, fchunk,
A.data().begin(), Stair, Rdead, Tau, W, &wscale, &wssq); A.data().begin(), Stair, Rdead, Tau, W, &wscale, &wssq);
toc("householder_denseqr: denseqr_front"); toc("householder_denseqr: denseqr_front");
//#ifndef NDEBUG
for(long j=0; j<npiv; ++j) for(long j=0; j<npiv; ++j)
if(Rdead[j]) { if(Rdead[j]) {
cout << "In householder_denseqr, aborting because some columns were found to be\n" cout << "In householder_denseqr, aborting because some columns were found to be\n"
@ -226,9 +217,7 @@ namespace gtsam {
cout << endl; cout << endl;
exit(1); exit(1);
} }
//#endif
// cholmod_l_finish(&cc);
toc("householder_denseqr"); toc("householder_denseqr");

1
base/DenseQRUtil.h
View File

@ -32,6 +32,7 @@ namespace gtsam {
/** Householder tranformation, zeros below diagonal */ /** Householder tranformation, zeros below diagonal */
void householder_denseqr(Matrix &A, long* Stair = NULL); void householder_denseqr(Matrix &A, long* Stair = NULL);
/** Householder tranformation in column mafor form */
void householder_denseqr_colmajor(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& A, long *Stair); void householder_denseqr_colmajor(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& A, long *Stair);
} }
#endif #endif

4
base/FastMap.h
View File

@ -41,8 +41,8 @@ public:
FastMap() {} FastMap() {}
/** Constructor from a range, passes through to base class */ /** Constructor from a range, passes through to base class */
template<typename InputIterator> template<typename INPUTITERATOR>
FastMap(InputIterator first, InputIterator last) : Base(first, last) {} FastMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
/** Copy constructor from another FastMap */ /** Copy constructor from another FastMap */
FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {} FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}

4
base/FastSet.h
View File

@ -38,8 +38,8 @@ public:
typedef std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > Base; typedef std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > Base;
/** Constructor from a range, passes through to base class */ /** Constructor from a range, passes through to base class */
template<typename InputIterator> template<typename INPUTITERATOR>
FastSet(InputIterator first, InputIterator last) : Base(first, last) {} FastSet(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
/** Copy constructor from another FastMap */ /** Copy constructor from another FastMap */
FastSet(const FastSet<VALUE>& x) : Base(x) {} FastSet(const FastSet<VALUE>& x) : Base(x) {}

1
base/LieVector.h
View File

@ -77,7 +77,6 @@ namespace gtsam {
* Default Implementation calls global binary function * Default Implementation calls global binary function
*/ */
inline Vector logmap(const LieVector& lp) const { inline Vector logmap(const LieVector& lp) const {
// return Logmap(between(lp)); // works
return lp.vector() - vector(); return lp.vector() - vector();
} }

20
base/Matrix-inl.h
View File

@ -33,11 +33,11 @@ using namespace std;
* @param b an RHS vector * @param b an RHS vector
* @return the solution x of U*x=b * @return the solution x of U*x=b
*/ */
template<class MatrixAE, class VectorAE> template<class MATRIXAE, class VECTORAE>
Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MatrixAE>& _U, Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MATRIXAE>& _U,
const boost::numeric::ublas::vector_expression<VectorAE>& _b, bool unit=false) { const boost::numeric::ublas::vector_expression<VECTORAE>& _b, bool unit=false) {
const MatrixAE& U((const MatrixAE&)_U); const MATRIXAE& U((const MATRIXAE&)_U);
const VectorAE& b((const VectorAE&)_b); const VECTORAE& b((const VECTORAE&)_b);
size_t n = U.size2(); size_t n = U.size2();
#ifndef NDEBUG #ifndef NDEBUG
size_t m = U.size1(); size_t m = U.size1();
@ -74,11 +74,11 @@ Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<Matrix
* @return the solution x of x'*U=b' * @return the solution x of x'*U=b'
* TODO: use boost * TODO: use boost
*/ */
template<class VectorAE, class MatrixAE> template<class VECTORAE, class MATRIXAE>
Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VectorAE>& _b, Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VECTORAE>& _b,
const boost::numeric::ublas::matrix_expression<MatrixAE>& _U, bool unit=false) { const boost::numeric::ublas::matrix_expression<MATRIXAE>& _U, bool unit=false) {
const VectorAE& b((const VectorAE&)_b); const VECTORAE& b((const VECTORAE&)_b);
const MatrixAE& U((const MatrixAE&)_U); const MATRIXAE& U((const MATRIXAE&)_U);
size_t n = U.size2(); size_t n = U.size2();
#ifndef NDEBUG #ifndef NDEBUG
size_t m = U.size1(); size_t m = U.size1();

145
base/Matrix.cpp
View File

@ -144,11 +144,7 @@ bool equal_with_abs_tol(const Matrix& A, const Matrix& B, double tol) {
equal = false; equal = false;
} }
// if(!equal) {
// cout << "not equal:" << endl;
// print(A,"expected = ");
// print(B,"actual = ");
// }
return equal; 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) { void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) {
#if defined GT_USE_CBLAS #if defined GT_USE_CBLAS
// uncomment and run tests to verify blas is enabled
// throw runtime_error("You are in multiplyAdd!");
// get sizes // get sizes
const size_t m = A.size1(), n = A.size2(); 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); cblas_dgemv(CblasRowMajor, CblasNoTrans, m, n, alpha, Aptr, ida, Xptr, incx, beta, Eptr, incy);
#else #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(); size_t m = A.size1(), n = A.size2();
double * ei = e.data().begin(); double * ei = e.data().begin();
const double * aij = A.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) { void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) {
// ublas e += prod(A,x) is terribly slow
#ifdef GT_USE_CBLAS #ifdef GT_USE_CBLAS
multiplyAdd(1.0, A, x, e); multiplyAdd(1.0, A, x, e);
#else #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); cblas_dgemv(CblasRowMajor, CblasTrans, m, n, alpha, Aptr, ida, Eptr, incx, beta, Xptr, incy);
#else #else
// ublas x += prod(trans(A),e) is terribly slow
// TODO: use BLAS // TODO: use BLAS
size_t m = A.size1(), n = A.size2(); size_t m = A.size1(), n = A.size2();
double * xj = x.data().begin(); 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) { void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) {
// ublas x += prod(trans(A),e) is terribly slow
#ifdef GT_USE_CBLAS #ifdef GT_USE_CBLAS
transposeMultiplyAdd(1.0, A, e, x); transposeMultiplyAdd(1.0, A, e, x);
#else #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) { void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) {
// ublas x += prod(trans(A),e) is terribly slow
// TODO: use BLAS // TODO: use BLAS
size_t m = A.size1(), n = A.size2(); size_t m = A.size1(), n = A.size2();
for (size_t j = 0; j < n; j++) { 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) { 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 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) { 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; size_t m=i2-i1, n=j2-j1;
Matrix B(m,n); Matrix B(m,n);
for (size_t i=i1,k=0;i<i2;i++,k++) for (size_t i=i1,k=0;i<i2;i++,k++)
@ -489,14 +481,14 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
/* ************************************************************************* */ /* ************************************************************************* */
inline void householder_update_manual(Matrix &A, size_t j, double beta, const Vector& vjm) { inline void householder_update_manual(Matrix &A, size_t j, double beta, const Vector& vjm) {
const size_t m = A.size1(), n = A.size2(); const size_t m = A.size1(), n = A.size2();
// w = beta*transpose(A(j:m,:))*v(j:m)
Vector w(n); Vector w(n);
for( size_t c = 0; c < n; c++) { for( size_t c = 0; c < n; c++) {
w(c) = 0.0; w(c) = 0.0;
// dangerous as relies on row-major scheme // dangerous as relies on row-major scheme
const double *a = &A(j,c), * const v = &vjm(0); const double *a = &A(j,c), * const v = &vjm(0);
for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n ) 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) += (*a) * v[s];
w(c) *= beta; 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 wc = w(c);
double *a = &A(j,c); const double * const v =&vjm(0); double *a = &A(j,c); const double * const v =&vjm(0);
for( size_t r=j, s=0 ; r < m ; r++, s++, a+=n ) 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; (*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 // CBLAS version not working, using manual approach
householder_update_manual(A,j,beta,vjm); 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 #else
householder_update_manual(A,j,beta,vjm); 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; b(i) -= ai * d;
double *Aij = A.data().begin() + i * n + j + 1; double *Aij = A.data().begin() + i * n + j + 1;
const double *rptr = r.data().begin() + 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++) for (size_t j2 = j + 1; j2 < n; j2++, Aij++, rptr++)
*Aij -= ai * (*rptr); *Aij -= ai * (*rptr);
} }
@ -618,12 +562,10 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
for (size_t j=0; j<n; ++j) { for (size_t j=0; j<n; ++j) {
// extract the first column of A // extract the first column of A
Vector a(column_(A, j)); // ublas::matrix_column is slower ! Vector a(column_(A, j)); // ublas::matrix_column is slower !
//print(a,"a");
// Calculate weighted pseudo-inverse and corresponding precision // Calculate weighted pseudo-inverse and corresponding precision
double precision = weightedPseudoinverse(a, weights, pseudo); double precision = weightedPseudoinverse(a, weights, pseudo);
// cout << precision << endl;
// print(pseudo,"pseudo");
// if precision is zero, no information on this column // if precision is zero, no information on this column
if (precision < 1e-8) continue; if (precision < 1e-8) continue;
@ -735,64 +677,7 @@ void householder(Matrix &A) {
#endif #endif
///* ************************************************************************* */ ///* ************************************************************************* */
//Vector backSubstituteUpper(const Matrix& U, const Vector& b, 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 = 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<double>::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<double>::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) { Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
size_t n = L.size2(); size_t n = L.size2();
#ifndef NDEBUG #ifndef NDEBUG

3
base/Matrix.h
View File

@ -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); 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 * solve AX=B via in-place Lu factorization and backsubstitution

2
base/Vector.cpp
View File

@ -65,7 +65,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// copy and paste from above, as two functions can not be easily merged
void odprintf(const char *format, ...) void odprintf(const char *format, ...)
{ {
char buf[4096], *p = buf; char buf[4096], *p = buf;

92
base/blockMatrices.h
View File

@ -29,15 +29,15 @@ namespace gtsam {
* reference to the range. The stored range stores a reference to the original * reference to the range. The stored range stores a reference to the original
* matrix. * matrix.
*/ */
template<class Matrix> template<class MATRIX>
class BlockColumn : public boost::numeric::ublas::vector_expression<BlockColumn<Matrix> > { class BlockColumn : public boost::numeric::ublas::vector_expression<BlockColumn<MATRIX> > {
protected: protected:
typedef boost::numeric::ublas::matrix_range<Matrix> Range; typedef boost::numeric::ublas::matrix_range<MATRIX> Range;
typedef boost::numeric::ublas::matrix_column<Range> Base; typedef boost::numeric::ublas::matrix_column<Range> Base;
Range range_; Range range_;
Base base_; Base base_;
public: public:
typedef BlockColumn<Matrix> Self; typedef BlockColumn<MATRIX> Self;
typedef typename Base::matrix_type matrix_type; typedef typename Base::matrix_type matrix_type;
typedef typename Base::size_type size_type; typedef typename Base::size_type size_type;
typedef typename Base::difference_type difference_type; typedef typename Base::difference_type difference_type;
@ -50,7 +50,7 @@ public:
typedef typename Base::iterator iterator; typedef typename Base::iterator iterator;
typedef typename Base::const_iterator const_iterator; typedef typename Base::const_iterator const_iterator;
BlockColumn(const boost::numeric::ublas::matrix_range<Matrix>& block, size_t column) : BlockColumn(const boost::numeric::ublas::matrix_range<MATRIX>& block, size_t column) :
range_(block), base_(range_, column) {} range_(block), base_(range_, column) {}
BlockColumn(const BlockColumn& rhs) : BlockColumn(const BlockColumn& rhs) :
range_(rhs.range_), base_(rhs.base_) {} range_(rhs.range_), base_(rhs.base_) {}
@ -83,14 +83,14 @@ public:
* This class also has three parameters that can be changed after construction * This class also has three parameters that can be changed after construction
* that change the * that change the
*/ */
template<class Matrix> template<class MATRIX>
class VerticalBlockView { class VerticalBlockView {
public: public:
typedef Matrix matrix_type; typedef MATRIX matrix_type;
typedef typename boost::numeric::ublas::matrix_range<Matrix> block_type; typedef typename boost::numeric::ublas::matrix_range<MATRIX> block_type;
typedef typename boost::numeric::ublas::matrix_range<const Matrix> const_block_type; typedef typename boost::numeric::ublas::matrix_range<const MATRIX> const_block_type;
typedef BlockColumn<Matrix> column_type; typedef BlockColumn<MATRIX> column_type;
typedef BlockColumn<const Matrix> const_column_type; typedef BlockColumn<const MATRIX> const_column_type;
protected: protected:
matrix_type& matrix_; matrix_type& matrix_;
@ -104,20 +104,30 @@ public:
VerticalBlockView(matrix_type& matrix); VerticalBlockView(matrix_type& matrix);
/** Construct from iterators over the sizes of each vertical block */ /** Construct from iterators over the sizes of each vertical block */
template<typename Iterator> template<typename ITERATOR>
VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim); VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim);
/** Construct from a vector of the sizes of each vertical block, resize the /** 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 * matrix so that its height is matrixNewHeight and its width fits the given
* block dimensions. * block dimensions.
*/ */
template<typename Iterator> template<typename ITERATOR>
VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight); VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight);
/** Row size
*/
size_t size1() const { assertInvariants(); return rowEnd_ - rowStart_; } size_t size1() const { assertInvariants(); return rowEnd_ - rowStart_; }
/** Column size
*/
size_t size2() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } size_t size2() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
/** Block count
*/
size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
block_type operator()(size_t block) { block_type operator()(size_t block) {
return range(block, block+1); return range(block, block+1);
} }
@ -183,8 +193,8 @@ public:
/** Copy the block structure and resize the underlying matrix, but do not /** Copy the block structure and resize the underlying matrix, but do not
* copy the matrix data. * copy the matrix data.
*/ */
template<class RhsMatrix> template<class RHSMATRIX>
void copyStructureFrom(const VerticalBlockView<RhsMatrix>& rhs); void copyStructureFrom(const VerticalBlockView<RHSMATRIX>& rhs);
/** Copy the block struture and matrix data, resizing the underlying matrix /** Copy the block struture and matrix data, resizing the underlying matrix
* in the process. This can deal with assigning between different types of * 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 * To avoid creating a temporary matrix this assumes no aliasing, i.e. that
* no part of the underlying matrices refer to the same memory! * no part of the underlying matrices refer to the same memory!
*/ */
template<class RhsMatrix> template<class RHSMATRIX>
VerticalBlockView<Matrix>& assignNoalias(const VerticalBlockView<RhsMatrix>& rhs); VerticalBlockView<MATRIX>& assignNoalias(const VerticalBlockView<RHSMATRIX>& rhs);
/** Swap the contents of the underlying matrix and the block information with /** Swap the contents of the underlying matrix and the block information with
* another VerticalBlockView. * another VerticalBlockView.
*/ */
void swap(VerticalBlockView<Matrix>& other); void swap(VerticalBlockView<MATRIX>& other);
protected: protected:
void assertInvariants() const { void assertInvariants() const {
@ -215,42 +225,42 @@ protected:
assert(variableColOffsets_[block] < matrix_.size2() && variableColOffsets_[block+1] <= matrix_.size2()); assert(variableColOffsets_[block] < matrix_.size2() && variableColOffsets_[block+1] <= matrix_.size2());
} }
template<typename Iterator> template<typename ITERATOR>
void fillOffsets(Iterator firstBlockDim, Iterator lastBlockDim) { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
size_t j=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; variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j; ++ j;
} }
} }
template<class RhsMatrix> template<class RHSMATRIX>
friend class VerticalBlockView<Matrix>; friend class VerticalBlockView<MATRIX>;
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<class Matrix> template<class MATRIX>
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix) : VerticalBlockView<MATRIX>::VerticalBlockView(matrix_type& matrix) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
fillOffsets((size_t*)0, (size_t*)0); fillOffsets((size_t*)0, (size_t*)0);
assertInvariants(); assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Matrix> template<class MATRIX>
template<typename Iterator> template<typename ITERATOR>
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim) : VerticalBlockView<MATRIX>::VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) { matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
fillOffsets(firstBlockDim, lastBlockDim); fillOffsets(firstBlockDim, lastBlockDim);
assertInvariants(); assertInvariants();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Matrix> template<class MATRIX>
template<typename Iterator> template<typename ITERATOR>
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight) : VerticalBlockView<MATRIX>::VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim, size_t matrixNewHeight) :
matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) { matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) {
fillOffsets(firstBlockDim, lastBlockDim); fillOffsets(firstBlockDim, lastBlockDim);
matrix_.resize(matrixNewHeight, variableColOffsets_.back(), false); matrix_.resize(matrixNewHeight, variableColOffsets_.back(), false);
@ -258,9 +268,9 @@ matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Matrix> template<class MATRIX>
template<class RhsMatrix> template<class RHSMATRIX>
void VerticalBlockView<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMatrix>& rhs) { void VerticalBlockView<MATRIX>::copyStructureFrom(const VerticalBlockView<RHSMATRIX>& rhs) {
matrix_.resize(rhs.rowEnd() - rhs.rowStart(), rhs.range(0, rhs.nBlocks()).size2(), false); matrix_.resize(rhs.rowEnd() - rhs.rowStart(), rhs.range(0, rhs.nBlocks()).size2(), false);
if(rhs.blockStart_ == 0) if(rhs.blockStart_ == 0)
variableColOffsets_ = rhs.variableColOffsets_; variableColOffsets_ = rhs.variableColOffsets_;
@ -281,17 +291,17 @@ void VerticalBlockView<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMat
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Matrix> template<class MATRIX>
template<class RhsMatrix> template<class RHSMATRIX>
VerticalBlockView<Matrix>& VerticalBlockView<Matrix>::assignNoalias(const VerticalBlockView<RhsMatrix>& rhs) { VerticalBlockView<MATRIX>& VerticalBlockView<MATRIX>::assignNoalias(const VerticalBlockView<RHSMATRIX>& rhs) {
copyStructureFrom(rhs); copyStructureFrom(rhs);
boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks()); boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks());
return *this; return *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Matrix> template<class MATRIX>
void VerticalBlockView<Matrix>::swap(VerticalBlockView<Matrix>& other) { void VerticalBlockView<MATRIX>::swap(VerticalBlockView<MATRIX>& other) {
matrix_.swap(other.matrix_); matrix_.swap(other.matrix_);
variableColOffsets_.swap(other.variableColOffsets_); variableColOffsets_.swap(other.variableColOffsets_);
std::swap(rowStart_, other.rowStart_); std::swap(rowStart_, other.rowStart_);

2
base/numericalDerivative.h
View File

@ -26,7 +26,7 @@
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
//#define LINEARIZE_AT_IDENTITY
namespace gtsam { namespace gtsam {

11
base/svdcmp.cpp
View File

@ -33,17 +33,6 @@ static int iminarg1, iminarg2;
#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\ #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
(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) { double pythag(double a, double b) {