diff --git a/gtsam/base/FixedVector.h b/gtsam/base/FixedVector.h index ab4a36552..8cb9ac076 100644 --- a/gtsam/base/FixedVector.h +++ b/gtsam/base/FixedVector.h @@ -70,10 +70,6 @@ public: */ inline static FixedVector repeat(double value) { return FixedVector(Base::Constant(value)); -// FixedVector v; -// for (size_t i=0; i qr(const Matrix& A) { * 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 @@ -585,29 +469,6 @@ void householder_(Matrix &A, size_t k) // 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)); } // 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)); for(size_t j=0; j < kprime; j++) A.col(j).segment(j+1, m-(j+1)).setZero(); -// for( size_t i = j+1 ; i < m ; i++ ) -// A(i,j) = 0.0; } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index a0f204ce4..438099b88 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -307,11 +307,6 @@ void inplace_QR(MATRIX& A, bool clear_below_diagonal=true) { 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 * 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); -//#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 * @param U an upper triangular matrix diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 7c526a626..625d767ba 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -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; cout << "not equal:" << endl; print(expected, "expected"); @@ -217,14 +217,14 @@ bool assert_equal(SubVector expected, SubVector actual, double tol) { return false; } -///* ************************************************************************* */ -//bool assert_equal(ConstSubVector expected, ConstSubVector actual, double tol) { -// if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true; -// cout << "not equal:" << endl; -// print(Vector(expected), "expected"); -// print(Vector(actual), "actual"); -// return false; -//} +/* ************************************************************************* */ +bool assert_equal(const ConstSubVector& expected, const ConstSubVector& actual, double tol) { + if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true; + cout << "not equal:" << endl; + print(Vector(expected), "expected"); + print(Vector(actual), "actual"); + return false; +} /* ************************************************************************* */ 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) { 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) { 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) { assert (y.size()==x.size()); 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) { assert (y.size()==x.size()); y += alpha * x; -// size_t n = x.size(); -// for (size_t i = 0; i < n; i++) -// y[i] += alpha * x[i]; } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 6005384f0..aba488e04 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -167,8 +167,8 @@ bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9); * @param tol 1e-9 * @return bool */ -bool assert_equal(SubVector vec1, SubVector vec2, double tol=1e-9); -//bool assert_equal(ConstSubVector vec1, ConstSubVector vec2, double tol=1e-9); +bool assert_equal(const SubVector& vec1, const SubVector& 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 diff --git a/gtsam/base/blockMatrices.h b/gtsam/base/blockMatrices.h index 3523a84d6..377b4db40 100644 --- a/gtsam/base/blockMatrices.h +++ b/gtsam/base/blockMatrices.h @@ -313,11 +313,10 @@ public: typedef Eigen::Block Block; typedef Eigen::Block constBlock; -// typedef typename MATRIX::ColXpr Column; -// typedef typename MATRIX::ConstColXpr constColumn; +private: + static FullMatrix matrixTemp_; // just for finding types protected: - static FullMatrix matrixTemp_; // the reference to the full matrix FullMatrix& matrix_; // the reference to the full matrix std::vector 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) { 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 j_actualBlock = j_block + blockStart_; checkBlock(i_actualBlock); @@ -468,12 +462,6 @@ public: variableColOffsets_[j_actualStartBlock] + columnOffset).segment( 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 { @@ -491,12 +479,6 @@ public: variableColOffsets_[j_actualStartBlock] + columnOffset).segment( 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 { diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index b88b4852b..40158b079 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -25,8 +25,6 @@ #include -//namespace ublas = boost::numeric::ublas; - using namespace std; namespace gtsam { @@ -58,17 +56,12 @@ static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) { ATA(k,k) = beta; if(k < (order-1)) { -// ublas::matrix_row Vf(ublas::row(ATA, k)); -// ublas::vector_range V(ublas::subrange(Vf, k+1,order)); - // Update A(k,k+1:end) <- A(k,k+1:end) / beta typedef typeof(ATA.row(k).segment(k+1, order-(k+1))) BlockRow; BlockRow V = ATA.row(k).segment(k+1, order-(k+1)); V *= betainv; // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha -// ublas::matrix_range 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; } @@ -123,8 +116,6 @@ void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) { const bool debug = ISDEBUG("choleskyPartial"); -// Eigen::Map ABC(&ABCublas(0,0), ABCublas.rows(), ABCublas.cols()); - assert(ABC.rows() == ABC.cols()); assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows())); diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 272a02e99..c0a11257d 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -758,31 +758,6 @@ TEST( matrix, eigen_QR ) 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) // 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); } -/* ************************************************************************* */ -// 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 ) { diff --git a/gtsam/base/tests/timeublas.cpp b/gtsam/base/tests/timeMatrixOps.cpp similarity index 53% rename from gtsam/base/tests/timeublas.cpp rename to gtsam/base/tests/timeMatrixOps.cpp index e05e48341..114968381 100644 --- a/gtsam/base/tests/timeublas.cpp +++ b/gtsam/base/tests/timeMatrixOps.cpp @@ -11,41 +11,47 @@ /** * @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 * @created Sep 18, 2010 */ -#include -#include -#include -#include +//#include +//#include +//#include +//#include #include #include #include #include #include +#include + #include #include #include using namespace std; -namespace ublas = boost::numeric::ublas; +//namespace ublas = boost::numeric::ublas; +//using namespace Eigen; using boost::timer; using boost::format; using namespace boost::lambda; static boost::variate_generator > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0)); -typedef ublas::matrix matrix; -typedef ublas::matrix_range matrix_range; -using ublas::range; -using ublas::triangular_matrix; +//typedef ublas::matrix matrix; +//typedef ublas::matrix_range matrix_range; +typedef Eigen::Matrix matrix; +typedef Eigen::Block matrix_block; + +//using ublas::range; +//using ublas::triangular_matrix; int main(int argc, char* argv[]) { - if(false) { - cout << "\nTiming matrix_range:" << endl; + if(true) { + cout << "\nTiming matrix_block:" << endl; // We use volatile here to make these appear to the optimizing compiler as // if their values are only known at run-time. @@ -56,9 +62,9 @@ int main(int argc, char* argv[]) { boost::variate_generator > rni(boost::mt19937(), boost::uniform_int(0,m-1)); boost::variate_generator > rnj(boost::mt19937(), boost::uniform_int(0,n-1)); matrix mat(m,n); - matrix_range full(mat, range(0,m), range(0,n)); - matrix_range top(mat, range(0,n), range(0,n)); - matrix_range block(mat, range(m/4, m-m/4), range(n/4, n-n/4)); + matrix_block full = mat.block(0, 0, m, n); + matrix_block top = mat.block(0, 0, n, n); + 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(" 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 for(size_t rep=0; rep<1000; ++rep) - for(size_t i=0; i triangular; +//// typedef ublas::matrix 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(mat); +// cout << " Assigned from triangular adapter: " << tri << endl; +// +// cout << " Triangular adapter of mat: " << ublas::triangular_adaptor(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(mat)) = tri; +// cout << " Assign triangular adaptor from triangular: " << mat << endl; +// } - typedef triangular_matrix triangular; - typedef ublas::matrix matrix; +// { +// cout << "\nTesting wide triangular matrices:" << endl; +// +// typedef triangular_matrix triangular; +// typedef ublas::matrix 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(mat); +// cout << " Assigned from triangular adapter: " << tri << endl; +// +// cout << " Triangular adapter of mat: " << ublas::triangular_adaptor(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(mat); +// cout << " Assign matrix from triangular adaptor of self: " << mat << endl; +// } - triangular tri(5,5); - - matrix mat(5,5); - for(size_t j=0; j(mat); - cout << " Assigned from triangular adapter: " << tri << endl; - - cout << " Triangular adapter of mat: " << ublas::triangular_adaptor(mat) << endl; - - for(size_t j=0; j(mat)) = tri; - cout << " Assign triangular adaptor from triangular: " << mat << endl; - } - - { - cout << "\nTesting wide triangular matrices:" << endl; - - typedef triangular_matrix triangular; - typedef ublas::matrix matrix; - - triangular tri(5,7); - - matrix mat(5,7); - for(size_t j=0; j(mat); - cout << " Assigned from triangular adapter: " << tri << endl; - - cout << " Triangular adapter of mat: " << ublas::triangular_adaptor(mat) << endl; - - for(size_t j=0; j(mat); - cout << " Assign matrix from triangular adaptor of self: " << mat << endl; - } - - { - cout << "\nTesting subvectors:" << endl; - - typedef ublas::matrix matrix; - matrix mat(4,4); - - for(size_t j=0; j information(&matrix_(0,0), matrix_.rows(), matrix_.cols()); - // Eigen::Map updateInform(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols()); - // Apply updates to the upper triangle tic(3, "update"); assert(this->info_.nBlocks() - 1 == scatter.size()); @@ -318,8 +297,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt update.print("with: "); } - // Eigen::Map information(&matrix_(0,0), matrix_.rows(), matrix_.cols()); - // Eigen::Map updateAf(&update.matrix_(0,0), update.matrix_.rows(), update.matrix_.cols()); Eigen::Block updateA(update.matrix_.block( 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& key tic(1, "zero"); BlockAb::Block remainingMatrix(Ab.range(0, Ab.nBlocks())); 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 col(ublas::column(remainingMatrix, j)); - // std::fill(col.begin() + j+1, col.end(), 0.0); -// } toc(1, "zero"); } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index ae4c79740..255cb7a2d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -256,11 +256,9 @@ namespace gtsam { j = 0; BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { keys_[j] = sourceSlot.first; - Ab_(j++) = oldAb(sourceSlot.second); -// ublas::noalias(Ab_(j++)) = oldAb(sourceSlot.second); + Ab_(j++).noalias() = oldAb(sourceSlot.second); } - Ab_(j) = oldAb(j); -// ublas::noalias(Ab_(j)) = oldAb(j); + Ab_(j).noalias() = oldAb(j); // Since we're permuting the variables, ensure that entire rows from this // factor are copied when Combine is called @@ -378,6 +376,8 @@ namespace gtsam { if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; 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"); // // Translate the left-most nonzero column indices into top-most zero row indices // vector firstZeroRows(Ab_.cols()); @@ -441,7 +441,6 @@ namespace gtsam { size_t varDim = Ab_(0).cols(); Ab_.rowEnd() = Ab_.rowStart() + varDim; const Eigen::VectorBlock sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); -// const ublas::vector_range sigmas(noiseModel->sigmas(), ublas::range(Ab_.rowStart(), Ab_.rowEnd())); conditionals->push_back(boost::make_shared(begin()+j, end(), 1, Ab_, sigmas)); if(debug) conditionals->back()->print("Extracted conditional: "); Ab_.rowStart() += varDim; @@ -521,14 +520,11 @@ namespace gtsam { if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) { const constABlock sourceBlock(source.Ab_(sourceSlot)); combinedBlock.row(row).noalias() = sourceBlock.row(sourceRow); -// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow); } else { combinedBlock.row(row).setZero(); -// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector(combinedBlock.cols()); } } else { combinedBlock.row(row).setZero(); -// ublas::noalias(ublas::row(combinedBlock, row)) = ublas::zero_vector(combinedBlock.cols()); } } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index ddb44fe06..81c932d1b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -35,16 +35,7 @@ static double inf = std::numeric_limits::infinity(); 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 noiseModel { /* ************************************************************************* */ @@ -57,15 +48,6 @@ template void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { size_t n = Ab.cols()-1; 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); -// } } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 28c6860c3..bec11bfda 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -504,14 +504,4 @@ namespace gtsam { } // namespace gtsam -/** Export keys for serialization */ -// FIXME: doesn't actually work outside of the library -//#include -//#include -//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"); - diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 8242bbfa8..1a64aa42b 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -36,7 +36,7 @@ #include -// FIXME: is this necessary? These don't even fit the right format +// FIXME: is this necessary? #define INSTANTIATE_NONLINEAR_FACTOR1(C,J) \ template class gtsam::NonlinearFactor1; #define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,J2) \