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
*/
template<class Key, class Value>
template<class KEY, class VALUE>
class BTree {
public:
typedef std::pair<Key, Value> value_type;
typedef std::pair<KEY, VALUE> 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<void(const Key&, const Value&)> f) const {
void iter(boost::function<void(const KEY&, const VALUE&)> 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<class To>
BTree<Key, To> map(boost::function<To(const Key&, const Value&)> f) const {
if (empty()) return BTree<Key, To> ();
std::pair<Key, To> xd(key(), f(key(), value()));
return BTree<Key, To> (left().map(f), xd, right().map(f));
template<class TO>
BTree<KEY, TO> map(boost::function<TO(const KEY&, const VALUE&)> f) const {
if (empty()) return BTree<KEY, TO> ();
std::pair<KEY, TO> xd(key(), f(key(), value()));
return BTree<KEY, TO> (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<class Acc>
Acc fold(boost::function<Acc(const Key&, const Value&, const Acc&)> f,
const Acc& a) const {
template<class ACC>
ACC fold(boost::function<ACC(const KEY&, const VALUE&, const ACC&)> 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<Key, Value> value_type;
typedef std::pair<KEY, VALUE> value_type;
typedef const value_type* pointer;
typedef const value_type& reference;

44
base/DSF.h
View File

@ -31,15 +31,15 @@
namespace gtsam {
template <class Key>
class DSF : protected BTree<Key, Key> {
template <class KEY>
class DSF : protected BTree<KEY, KEY> {
public:
typedef Key Label; // label can be different from key, but for now they are same
typedef DSF<Key> Self;
typedef std::set<Key> Set;
typedef BTree<Key, Label> Tree;
typedef std::pair<Key, Label> KeyLabel;
typedef KEY Label; // label can be different from key, but for now they are same
typedef DSF<KEY> Self;
typedef std::set<KEY> Set;
typedef BTree<KEY, Label> Tree;
typedef std::pair<KEY, Label> 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<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
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<Key>& keys) const {
Self makeList(const std::list<KEY>& 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<Key(const Key&)> func) const {
DSF map(boost::function<KEY(const KEY&)> 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<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;
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;
}

17
base/DenseQRUtil.cpp
View File

@ -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; j<npiv; ++j)
if(Rdead[j]) {
cout << "In householder_denseqr, aborting because some columns were found to be\n"
@ -156,7 +154,6 @@ namespace gtsam {
cout << endl;
exit(1);
}
//#endif
tic("householder_denseqr: col->row");
long k0 = 0;
@ -170,14 +167,11 @@ namespace gtsam {
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");
// 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<npiv; ++j)
if(Rdead[j]) {
cout << "In householder_denseqr, aborting because some columns were found to be\n"
@ -226,9 +217,7 @@ namespace gtsam {
cout << endl;
exit(1);
}
//#endif
// cholmod_l_finish(&cc);
toc("householder_denseqr");

1
base/DenseQRUtil.h
View File

@ -32,6 +32,7 @@ namespace gtsam {
/** Householder tranformation, zeros below diagonal */
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);
}
#endif

4
base/FastMap.h
View File

@ -41,8 +41,8 @@ public:
FastMap() {}
/** Constructor from a range, passes through to base class */
template<typename InputIterator>
FastMap(InputIterator first, InputIterator last) : Base(first, last) {}
template<typename INPUTITERATOR>
FastMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
/** Copy constructor from another FastMap */
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;
/** Constructor from a range, passes through to base class */
template<typename InputIterator>
FastSet(InputIterator first, InputIterator last) : Base(first, last) {}
template<typename INPUTITERATOR>
FastSet(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
/** Copy constructor from another FastMap */
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
*/
inline Vector logmap(const LieVector& lp) const {
// return Logmap(between(lp)); // works
return lp.vector() - vector();
}

20
base/Matrix-inl.h
View File

@ -33,11 +33,11 @@ using namespace std;
* @param b an RHS vector
* @return the solution x of U*x=b
*/
template<class MatrixAE, class VectorAE>
Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MatrixAE>& _U,
const boost::numeric::ublas::vector_expression<VectorAE>& _b, bool unit=false) {
const MatrixAE& U((const MatrixAE&)_U);
const VectorAE& b((const VectorAE&)_b);
template<class MATRIXAE, class VECTORAE>
Vector backSubstituteUpper(const boost::numeric::ublas::matrix_expression<MATRIXAE>& _U,
const boost::numeric::ublas::vector_expression<VECTORAE>& _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<Matrix
* @return the solution x of x'*U=b'
* TODO: use boost
*/
template<class VectorAE, class MatrixAE>
Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VectorAE>& _b,
const boost::numeric::ublas::matrix_expression<MatrixAE>& _U, bool unit=false) {
const VectorAE& b((const VectorAE&)_b);
const MatrixAE& U((const MatrixAE&)_U);
template<class VECTORAE, class MATRIXAE>
Vector backSubstituteUpper(const boost::numeric::ublas::vector_expression<VECTORAE>& _b,
const boost::numeric::ublas::matrix_expression<MATRIXAE>& _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();

147
base/Matrix.cpp
View File

@ -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<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) {
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<n; ++j) {
// extract the first column of A
Vector a(column_(A, j)); // ublas::matrix_column is slower !
//print(a,"a");
// Calculate weighted pseudo-inverse and corresponding precision
double precision = weightedPseudoinverse(a, weights, pseudo);
// cout << precision << endl;
// print(pseudo,"pseudo");
// if precision is zero, no information on this column
if (precision < 1e-8) continue;
@ -735,64 +677,7 @@ void householder(Matrix &A) {
#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) {
size_t n = L.size2();
#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);
// 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

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, ...)
{
char buf[4096], *p = buf;

94
base/blockMatrices.h
View File

@ -29,15 +29,15 @@ namespace gtsam {
* reference to the range. The stored range stores a reference to the original
* matrix.
*/
template<class Matrix>
class BlockColumn : public boost::numeric::ublas::vector_expression<BlockColumn<Matrix> > {
template<class MATRIX>
class BlockColumn : public boost::numeric::ublas::vector_expression<BlockColumn<MATRIX> > {
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;
Range range_;
Base base_;
public:
typedef BlockColumn<Matrix> Self;
typedef BlockColumn<MATRIX> 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<Matrix>& block, size_t column) :
BlockColumn(const boost::numeric::ublas::matrix_range<MATRIX>& 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<class Matrix>
template<class MATRIX>
class VerticalBlockView {
public:
typedef Matrix matrix_type;
typedef typename boost::numeric::ublas::matrix_range<Matrix> block_type;
typedef typename boost::numeric::ublas::matrix_range<const Matrix> const_block_type;
typedef BlockColumn<Matrix> column_type;
typedef BlockColumn<const Matrix> const_column_type;
typedef MATRIX matrix_type;
typedef typename boost::numeric::ublas::matrix_range<MATRIX> block_type;
typedef typename boost::numeric::ublas::matrix_range<const MATRIX> const_block_type;
typedef BlockColumn<MATRIX> column_type;
typedef BlockColumn<const MATRIX> 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<typename Iterator>
VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim);
template<typename ITERATOR>
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<typename Iterator>
VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight);
template<typename ITERATOR>
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<class RhsMatrix>
void copyStructureFrom(const VerticalBlockView<RhsMatrix>& rhs);
template<class RHSMATRIX>
void copyStructureFrom(const VerticalBlockView<RHSMATRIX>& 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<class RhsMatrix>
VerticalBlockView<Matrix>& assignNoalias(const VerticalBlockView<RhsMatrix>& rhs);
template<class RHSMATRIX>
VerticalBlockView<MATRIX>& assignNoalias(const VerticalBlockView<RHSMATRIX>& rhs);
/** Swap the contents of the underlying matrix and the block information with
* another VerticalBlockView.
*/
void swap(VerticalBlockView<Matrix>& other);
void swap(VerticalBlockView<MATRIX>& other);
protected:
void assertInvariants() const {
@ -215,42 +225,42 @@ protected:
assert(variableColOffsets_[block] < matrix_.size2() && variableColOffsets_[block+1] <= matrix_.size2());
}
template<typename Iterator>
void fillOffsets(Iterator firstBlockDim, Iterator lastBlockDim) {
template<typename ITERATOR>
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<class RhsMatrix>
friend class VerticalBlockView<Matrix>;
template<class RHSMATRIX>
friend class VerticalBlockView<MATRIX>;
};
/* ************************************************************************* */
template<class Matrix>
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix) :
template<class MATRIX>
VerticalBlockView<MATRIX>::VerticalBlockView(matrix_type& matrix) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
fillOffsets((size_t*)0, (size_t*)0);
assertInvariants();
}
/* ************************************************************************* */
template<class Matrix>
template<typename Iterator>
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim) :
template<class MATRIX>
template<typename ITERATOR>
VerticalBlockView<MATRIX>::VerticalBlockView(matrix_type& matrix, ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
fillOffsets(firstBlockDim, lastBlockDim);
assertInvariants();
}
/* ************************************************************************* */
template<class Matrix>
template<typename Iterator>
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight) :
template<class MATRIX>
template<typename ITERATOR>
VerticalBlockView<MATRIX>::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<class Matrix>
template<class RhsMatrix>
void VerticalBlockView<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMatrix>& rhs) {
template<class MATRIX>
template<class RHSMATRIX>
void VerticalBlockView<MATRIX>::copyStructureFrom(const VerticalBlockView<RHSMATRIX>& 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<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMat
}
/* ************************************************************************* */
template<class Matrix>
template<class RhsMatrix>
VerticalBlockView<Matrix>& VerticalBlockView<Matrix>::assignNoalias(const VerticalBlockView<RhsMatrix>& rhs) {
template<class MATRIX>
template<class RHSMATRIX>
VerticalBlockView<MATRIX>& VerticalBlockView<MATRIX>::assignNoalias(const VerticalBlockView<RHSMATRIX>& rhs) {
copyStructureFrom(rhs);
boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks());
return *this;
}
/* ************************************************************************* */
template<class Matrix>
void VerticalBlockView<Matrix>::swap(VerticalBlockView<Matrix>& other) {
template<class MATRIX>
void VerticalBlockView<MATRIX>::swap(VerticalBlockView<MATRIX>& other) {
matrix_.swap(other.matrix_);
variableColOffsets_.swap(other.variableColOffsets_);
std::swap(rowStart_, other.rowStart_);

2
base/numericalDerivative.h
View File

@ -26,7 +26,7 @@
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h>
//#define LINEARIZE_AT_IDENTITY
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) ?\
(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) {