diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index 126a5e131..4956ca4e2 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -26,7 +26,7 @@ #include #include #include -//#include + using namespace std; using namespace gtsam; @@ -104,17 +104,6 @@ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, } } -///* ************************************************************************* */ -//VectorValues GaussianFactorGraph::operator^(const Errors& e) const { -// VectorValues x; -// // For each factor add the gradient contribution -// Errors::const_iterator it = e.begin(); -// BOOST_FOREACH(const sharedFactor& Ai,factors_) { -// VectorValues xi = (*Ai)^(*(it++)); -// x.insertAdd(xi); -// } -// return x; -//} /* ************************************************************************* */ // x += alpha*A'*e diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 307b79d9f..3f2a36173 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -117,9 +117,6 @@ namespace gtsam { /* In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; -// /** return A^e */ -// VectorValues operator^(const Errors& e) const; - /** x += alpha*A'*e */ void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const; @@ -270,28 +267,7 @@ namespace gtsam { dims_[*var] = factor->getDim(var); } } -// for(Index var=0; var= varIndex.dims_.size() || varIndex.dims_[var] == 0) -// assert(dims_[var] != 0); -// else if(varIndex.dims_[var] != 0 && dims_[var] != 0) -// assert(dims_[var] == varIndex.dims_[var]); -//#endif -// if(dims_[var] == 0) -// dims_[var] = varIndex.dims_[var]; -// } } -// /** -// * Returns the augmented matrix version of a set of factors -// * with the corresponding noiseModel -// * @param factors is the set of factors to combine -// * @param ordering of variables needed for matrix column order -// * @return the augmented matrix and a noise model -// */ -// template -// std::pair combineFactorsAndCreateMatrix( -// const FACTORS& factors, -// const Ordering& order, const Dimensions& dimensions); } // namespace gtsam diff --git a/linear/GaussianISAM.cpp b/linear/GaussianISAM.cpp index 1bd1cd73e..1a3ff89aa 100644 --- a/linear/GaussianISAM.cpp +++ b/linear/GaussianISAM.cpp @@ -36,8 +36,6 @@ void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result) { result[cg->key()] = x; // store result in partial solution } BOOST_FOREACH(const GaussianISAM::sharedClique& child, clique->children_) { -// list::const_iterator child; -// for (child = clique->children_.begin(); child != clique->children_.end(); child++) { optimize(child, result); } } diff --git a/linear/NoiseModel.h b/linear/NoiseModel.h index 5402655a6..7033713e2 100644 --- a/linear/NoiseModel.h +++ b/linear/NoiseModel.h @@ -190,9 +190,6 @@ namespace gtsam { }; // Gaussian - // FD: does not work, ambiguous overload :-( - // inline Vector operator*(const Gaussian& R, const Vector& v) {return R.whiten(v);} - /** * A diagonal noise model implements a diagonal covariance matrix, with the * elements of the diagonal specified in a Vector. This class has no public diff --git a/linear/SubgraphPreconditioner.cpp b/linear/SubgraphPreconditioner.cpp index f2afafe95..d8d556d66 100644 --- a/linear/SubgraphPreconditioner.cpp +++ b/linear/SubgraphPreconditioner.cpp @@ -50,17 +50,6 @@ namespace gtsam { /* ************************************************************************* */ double SubgraphPreconditioner::error(const VectorValues& y) const { -// Errors e; -// // Use BayesNet order to add y contributions in order -// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { -// const Symbol& j = cg->key(); -// e.push_back(y[j]); // append y -// } -// // Add A2 contribution -// VectorValues x = this->x(y); -// Errors e2 = Ab2_->errors(x); -// e.splice(e.end(), e2); -// return 0.5 * dot(e, e); Errors e(y); @@ -84,14 +73,6 @@ namespace gtsam { // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { -// Errors e; -// -// // Use BayesNet order to add y contributions in order -// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { -// const Symbol& j = cg->key(); -// e.push_back(y[j]); // append y -// } - Errors e(y); // Add A2 contribution @@ -107,17 +88,6 @@ namespace gtsam { // In-place version that overwrites e void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { -// Errors::iterator ei = e.begin(); -// // Use BayesNet order to add y contributions in order -// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { -// const Symbol& j = cg->key(); -// *ei = y[j]; // append y -// ei++; -// } -// // Add A2 contribution -// VectorValues x = y; // TODO avoid ? -// gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y -// Ab2_->multiplyInPlace(x,ei); // use iterator version Errors::iterator ei = e.begin(); for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) { @@ -134,20 +104,6 @@ namespace gtsam { // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { -// VectorValues y; -// -// // Use BayesNet order to remove y contributions in order -// Errors::const_iterator it = e.begin(); -// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { -// const Symbol& j = cg->key(); -// const Vector& ej = *(it++); -// y.insert(j,ej); -// } -// -// // get A2 part -// transposeMultiplyAdd2(1.0,it,e.end(),y); -// -// return y; Errors::const_iterator it = e.begin(); VectorValues y = zero(); @@ -162,15 +118,6 @@ namespace gtsam { void SubgraphPreconditioner::transposeMultiplyAdd (double alpha, const Errors& e, VectorValues& y) const { -// // Use BayesNet order to remove y contributions in order -// Errors::const_iterator it = e.begin(); -// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { -// const Symbol& j = cg->key(); -// const Vector& ej = *(it++); -// axpy(alpha,ej,y[j]); -// } -// // get A2 part -// transposeMultiplyAdd2(alpha,it,e.end(),y); Errors::const_iterator it = e.begin(); for ( Index i = 0 ; i < y.size() ; ++i, ++it ) { @@ -191,10 +138,6 @@ namespace gtsam { while (it != end) e2.push_back(*(it++)); - // Old code: - // VectorValues x = *Ab2_ ^ e2; // x = A2'*e2 - // y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; - // New Code: VectorValues x = VectorValues::zero(y); // x = 0 Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x diff --git a/linear/SubgraphPreconditioner.h b/linear/SubgraphPreconditioner.h index d49225c77..49fe760ba 100644 --- a/linear/SubgraphPreconditioner.h +++ b/linear/SubgraphPreconditioner.h @@ -55,13 +55,6 @@ namespace gtsam { */ SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedValues& xbar); -// std::pair Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); } -// std::pair Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); } -// Matrix A1(const Ordering& ordering) const { return Ab1_->sparse(ordering); } -// Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(Ab1_->columnIndices(ordering)); } -// Vector b1() const { return Ab1_->rhsVector(); } -// Vector b2() const { return Ab2_->rhsVector(); } -// VectorValues assembleValues(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleValues(v, ordering); } /** * Add zero-mean i.i.d. Gaussian prior terms to each variable diff --git a/linear/VectorBTree.h b/linear/VectorBTree.h index 57bcbe904..0417dab52 100644 --- a/linear/VectorBTree.h +++ b/linear/VectorBTree.h @@ -20,7 +20,6 @@ #pragma once #include -//#include #include #include diff --git a/linear/VectorValues.h b/linear/VectorValues.h index 8c9a9ce3e..a3ee25dfa 100644 --- a/linear/VectorValues.h +++ b/linear/VectorValues.h @@ -45,11 +45,6 @@ public: typedef boost::numeric::ublas::vector_range mapped_type; typedef boost::numeric::ublas::vector_range const_mapped_type; -// /** -// * Constructor requires an existing GaussianVariableIndex to get variable -// * dimensions. -// */ -// VectorValues(const GaussianVariableIndex& variableIndex); /** * Default constructor creates an empty VectorValues. reserve(...) must be @@ -203,15 +198,6 @@ public: }; -//inline VectorValues::VectorValues(const GaussianVariableIndex& variableIndex) : varStarts_(variableIndex.size()+1) { -// size_t varStart = 0; -// varStarts_[0] = 0; -// for(Index var=0; var inline VectorValues::VectorValues(const CONTAINER& dimensions) : varStarts_(dimensions.size()+1) { diff --git a/myconfigure b/myconfigure index 5e1ec71a0..64ed7079c 100755 --- a/myconfigure +++ b/myconfigure @@ -1,4 +1,4 @@ -../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" CPPFLAGS="-D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install +../configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" CPPFLAGS="-D_GLIBCXX_DEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install #../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/usr/local/bin/gfilt" CXXFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -Wall -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g -Wall" --disable-static --enable-blas --enable-lapack --disable-fast-install #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-fno-inline -g -DNDEBUG -O3" CFLAGS="-fno-inline -g -DNDEBUG -O3" LDFLAGS="-fno-inline -g -DNDEBUG -O3" --disable-static --enable-blas --enable-lapack #cd build && ../configure --prefix=$HOME/borg-simplelinear --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-DNDEBUG -O3" CFLAGS="-g -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static --enable-blas --enable-lapack