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

Removed some extraneous comments from eigen transition

release/4.3a0
Alex Cunningham 2011-05-20 18:43:37 +00:00
parent 8f417150f5
commit 2e942f08ac
16 changed files with 146 additions and 457 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/base/FixedVector.h
View File

@ -70,10 +70,6 @@ public:
*/ */
inline static FixedVector repeat(double value) { inline static FixedVector repeat(double value) {
return FixedVector(Base::Constant(value)); return FixedVector(Base::Constant(value));
// FixedVector v;
// for (size_t i=0; i<N; ++i)
// v(i) = value;
// return v;
} }
/** /**
@ -85,9 +81,6 @@ public:
*/ */
inline static FixedVector delta(size_t i, double value) { inline static FixedVector delta(size_t i, double value) {
return FixedVector(Base::Unit(i) * value); return FixedVector(Base::Unit(i) * value);
// FixedVector v = zero();
// v(i) = value;
// return v;
} }
/** /**

2
gtsam/base/Makefile.am
View File

@ -40,7 +40,7 @@ check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector
# Timing tests # Timing tests
noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeTest noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeTest
#noinst_PROGRAMS += tests/timeublas noinst_PROGRAMS += tests/timeMatrixOps
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed # Create a libtool library that is not installed

141
gtsam/base/Matrix.cpp
View File

@ -187,55 +187,12 @@ 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) {
// eigen call to exploit template optimizations
e += alpha * A * x; e += alpha * A * x;
//
//#if defined GT_USE_CBLAS
//
// // get sizes
// const size_t m = A.rows(), n = A.cols();
//
// // get pointers
// const double * Aptr = A.data();
// const double * Xptr = x.data();
// double * Eptr = e.data();
//
// // fill in parameters
// const double beta = 1.0;
// const size_t incx = 1, incy = 1, ida = n;
//
// // execute blas call
// cblas_dgemv(CblasRowMajor, CblasNoTrans, m, n, alpha, Aptr, ida, Xptr, incx, beta, Eptr, incy);
//
//#else
//
// size_t m = A.rows(), n = A.cols();
// double * ei = e.data();
// const double * aij = A.data();
// for(size_t i = 0; i < m; i++, ei++) {
// const double * xj = x.data();
// for(size_t j = 0; j < n; j++, aij++, xj++)
// (*ei) += alpha * (*aij) * (*xj);
// }
//#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) {
e += A * x; e += A * x;
//#ifdef GT_USE_CBLAS
// multiplyAdd(1.0, A, x, e);
//#else
// size_t m = A.rows(), n = A.cols();
// double * ei = e.data();
// const double * aij = A.data();
// for(size_t i = 0; i < m; i++, ei++) {
// const double * xj = x.data();
// for(size_t j = 0; j < n; j++, aij++, xj++)
// (*ei) += (*aij) * (*xj);
// }
//#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -251,67 +208,16 @@ Vector operator^(const Matrix& A, const Vector & v) {
/* ************************************************************************* */ /* ************************************************************************* */
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) {
x += alpha * A.transpose() * e; x += alpha * A.transpose() * e;
//
//#if defined GT_USE_CBLAS
//
// // get sizes
// const size_t m = A.rows(), n = A.cols();
//
// // get pointers
// const double * Aptr = A.data();
// const double * Eptr = e.data();
// double * Xptr = x.data();
//
// // fill in parameters
// const double beta = 1.0;
// const size_t incx = 1, incy = 1, ida = n;
//
// // execute blas call
// cblas_dgemv(CblasRowMajor, CblasTrans, m, n, alpha, Aptr, ida, Eptr, incx, beta, Xptr, incy);
//
//#else
//
// // TODO: use BLAS
// size_t m = A.rows(), n = A.cols();
// double * xj = x.data();
// for(size_t j = 0; j < n; j++,xj++) {
// const double * ei = e.data();
// const double * aij = A.data() + j;
// for(size_t i = 0; i < m; i++, aij+=n, ei++)
// (*xj) += alpha * (*aij) * (*ei);
// }
//#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) {
x += A.transpose() * e; x += A.transpose() * e;
//#ifdef GT_USE_CBLAS
// transposeMultiplyAdd(1.0, A, e, x);
//#else
// size_t m = A.rows(), n = A.cols();
// double * xj = x.data();
// for(size_t j = 0; j < n; j++,xj++) {
// const double * ei = e.data();
// const double * aij = A.data() + j;
// for(size_t i = 0; i < m; i++, aij+=n, ei++)
// (*xj) += (*aij) * (*ei);
// }
//#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) {
x += alpha * A.transpose() * e; x += alpha * A.transpose() * e;
// // TODO: use BLAS
// size_t m = A.rows(), n = A.cols();
// for (size_t j = 0; j < n; j++) {
// const double * ei = e.data();
// const double * aij = A.data() + j;
// for (size_t i = 0; i < m; i++, aij+=n, ei++)
// x(j) += alpha * (*aij) * (*ei);
// }
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -472,28 +378,6 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
* on a number of different matrices for which all columns change. * on a number of different matrices for which all columns change.
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
//void householder_update(Matrix &A, size_t j, double beta, const Vector& vjm) {
// const size_t m = A.rows(), n = A.cols();
//
// 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) * v[s];
// w(c) *= beta;
// }
//
// // rank 1 update A(j:m,:) -= v(j:m)*w'
// for( size_t c = 0 ; c < n; c++) {
// 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) -= v[s] * wc;
// }
//}
/** /**
* Perform updates of system matrices * Perform updates of system matrices
@ -585,29 +469,6 @@ void householder_(Matrix &A, size_t k)
// the Householder vector is copied in the zeroed out part // the Householder vector is copied in the zeroed out part
A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1)); A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1));
} // column j } // column j
// const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
// // loop over the kprime first columns
// for(size_t j=0; j < kprime; j++){
// // below, the indices r,c always refer to original A
//
// // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j)
// Vector xjm(m-j);
// for(size_t r = j ; r < m; r++)
// xjm(r-j) = A(r,j);
//
// // calculate the Householder vector, in place
// double beta = houseInPlace(xjm);
// Vector& vjm = xjm;
//
// // do outer product update A = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w'
// householder_update(A, j, beta, vjm);
//
// // the Householder vector is copied in the zeroed out part
// for( size_t r = j+1 ; r < m ; r++ )
// A(r,j) = vjm(r-j);
//
// } // column j
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -617,8 +478,6 @@ void householder(Matrix &A, size_t k) {
const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
for(size_t j=0; j < kprime; j++) for(size_t j=0; j < kprime; j++)
A.col(j).segment(j+1, m-(j+1)).setZero(); A.col(j).segment(j+1, m-(j+1)).setZero();
// for( size_t i = j+1 ; i < m ; i++ )
// A(i,j) = 0.0;
} }
/* ************************************************************************* */ /* ************************************************************************* */

24
gtsam/base/Matrix.h
View File

@ -307,11 +307,6 @@ void inplace_QR(MATRIX& A, bool clear_below_diagonal=true) {
zeroBelowDiagonal(A); zeroBelowDiagonal(A);
} }
/**
* Imperative version of Householder rank 1 update
*/
//void householder_update(Matrix &A, size_t j, double beta, const Vector& vjm);
/** /**
* Imperative algorithm for in-place full elimination with * Imperative algorithm for in-place full elimination with
* weights and constraint handling * weights and constraint handling
@ -358,25 +353,6 @@ void householder_(MatrixColMajor& A, size_t k, bool copy_vectors=true);
*/ */
void householder(MatrixColMajor& A, size_t k); void householder(MatrixColMajor& A, size_t k);
//#ifdef GT_USE_LAPACK
//#ifdef USE_LAPACK_QR
///**
// * Householder tranformation, zeros below diagonal
// * @return nothing: in place !!!
// */
//void householder(Matrix& A);
//
///**
// * Householder tranformation directly on a column-major matrix. Does not zero
// * below the diagonal, so it will contain Householder vectors.
// * @return nothing: in place !!!
// * FIXME: this uses the LAPACK QR function, so it cannot be used on Ubuntu (without
// * lots of hacks, at least)
// */
//void householderColMajor(MatrixColMajor& A);
//#endif
//#endif
/** /**
* backSubstitute U*x=b * backSubstitute U*x=b
* @param U an upper triangular matrix * @param U an upper triangular matrix

32
gtsam/base/Vector.cpp
View File

@ -209,7 +209,7 @@ bool assert_equal(const Vector& expected, const Vector& actual, double tol) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool assert_equal(SubVector expected, SubVector actual, double tol) { bool assert_equal(const SubVector& expected, const SubVector& actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true; if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl; cout << "not equal:" << endl;
print(expected, "expected"); print(expected, "expected");
@ -217,14 +217,14 @@ bool assert_equal(SubVector expected, SubVector actual, double tol) {
return false; return false;
} }
///* ************************************************************************* */ /* ************************************************************************* */
//bool assert_equal(ConstSubVector expected, ConstSubVector actual, double tol) { bool assert_equal(const ConstSubVector& expected, const ConstSubVector& actual, double tol) {
// if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true; if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true;
// cout << "not equal:" << endl; cout << "not equal:" << endl;
// print(Vector(expected), "expected"); print(Vector(expected), "expected");
// print(Vector(actual), "actual"); print(Vector(actual), "actual");
// return false; return false;
//} }
/* ************************************************************************* */ /* ************************************************************************* */
bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
@ -281,11 +281,6 @@ Vector ediv_(const Vector &a, const Vector &b) {
/* ************************************************************************* */ /* ************************************************************************* */
double sum(const Vector &a) { double sum(const Vector &a) {
return a.sum(); return a.sum();
// double result = 0.0;
// size_t n = a.size();
// for( size_t i = 0; i < n; i++ )
// result += a(i);
// return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -305,9 +300,6 @@ Vector reciprocal(const Vector &a) {
/* ************************************************************************* */ /* ************************************************************************* */
Vector esqrt(const Vector& v) { Vector esqrt(const Vector& v) {
return v.cwiseSqrt(); return v.cwiseSqrt();
// Vector s(v.size());
// transform(v.begin(), v.end(), s.begin(), ::sqrt);
// return s;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -335,18 +327,12 @@ void scal(double alpha, Vector& x) {
void axpy(double alpha, const Vector& x, Vector& y) { void axpy(double alpha, const Vector& x, Vector& y) {
assert (y.size()==x.size()); assert (y.size()==x.size());
y += alpha * x; y += alpha * x;
// size_t n = x.size();
// for (size_t i = 0; i < n; i++)
// y[i] += alpha * x[i];
} }
/* ************************************************************************* */ /* ************************************************************************* */
void axpy(double alpha, const Vector& x, SubVector y) { void axpy(double alpha, const Vector& x, SubVector y) {
assert (y.size()==x.size()); assert (y.size()==x.size());
y += alpha * x; y += alpha * x;
// size_t n = x.size();
// for (size_t i = 0; i < n; i++)
// y[i] += alpha * x[i];
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/base/Vector.h
View File

@ -167,8 +167,8 @@ bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
* @param tol 1e-9 * @param tol 1e-9
* @return bool * @return bool
*/ */
bool assert_equal(SubVector vec1, SubVector vec2, double tol=1e-9); bool assert_equal(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
//bool assert_equal(ConstSubVector vec1, ConstSubVector vec2, double tol=1e-9); bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& vec2, double tol=1e-9);
/** /**
* check whether two vectors are linearly dependent * check whether two vectors are linearly dependent

22
gtsam/base/blockMatrices.h
View File

@ -313,11 +313,10 @@ public:
typedef Eigen::Block<MATRIX> Block; typedef Eigen::Block<MATRIX> Block;
typedef Eigen::Block<const MATRIX> constBlock; typedef Eigen::Block<const MATRIX> constBlock;
// typedef typename MATRIX::ColXpr Column; private:
// typedef typename MATRIX::ConstColXpr constColumn; static FullMatrix matrixTemp_; // just for finding types
protected: protected:
static FullMatrix matrixTemp_; // the reference to the full matrix
FullMatrix& matrix_; // the reference to the full matrix FullMatrix& matrix_; // the reference to the full matrix
std::vector<size_t> variableColOffsets_; // the starting columns of each block (0-based) std::vector<size_t> variableColOffsets_; // the starting columns of each block (0-based)
@ -426,11 +425,6 @@ public:
Column column(size_t i_block, size_t j_block, size_t columnOffset) { Column column(size_t i_block, size_t j_block, size_t columnOffset) {
assertInvariants(); assertInvariants();
// size_t j_actualBlock = j_block + blockStart_;
// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
// Block blockMat(operator()(i_block, j_block));
// return blockMat.col(columnOffset);
size_t i_actualBlock = i_block + blockStart_; size_t i_actualBlock = i_block + blockStart_;
size_t j_actualBlock = j_block + blockStart_; size_t j_actualBlock = j_block + blockStart_;
checkBlock(i_actualBlock); checkBlock(i_actualBlock);
@ -468,12 +462,6 @@ public:
variableColOffsets_[j_actualStartBlock] + columnOffset).segment( variableColOffsets_[j_actualStartBlock] + columnOffset).segment(
variableColOffsets_[i_actualStartBlock], variableColOffsets_[i_actualStartBlock],
variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]);
// column of a block approach
// size_t j_actualBlock = j_block + blockStart_;
// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
// Block blockMat = this->range(i_startBlock, i_endBlock, j_block, j_block+1);
// return Column(blockMat, columnOffset);
} }
constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const { constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const {
@ -491,12 +479,6 @@ public:
variableColOffsets_[j_actualStartBlock] + columnOffset).segment( variableColOffsets_[j_actualStartBlock] + columnOffset).segment(
variableColOffsets_[i_actualStartBlock], variableColOffsets_[i_actualStartBlock],
variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]);
// column of a block approach
// size_t j_actualBlock = j_block + blockStart_;
// assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]);
// constBlock blockMat = this->range(i_startBlock, i_endBlock, j_block, j_block+1);
// return constColumn(blockMat, columnOffset);
} }
size_t offset(size_t block) const { size_t offset(size_t block) const {

9
gtsam/base/cholesky.cpp
View File

@ -25,8 +25,6 @@
#include <boost/format.hpp> #include <boost/format.hpp>
//namespace ublas = boost::numeric::ublas;
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
@ -58,17 +56,12 @@ static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) {
ATA(k,k) = beta; ATA(k,k) = beta;
if(k < (order-1)) { if(k < (order-1)) {
// ublas::matrix_row<MatrixColMajor> Vf(ublas::row(ATA, k));
// ublas::vector_range<typeof(Vf)> V(ublas::subrange(Vf, k+1,order));
// Update A(k,k+1:end) <- A(k,k+1:end) / beta // Update A(k,k+1:end) <- A(k,k+1:end) / beta
typedef typeof(ATA.row(k).segment(k+1, order-(k+1))) BlockRow; typedef typeof(ATA.row(k).segment(k+1, order-(k+1))) BlockRow;
BlockRow V = ATA.row(k).segment(k+1, order-(k+1)); BlockRow V = ATA.row(k).segment(k+1, order-(k+1));
V *= betainv; V *= betainv;
// Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
// ublas::matrix_range<MatrixColMajor> L(ublas::subrange(ATA, k+1,order, k+1,order));
// L -= ublas::outer_prod(V, V);
ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V;
} }
@ -123,8 +116,6 @@ void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) {
const bool debug = ISDEBUG("choleskyPartial"); const bool debug = ISDEBUG("choleskyPartial");
// Eigen::Map<Eigen::MatrixXd> ABC(&ABCublas(0,0), ABCublas.rows(), ABCublas.cols());
assert(ABC.rows() == ABC.cols()); assert(ABC.rows() == ABC.cols());
assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows())); assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows()));

43
gtsam/base/tests/testMatrix.cpp
View File

@ -758,31 +758,6 @@ TEST( matrix, eigen_QR )
EXPECT(assert_equal(expected, A, 1e-3)); EXPECT(assert_equal(expected, A, 1e-3));
} }
///* ************************************************************************* */
//// unit tests for householder transformation
///* ************************************************************************* */
//#ifdef GT_USE_LAPACK
//#ifdef YA_BLAS
//TEST( matrix, houseHolder2 )
//{
// double data[] = { -5, 0, 5, 0, 0, 0, -1,
// 00,-5, 0, 5, 0, 0, 1.5,
// 10, 0, 0, 0,-10,0, 2,
// 00, 10,0, 0, 0, -10, -1 };
//
// // check in-place householder, with v vectors below diagonal
// double data1[] = { 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
// 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
// 0, 0, 4.4721, 0, -4.4721, 0, 0,
// 0, 0, 0, 4.4721, 0, -4.4721, 0.894 };
// Matrix expected1 = Matrix_(4, 7, data1);
// Matrix A1 = Matrix_(4, 7, data);
// householder(A1);
// EXPECT(assert_equal(expected1, A1, 1e-3));
//}
//#endif
//#endif
/* ************************************************************************* */ /* ************************************************************************* */
// unit test for qr factorization (and hence householder) // unit test for qr factorization (and hence householder)
// This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs // This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs
@ -840,24 +815,6 @@ TEST( matrix, row_major_access )
DOUBLES_EQUAL(3,a[2],1e-9); DOUBLES_EQUAL(3,a[2],1e-9);
} }
/* ************************************************************************* */
// update A, b
// A' \define A_{S}-ar and b'\define b-ad
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
//static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
// const Vector& r, double d) {
// const size_t m = A.rows(), n = A.cols();
// for (int i = 0; i < m; i++) { // update all rows
// double ai = a(i);
// 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 (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++)
// *Aij -= ai * (*rptr);
// }
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, weighted_elimination ) TEST( matrix, weighted_elimination )
{ {

245
gtsam/base/tests/timeublas.cpp → gtsam/base/tests/timeMatrixOps.cpp
View File

@ -11,41 +11,47 @@
/** /**
* @file timeublas.cpp * @file timeublas.cpp
* @brief Tests to help determine which way of accomplishing something in ublas is faster * @brief Tests to help determine which way of accomplishing something with Eigen is faster
* @author Richard Roberts * @author Richard Roberts
* @created Sep 18, 2010 * @created Sep 18, 2010
*/ */
#include <boost/numeric/ublas/matrix.hpp> //#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp> //#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp> //#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp> //#include <boost/numeric/ublas/io.hpp>
#include <boost/random.hpp> #include <boost/random.hpp>
#include <boost/timer.hpp> #include <boost/timer.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/base/Matrix.h>
#include <iostream> #include <iostream>
#include <vector> #include <vector>
#include <utility> #include <utility>
using namespace std; using namespace std;
namespace ublas = boost::numeric::ublas; //namespace ublas = boost::numeric::ublas;
//using namespace Eigen;
using boost::timer; using boost::timer;
using boost::format; using boost::format;
using namespace boost::lambda; using namespace boost::lambda;
static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0)); static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0));
typedef ublas::matrix<double> matrix; //typedef ublas::matrix<double> matrix;
typedef ublas::matrix_range<matrix> matrix_range; //typedef ublas::matrix_range<matrix> matrix_range;
using ublas::range; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
using ublas::triangular_matrix; typedef Eigen::Block<matrix> matrix_block;
//using ublas::range;
//using ublas::triangular_matrix;
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
if(false) { if(true) {
cout << "\nTiming matrix_range:" << endl; cout << "\nTiming matrix_block:" << endl;
// We use volatile here to make these appear to the optimizing compiler as // We use volatile here to make these appear to the optimizing compiler as
// if their values are only known at run-time. // if their values are only known at run-time.
@ -56,9 +62,9 @@ int main(int argc, char* argv[]) {
boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rni(boost::mt19937(), boost::uniform_int<size_t>(0,m-1)); boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rni(boost::mt19937(), boost::uniform_int<size_t>(0,m-1));
boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rnj(boost::mt19937(), boost::uniform_int<size_t>(0,n-1)); boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rnj(boost::mt19937(), boost::uniform_int<size_t>(0,n-1));
matrix mat(m,n); matrix mat(m,n);
matrix_range full(mat, range(0,m), range(0,n)); matrix_block full = mat.block(0, 0, m, n);
matrix_range top(mat, range(0,n), range(0,n)); matrix_block top = mat.block(0, 0, n, n);
matrix_range block(mat, range(m/4, m-m/4), range(n/4, n-n/4)); matrix_block block = mat.block(m/4, n/4, m-m/2, n-n/2);
cout << format(" Basic: %1%x%2%\n") % m % n; cout << format(" Basic: %1%x%2%\n") % m % n;
cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n; cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n;
@ -74,41 +80,41 @@ int main(int argc, char* argv[]) {
// Do a few initial assignments to let any cache effects stabilize // Do a few initial assignments to let any cache effects stabilize
for(size_t rep=0; rep<1000; ++rep) for(size_t rep=0; rep<1000; ++rep)
for(size_t i=0; i<mat.size1(); ++i) for(size_t i=0; i<(size_t)mat.rows(); ++i)
for(size_t j=0; j<mat.size2(); ++j) for(size_t j=0; j<(size_t)mat.cols(); ++j)
mat(i,j) = rng(); mat(i,j) = rng();
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t i=0; i<mat.size1(); ++i) for(size_t i=0; i<(size_t)mat.rows(); ++i)
for(size_t j=0; j<mat.size2(); ++j) for(size_t j=0; j<(size_t)mat.cols(); ++j)
mat(i,j) = rng(); mat(i,j) = rng();
basicTime = tim.elapsed(); basicTime = tim.elapsed();
cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.size1()*mat.size2()*nReps)) << endl; cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl;
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t i=0; i<full.size1(); ++i) for(size_t i=0; i<(size_t)full.rows(); ++i)
for(size_t j=0; j<full.size2(); ++j) for(size_t j=0; j<(size_t)full.cols(); ++j)
full(i,j) = rng(); full(i,j) = rng();
fullTime = tim.elapsed(); fullTime = tim.elapsed();
cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.size1()*full.size2()*nReps)) << endl; cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl;
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t i=0; i<top.size1(); ++i) for(size_t i=0; i<(size_t)top.rows(); ++i)
for(size_t j=0; j<top.size2(); ++j) for(size_t j=0; j<(size_t)top.cols(); ++j)
top(i,j) = rng(); top(i,j) = rng();
topTime = tim.elapsed(); topTime = tim.elapsed();
cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.size1()*top.size2()*nReps)) << endl; cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl;
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t i=0; i<block.size1(); ++i) for(size_t i=0; i<(size_t)block.rows(); ++i)
for(size_t j=0; j<block.size2(); ++j) for(size_t j=0; j<(size_t)block.cols(); ++j)
block(i,j) = rng(); block(i,j) = rng();
blockTime = tim.elapsed(); blockTime = tim.elapsed();
cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.size1()*block.size2()*nReps)) << endl; cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl;
cout << endl; cout << endl;
} }
@ -121,41 +127,41 @@ int main(int argc, char* argv[]) {
// Do a few initial assignments to let any cache effects stabilize // Do a few initial assignments to let any cache effects stabilize
for(size_t rep=0; rep<1000; ++rep) for(size_t rep=0; rep<1000; ++rep)
for(size_t j=0; j<mat.size2(); ++j) for(size_t j=0; j<(size_t)mat.cols(); ++j)
for(size_t i=0; i<mat.size1(); ++i) for(size_t i=0; i<(size_t)mat.rows(); ++i)
mat(i,j) = rng(); mat(i,j) = rng();
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t j=0; j<mat.size2(); ++j) for(size_t j=0; j<(size_t)mat.cols(); ++j)
for(size_t i=0; i<mat.size1(); ++i) for(size_t i=0; i<(size_t)mat.rows(); ++i)
mat(i,j) = rng(); mat(i,j) = rng();
basicTime = tim.elapsed(); basicTime = tim.elapsed();
cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.size1()*mat.size2()*nReps)) << endl; cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl;
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t j=0; j<full.size2(); ++j) for(size_t j=0; j<(size_t)full.cols(); ++j)
for(size_t i=0; i<full.size1(); ++i) for(size_t i=0; i<(size_t)full.rows(); ++i)
full(i,j) = rng(); full(i,j) = rng();
fullTime = tim.elapsed(); fullTime = tim.elapsed();
cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.size1()*full.size2()*nReps)) << endl; cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl;
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t j=0; j<top.size2(); ++j) for(size_t j=0; j<(size_t)top.cols(); ++j)
for(size_t i=0; i<top.size1(); ++i) for(size_t i=0; i<(size_t)top.rows(); ++i)
top(i,j) = rng(); top(i,j) = rng();
topTime = tim.elapsed(); topTime = tim.elapsed();
cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.size1()*top.size2()*nReps)) << endl; cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl;
tim.restart(); tim.restart();
for(size_t rep=0; rep<nReps; ++rep) for(size_t rep=0; rep<nReps; ++rep)
for(size_t j=0; j<block.size2(); ++j) for(size_t j=0; j<(size_t)block.cols(); ++j)
for(size_t i=0; i<block.size1(); ++i) for(size_t i=0; i<(size_t)block.rows(); ++i)
block(i,j) = rng(); block(i,j) = rng();
blockTime = tim.elapsed(); blockTime = tim.elapsed();
cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.size1()*block.size2()*nReps)) << endl; cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl;
cout << endl; cout << endl;
} }
@ -185,13 +191,13 @@ int main(int argc, char* argv[]) {
fullTime = tim.elapsed(); fullTime = tim.elapsed();
cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl;
for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.size1(),rnj())); for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj()));
for(size_t rep=0; rep<1000; ++rep) for(size_t rep=0; rep<1000; ++rep)
BOOST_FOREACH(const ij_t& ij, ijs) { top(ij.first, ij.second) = rng(); } BOOST_FOREACH(const ij_t& ij, ijs) { top(ij.first, ij.second) = rng(); }
topTime = tim.elapsed(); topTime = tim.elapsed();
cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl;
for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.size1(),rnj()%block.size2())); for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols()));
for(size_t rep=0; rep<1000; ++rep) for(size_t rep=0; rep<1000; ++rep)
BOOST_FOREACH(const ij_t& ij, ijs) { block(ij.first, ij.second) = rng(); } BOOST_FOREACH(const ij_t& ij, ijs) { block(ij.first, ij.second) = rng(); }
blockTime = tim.elapsed(); blockTime = tim.elapsed();
@ -201,82 +207,83 @@ int main(int argc, char* argv[]) {
} }
} }
if(true) { // if(true) {
cout << "\nTesting square triangular matrices:" << endl; // cout << "\nTesting square triangular matrices:" << endl;
//
//// typedef triangular_matrix<double, ublas::upper, ublas::column_major> triangular;
//// typedef ublas::matrix<double, ublas::column_major> matrix;
// typedef MatrixXd matrix; // default col major
//
//// triangular tri(5,5);
//
// matrix mat(5,5);
// for(size_t j=0; j<(size_t)mat.cols(); ++j)
// for(size_t i=0; i<(size_t)mat.rows(); ++i)
// mat(i,j) = rng();
//
// tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
// cout << " Assigned from triangular adapter: " << tri << endl;
//
// cout << " Triangular adapter of mat: " << ublas::triangular_adaptor<matrix, ublas::upper>(mat) << endl;
//
// for(size_t j=0; j<(size_t)mat.cols(); ++j)
// for(size_t i=0; i<(size_t)mat.rows(); ++i)
// mat(i,j) = rng();
// mat = tri;
// cout << " Assign matrix from triangular: " << mat << endl;
//
// for(size_t j=0; j<(size_t)mat.cols(); ++j)
// for(size_t i=0; i<(size_t)mat.rows(); ++i)
// mat(i,j) = rng();
// (ublas::triangular_adaptor<matrix, ublas::upper>(mat)) = tri;
// cout << " Assign triangular adaptor from triangular: " << mat << endl;
// }
typedef triangular_matrix<double, ublas::upper, ublas::column_major> triangular; // {
typedef ublas::matrix<double, ublas::column_major> matrix; // cout << "\nTesting wide triangular matrices:" << endl;
//
// typedef triangular_matrix<double, ublas::upper, ublas::column_major> triangular;
// typedef ublas::matrix<double, ublas::column_major> matrix;
//
// triangular tri(5,7);
//
// matrix mat(5,7);
// for(size_t j=0; j<(size_t)mat.cols(); ++j)
// for(size_t i=0; i<(size_t)mat.rows(); ++i)
// mat(i,j) = rng();
//
// tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
// cout << " Assigned from triangular adapter: " << tri << endl;
//
// cout << " Triangular adapter of mat: " << ublas::triangular_adaptor<matrix, ublas::upper>(mat) << endl;
//
// for(size_t j=0; j<(size_t)mat.cols(); ++j)
// for(size_t i=0; i<(size_t)mat.rows(); ++i)
// mat(i,j) = rng();
// mat = tri;
// cout << " Assign matrix from triangular: " << mat << endl;
//
// for(size_t j=0; j<(size_t)mat.cols(); ++j)
// for(size_t i=0; i<(size_t)mat.rows(); ++i)
// mat(i,j) = rng();
// mat = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
// cout << " Assign matrix from triangular adaptor of self: " << mat << endl;
// }
triangular tri(5,5); // {
// cout << "\nTesting subvectors:" << endl;
matrix mat(5,5); //
for(size_t j=0; j<mat.size2(); ++j) // typedef MatrixXd matrix;
for(size_t i=0; i<mat.size1(); ++i) // matrix mat(4,4);
mat(i,j) = rng(); //
// for(size_t j=0; j<(size_t)mat.cols(); ++j)
tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat); // for(size_t i=0; i<(size_t)mat.rows(); ++i)
cout << " Assigned from triangular adapter: " << tri << endl; // mat(i,j) = i*mat.rows() + j;
// cout << " mat = " << mat;
cout << " Triangular adapter of mat: " << ublas::triangular_adaptor<matrix, ublas::upper>(mat) << endl; //
// cout << " vec(1:4, 2:2) = " << mat.block(1,2, ), ublas::range(1,4), ublas::range(2,2));
for(size_t j=0; j<mat.size2(); ++j) //
for(size_t i=0; i<mat.size1(); ++i) // }
mat(i,j) = rng();
mat = tri;
cout << " Assign matrix from triangular: " << mat << endl;
for(size_t j=0; j<mat.size2(); ++j)
for(size_t i=0; i<mat.size1(); ++i)
mat(i,j) = rng();
(ublas::triangular_adaptor<matrix, ublas::upper>(mat)) = tri;
cout << " Assign triangular adaptor from triangular: " << mat << endl;
}
{
cout << "\nTesting wide triangular matrices:" << endl;
typedef triangular_matrix<double, ublas::upper, ublas::column_major> triangular;
typedef ublas::matrix<double, ublas::column_major> matrix;
triangular tri(5,7);
matrix mat(5,7);
for(size_t j=0; j<mat.size2(); ++j)
for(size_t i=0; i<mat.size1(); ++i)
mat(i,j) = rng();
tri = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
cout << " Assigned from triangular adapter: " << tri << endl;
cout << " Triangular adapter of mat: " << ublas::triangular_adaptor<matrix, ublas::upper>(mat) << endl;
for(size_t j=0; j<mat.size2(); ++j)
for(size_t i=0; i<mat.size1(); ++i)
mat(i,j) = rng();
mat = tri;
cout << " Assign matrix from triangular: " << mat << endl;
for(size_t j=0; j<mat.size2(); ++j)
for(size_t i=0; i<mat.size1(); ++i)
mat(i,j) = rng();
mat = ublas::triangular_adaptor<matrix, ublas::upper>(mat);
cout << " Assign matrix from triangular adaptor of self: " << mat << endl;
}
{
cout << "\nTesting subvectors:" << endl;
typedef ublas::matrix<double, ublas::column_major> matrix;
matrix mat(4,4);
for(size_t j=0; j<mat.size2(); ++j)
for(size_t i=0; i<mat.size1(); ++i)
mat(i,j) = i*mat.size1() + j;
cout << " mat = " << mat;
cout << " vec(1:4, 2:2) = " << ublas::matrix_vector_range<matrix>(mat, ublas::range(1,4), ublas::range(2,2));
}
return 0; return 0;

4
gtsam/linear/GaussianConditional.cpp
View File

@ -163,10 +163,8 @@ Vector GaussianConditional::solve(const VectorValues& x) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianConditional::solve(const Permuted<VectorValues>& x) const { Vector GaussianConditional::solve(const Permuted<VectorValues>& x) const {
Vector rhs(get_d()); Vector rhs(get_d());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { for (const_iterator parent = beginParents(); parent != endParents(); ++parent)
rhs += -get_S(parent) * x[*parent]; rhs += -get_S(parent) * x[*parent];
// ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false);
}
return backSubstituteUpper(get_R(), rhs, false); return backSubstituteUpper(get_R(), rhs, false);
} }

28
gtsam/linear/HessianFactor.cpp
View File

@ -125,15 +125,6 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
info_.copyStructureFrom(jf.Ab_); info_.copyStructureFrom(jf.Ab_);
BlockInfo::constBlock A = jf.Ab_.full(); BlockInfo::constBlock A = jf.Ab_.full();
matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A;
// typedef Eigen::Map<Eigen::MatrixXd> EigenMap;
// typedef typeof(EigenMap(&jf.matrix_(0,0),0,0).block(0,0,0,0)) EigenBlock;
// EigenBlock A(EigenMap(&jf.matrix_(0,0),jf.matrix_.rows(),jf.matrix_.cols()).block(jf.Ab_.rowStart(),jf.Ab_.offset(0), jf.Ab_.full().rows(), jf.Ab_.full().cols()));
// typedef typeof(Eigen::Map<Eigen::VectorXd>(&invsigmas(0),0).asDiagonal()) EigenDiagonal;
// EigenDiagonal R(Eigen::Map<Eigen::VectorXd>(&invsigmas(0),jf.model_->dim()).asDiagonal());
// info_.copyStructureFrom(jf.Ab_);
// EigenMap L(EigenMap(&matrix_(0,0), matrix_.rows(), matrix_.cols()));
// L.noalias() = A.transpose() * R * R * A;
} }
} else if(dynamic_cast<const HessianFactor*>(&gf)) { } else if(dynamic_cast<const HessianFactor*>(&gf)) {
const HessianFactor& hf(static_cast<const HessianFactor&>(gf)); const HessianFactor& hf(static_cast<const HessianFactor&>(gf));
@ -217,15 +208,6 @@ HessianFactor::constColumn HessianFactor::linear_term() const {
/* ************************************************************************* */ /* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const { double HessianFactor::error(const VectorValues& c) const {
// error 0.5*(f - 2*x'*g + x'*G*x) // error 0.5*(f - 2*x'*g + x'*G*x)
// return 0.5 * (ublas::inner_prod(c.vector(),
// ublas::prod(
// ublas::symmetric_adaptor<const constBlock,ublas::upper>(info_.range(0, this->size(), 0, this->size())),
// c.vector())) -
// 2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0)) +
// info_(this->size(), this->size())(0,0));
// double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView<Eigen::Upper>() * dxv);
const double f = constant_term(); const double f = constant_term();
const double xtg = c.vector().dot(linear_term()); const double xtg = c.vector().dot(linear_term());
const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * c.vector(); const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * c.vector();
@ -257,9 +239,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
update.print("with: "); update.print("with: ");
} }
// Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.rows(), matrix_.cols());
// Eigen::Map<Eigen::MatrixXd> updateInform(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols());
// Apply updates to the upper triangle // Apply updates to the upper triangle
tic(3, "update"); tic(3, "update");
assert(this->info_.nBlocks() - 1 == scatter.size()); assert(this->info_.nBlocks() - 1 == scatter.size());
@ -318,8 +297,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
update.print("with: "); update.print("with: ");
} }
// Eigen::Map<Eigen::MatrixXd> information(&matrix_(0,0), matrix_.rows(), matrix_.cols());
// Eigen::Map<Eigen::MatrixXd> updateAf(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols());
Eigen::Block<typeof(update.matrix_)> updateA(update.matrix_.block( Eigen::Block<typeof(update.matrix_)> updateA(update.matrix_.block(
update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols())); update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols()));
@ -463,11 +440,6 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& key
tic(1, "zero"); tic(1, "zero");
BlockAb::Block remainingMatrix(Ab.range(0, Ab.nBlocks())); BlockAb::Block remainingMatrix(Ab.range(0, Ab.nBlocks()));
zeroBelowDiagonal(remainingMatrix); zeroBelowDiagonal(remainingMatrix);
// for(size_t j = 0; j < (size_t) remainingMatrix.rows() - 1; ++j) {
// remainingMatrix.col(j).tail(remainingMatrix.rows() - (j+1)).setZero();
// ublas::matrix_column<BlockAb::Block> col(ublas::column(remainingMatrix, j));
// std::fill(col.begin() + j+1, col.end(), 0.0);
// }
toc(1, "zero"); toc(1, "zero");
} }

12
gtsam/linear/JacobianFactor.cpp
View File

@ -256,11 +256,9 @@ namespace gtsam {
j = 0; j = 0;
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
keys_[j] = sourceSlot.first; keys_[j] = sourceSlot.first;
Ab_(j++) = oldAb(sourceSlot.second); Ab_(j++).noalias() = oldAb(sourceSlot.second);
// ublas::noalias(Ab_(j++)) = oldAb(sourceSlot.second);
} }
Ab_(j) = oldAb(j); Ab_(j).noalias() = oldAb(j);
// ublas::noalias(Ab_(j)) = oldAb(j);
// Since we're permuting the variables, ensure that entire rows from this // Since we're permuting the variables, ensure that entire rows from this
// factor are copied when Combine is called // factor are copied when Combine is called
@ -378,6 +376,8 @@ namespace gtsam {
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
if(debug) this->print("Eliminating JacobianFactor: "); if(debug) this->print("Eliminating JacobianFactor: ");
// NOTE: stairs are not currently used in the Eigen QR implementation
// add this back if DenseQR is ever reimplemented
// tic(1, "stairs"); // tic(1, "stairs");
// // Translate the left-most nonzero column indices into top-most zero row indices // // Translate the left-most nonzero column indices into top-most zero row indices
// vector<int> firstZeroRows(Ab_.cols()); // vector<int> firstZeroRows(Ab_.cols());
@ -441,7 +441,6 @@ namespace gtsam {
size_t varDim = Ab_(0).cols(); size_t varDim = Ab_(0).cols();
Ab_.rowEnd() = Ab_.rowStart() + varDim; Ab_.rowEnd() = Ab_.rowStart() + varDim;
const Eigen::VectorBlock<const Vector> sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); const Eigen::VectorBlock<const Vector> sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
// const ublas::vector_range<const Vector> sigmas(noiseModel->sigmas(), ublas::range(Ab_.rowStart(), Ab_.rowEnd()));
conditionals->push_back(boost::make_shared<ConditionalType>(begin()+j, end(), 1, Ab_, sigmas)); conditionals->push_back(boost::make_shared<ConditionalType>(begin()+j, end(), 1, Ab_, sigmas));
if(debug) conditionals->back()->print("Extracted conditional: "); if(debug) conditionals->back()->print("Extracted conditional: ");
Ab_.rowStart() += varDim; Ab_.rowStart() += varDim;
@ -521,14 +520,11 @@ namespace gtsam {
if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) { if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) {
const constABlock sourceBlock(source.Ab_(sourceSlot)); const constABlock sourceBlock(source.Ab_(sourceSlot));
combinedBlock.row(row).noalias() = sourceBlock.row(sourceRow); combinedBlock.row(row).noalias() = sourceBlock.row(sourceRow);
// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow);
} else { } else {
combinedBlock.row(row).setZero(); combinedBlock.row(row).setZero();
// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<double>(combinedBlock.cols());
} }
} else { } else {
combinedBlock.row(row).setZero(); combinedBlock.row(row).setZero();
// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector<double>(combinedBlock.cols());
} }
} }

18
gtsam/linear/NoiseModel.cpp
View File

@ -35,16 +35,7 @@
static double inf = std::numeric_limits<double>::infinity(); static double inf = std::numeric_limits<double>::infinity();
using namespace std; using namespace std;
/** implement the export code for serialization */
// FIXME: doesn't work outside of the library
//BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::noiseModel::Constrained);
//BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::noiseModel::Diagonal);
//BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::noiseModel::Gaussian);
//BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::noiseModel::Unit);
//BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::noiseModel::Isotropic);
namespace gtsam { namespace gtsam {
namespace noiseModel { namespace noiseModel {
/* ************************************************************************* */ /* ************************************************************************* */
@ -57,15 +48,6 @@ template<class MATRIX>
void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
size_t n = Ab.cols()-1; size_t n = Ab.cols()-1;
Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
// size_t m = Ab.rows(), n = Ab.cols()-1;
// for (size_t i = 0; i < m; i++) { // update all rows
// double ai = a(i);
// double *Aij = Ab.data() + i * (n+1) + j + 1;
// const double *rptr = rd.data() + j + 1;
// // Ab(i,j+1:end) -= ai*rd(j+1:end)
// for (size_t j2 = j + 1; j2 < n+1; j2++, Aij++, rptr++)
// *Aij -= ai * (*rptr);
// }
} }

10
gtsam/linear/NoiseModel.h
View File

@ -504,14 +504,4 @@ namespace gtsam {
} // namespace gtsam } // namespace gtsam
/** Export keys for serialization */
// FIXME: doesn't actually work outside of the library
//#include <boost/serialization/export.hpp>
//#include <boost/serialization/extended_type_info.hpp>
//BOOST_CLASS_EXPORT_KEY2(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
//BOOST_CLASS_EXPORT_KEY2(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
//BOOST_CLASS_EXPORT_KEY2(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
//BOOST_CLASS_EXPORT_KEY2(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
//BOOST_CLASS_EXPORT_KEY2(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");

2
gtsam/nonlinear/NonlinearFactor.h
View File

@ -36,7 +36,7 @@
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
// FIXME: is this necessary? These don't even fit the right format // FIXME: is this necessary?
#define INSTANTIATE_NONLINEAR_FACTOR1(C,J) \ #define INSTANTIATE_NONLINEAR_FACTOR1(C,J) \
template class gtsam::NonlinearFactor1<C,J>; template class gtsam::NonlinearFactor1<C,J>;
#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,J2) \ #define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,J2) \