From 3247751b5db8193b5fbd924b66b1e48325db7e7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Feb 2010 03:29:12 +0000 Subject: [PATCH] Major check-in: there are now two interchangeable implementations of VectorConfig. VectorMap uses a straightforward stl::map of Vectors. It has O(log n) insert and access, and is fairly fast at both. However, it has high overhead for arithmetic operations such as +, scale, axpy etc... VectorBTree uses a functional BTree as a way to access SubVectors in an ordinary Vector. Inserting is O(n) and much slower, but accessing, is O(log n) and might be a bit slower than VectorMap. Arithmetic operations are blindingly fast, however. The cost is it is not as KISS as VectorMap. Access to vectors is now exclusively via operator[] Vector access in VectorMap is via a Vector reference Vector access in VectorBtree is via the SubVector type (see Vector.h) Feb 16 2010: FD: I made VectorMap the default, because I decided to try and speed up conjugate gradients by using Sparse FactorGraphs all the way. --- .cproject | 15 +- .project | 2 +- cpp/BTree.h | 36 +- cpp/BayesNetPreconditioner.cpp | 9 +- cpp/BayesTree-inl.h | 4 +- cpp/GaussianBayesNet.cpp | 23 +- cpp/GaussianFactor.cpp | 27 +- cpp/GaussianFactor.h | 7 - cpp/GaussianFactorGraph.cpp | 33 +- cpp/GaussianFactorGraph.h | 8 - cpp/GaussianISAM.cpp | 2 +- cpp/GaussianISAM2.cpp | 2 +- cpp/LieConfig.h | 1 + cpp/Makefile.am | 20 +- cpp/Matrix.cpp | 2 +- cpp/Matrix.h | 2 +- cpp/Pose3.cpp | 8 +- cpp/SubgraphPreconditioner-inl.h | 4 +- cpp/SubgraphPreconditioner.cpp | 47 ++- cpp/SubgraphPreconditioner.h | 18 +- cpp/Vector.cpp | 22 +- cpp/Vector.h | 13 +- cpp/VectorBTree.cpp | 208 ++++++++++++ cpp/VectorBTree.h | 258 +++++++++++++++ cpp/VectorConfig.h | 180 ++-------- cpp/{VectorConfig.cpp => VectorMap.cpp} | 166 +++++----- cpp/VectorMap.h | 165 ++++++++++ cpp/testBayesNetPreconditioner.cpp | 19 +- cpp/testGaussianFactor.cpp | 21 +- cpp/testGaussianFactorGraph.cpp | 2 +- cpp/testIterative.cpp | 11 +- cpp/testNonlinearOptimizer.cpp | 33 +- cpp/testPose2Factor.cpp | 3 +- cpp/testSubgraphPreconditioner.cpp | 69 ++-- cpp/testVectorBTree.cpp | 310 ++++++++++++++++++ ...testVectorConfig.cpp => testVectorMap.cpp} | 131 ++++---- cpp/timeVectorConfig.cpp | 76 ++--- 37 files changed, 1385 insertions(+), 572 deletions(-) create mode 100644 cpp/VectorBTree.cpp create mode 100644 cpp/VectorBTree.h rename cpp/{VectorConfig.cpp => VectorMap.cpp} (53%) create mode 100644 cpp/VectorMap.h create mode 100644 cpp/testVectorBTree.cpp rename cpp/{testVectorConfig.cpp => testVectorMap.cpp} (66%) diff --git a/.cproject b/.cproject index 53fe228ba..2fd07acfc 100644 --- a/.cproject +++ b/.cproject @@ -20,7 +20,7 @@ - + @@ -469,6 +469,7 @@ make + testBayesTree.run true false @@ -476,7 +477,6 @@ make - testSymbolicBayesNet.run true false @@ -484,6 +484,7 @@ make + testSymbolicFactorGraph.run true false @@ -699,6 +700,7 @@ make + testGraph.run true false @@ -754,7 +756,6 @@ make - testSimulated2D.run true false @@ -792,6 +793,14 @@ true true + +make + +testBTree.run +true +false +true + make -j2 diff --git a/.project b/.project index 893b1174b..c10203bc2 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j2 + org.eclipse.cdt.make.core.buildCommand diff --git a/cpp/BTree.h b/cpp/BTree.h index 9cb3a2363..862501bcd 100644 --- a/cpp/BTree.h +++ b/cpp/BTree.h @@ -29,9 +29,9 @@ namespace gtsam { */ struct Node { - size_t height_; + const size_t height_; const value_type keyValue_; - BTree left, right; + const BTree left, right; /** default constructor */ Node() { @@ -48,9 +48,8 @@ namespace gtsam { * Create a node from two subtrees and a key value pair */ Node(const BTree& l, const value_type& keyValue, const BTree& r) : - left(l), keyValue_(keyValue), right(r) { - size_t hl = l.height(), hr = r.height(); - height_ = hl >= hr ? hl + 1 : hr + 1; + left(l), keyValue_(keyValue), right(r), + height_(l.height() >= r.height() ? l.height() + 1 : r.height() + 1) { } inline const Key& key() const { return keyValue_.first;} @@ -60,7 +59,7 @@ namespace gtsam { // We store a shared pointer to the root of the functional tree // composed of Node classes. If root_==NULL, the tree is empty. - typedef boost::shared_ptr sharedNode; + typedef boost::shared_ptr sharedNode; sharedNode root_; inline const value_type& keyValue() const { return root_->keyValue_;} @@ -100,6 +99,11 @@ namespace gtsam { BTree() { } + /** copy constructor */ + BTree(const BTree& other) : + root_(other.root_) { + } + /** create leaf from key-value pair */ BTree(const value_type& keyValue) : root_(new Node(keyValue)) { @@ -208,25 +212,12 @@ namespace gtsam { return left().size() + 1 + right().size(); } - -#ifdef RECURSIVE_FIND - /** find a value given a key, throws exception when not found */ - const Value& find(const Key& k) const { - if (!root_) throw std::invalid_argument("BTree::find: key '" - + (std::string) k + "' not found"); - const Node& node = *root_; - const Key& key = node.key(); - if (k < key) return node.left.find(k); - if (key < k) return node.right.find(k); - return node.value(); // (key() == k) - } -#else /** * 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 { - Node* node = root_.get(); + const Node* node = root_.get(); while (node) { const Key& key = node->key(); if (k < key) node = node->left.root_.get(); @@ -235,7 +226,6 @@ namespace gtsam { } throw std::invalid_argument("BTree::find: key '" + (std::string) k + "' not found"); } -#endif /** print in-order */ void print(const std::string& s = "") const { @@ -317,7 +307,7 @@ namespace gtsam { path_.top().second = true; // flag we visited right // push right root and its left-most path onto the stack while (t) { - path_.push(make_pair(t, false)); + path_.push(std::make_pair(t, false)); t = t->left.root_; } } @@ -340,7 +330,7 @@ namespace gtsam { const_iterator(const sharedNode& root) { sharedNode t = root; while (t) { - path_.push(make_pair(t, false)); + path_.push(std::make_pair(t, false)); t = t->left.root_; } } diff --git a/cpp/BayesNetPreconditioner.cpp b/cpp/BayesNetPreconditioner.cpp index abe87086f..125903fc2 100644 --- a/cpp/BayesNetPreconditioner.cpp +++ b/cpp/BayesNetPreconditioner.cpp @@ -36,7 +36,9 @@ namespace gtsam { /* ************************************************************************* */ // gradient is inv(R')*A'*(A*inv(R)*y-b), VectorConfig BayesNetPreconditioner::gradient(const VectorConfig& y) const { - VectorConfig gx = Ab_ ^ Ab_.errors(x(y)); + VectorConfig gx = VectorConfig::zero(y); + Errors e = Ab_.errors(x(y)); + Ab_.transposeMultiplyAdd(1.0,e,gx); return gtsam::backSubstituteTranspose(Rd_, gx); } @@ -66,8 +68,9 @@ namespace gtsam { // y += alpha*inv(R')*A'*e void BayesNetPreconditioner::transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& y) const { - VectorConfig x = Ab_ ^ e; // x = A'*e2 - y += alpha * gtsam::backSubstituteTranspose(Rd_, x); // TODO avoid temp + VectorConfig x = VectorConfig::zero(y); + Ab_.transposeMultiplyAdd(1.0,e,x); // x += A'*e + axpy(alpha, gtsam::backSubstituteTranspose(Rd_, x), y); // y += alpha*inv(R')*x } /* ************************************************************************* */ diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 10e194b3a..94980879f 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -60,7 +60,7 @@ namespace gtsam { template size_t BayesTree::Clique::treeSize() const { size_t size = 1; - BOOST_FOREACH(shared_ptr child, children_) + BOOST_FOREACH(const shared_ptr& child, children_) size += child->treeSize(); return size; } @@ -69,7 +69,7 @@ namespace gtsam { template void BayesTree::Clique::printTree(const string& indent) const { print(indent); - BOOST_FOREACH(shared_ptr child, children_) + BOOST_FOREACH(const shared_ptr& child, children_) child->printTree(indent+" "); } diff --git a/cpp/GaussianBayesNet.cpp b/cpp/GaussianBayesNet.cpp index 91d488894..190b5d22f 100644 --- a/cpp/GaussianBayesNet.cpp +++ b/cpp/GaussianBayesNet.cpp @@ -98,11 +98,9 @@ void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y) { for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) { const Symbol& j = it->first; const Matrix& Rij = it->second; - Vector& xj = x.getReference(j); - multiplyAdd(-1.0,Rij,xj,zi); + multiplyAdd(-1.0,Rij,x[j],zi); } - Vector& xi = x.getReference(i); - xi = gtsam::backSubstituteUpper(cg->get_R(), zi); + x[i] = gtsam::backSubstituteUpper(cg->get_R(), zi); } } @@ -114,33 +112,26 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn, const VectorConfig& gx) { // Initialize gy from gx - VectorConfig gy; - BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) { - const Symbol& j = cg->key(); - Vector gyj = gx.contains(j) ? gx[j] : zero(cg->dim()); - gy.insert(j,gyj); // initialize result - } + // TODO: used to insert zeros if gx did not have an entry for a variable in bn + VectorConfig gy = gx; // we loop from first-eliminated to last-eliminated // i^th part of L*gy=gx is done block-column by block-column of L BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) { const Symbol& j = cg->key(); - Vector& gyj = gy.getReference(j); // should never fail - gyj = gtsam::backSubstituteUpper(gyj,cg->get_R()); + gy[j] = gtsam::backSubstituteUpper(gy[j],cg->get_R()); GaussianConditional::const_iterator it; for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) { const Symbol& i = it->first; const Matrix& Rij = it->second; - Vector& gyi = gy.getReference(i); // should never fail - transposeMultiplyAdd(-1.0,Rij,gyj,gyi); + transposeMultiplyAdd(-1.0,Rij,gy[j],gy[i]); } } // Scale gy BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) { const Symbol& j = cg->key(); - Vector& gyj = gy.getReference(j); // should never fail - gyj = emul(gyj,cg->get_sigmas()); + gy[j] = emul(gy[j],cg->get_sigmas()); } return gy; diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index 1ed569239..3b410ec77 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -215,10 +215,7 @@ void GaussianFactor::transposeMultiplyAdd(double alpha, const Vector& e, Vector E = alpha * model_->whiten(e); // Just iterate over all A matrices and insert Ai^e into VectorConfig FOREACH_PAIR(j, Aj, As_) - { - Vector& Xj = x.getReference(*j); - gtsam::transposeMultiplyAdd(1.0, *Aj, E, Xj); - } + gtsam::transposeMultiplyAdd(1.0, *Aj, E, x[*j]); } /* ************************************************************************* */ @@ -481,28 +478,6 @@ GaussianFactor::eliminate(const Symbol& key) const return eliminateMatrix(Ab, model_, ordering, dimensions()); } -/* ************************************************************************* */ -// Creates a factor on step-size, given initial estimate and direction d, e.g. -// Factor |A1*x+A2*y-b|/sigma -> |A1*(x0+alpha*dx)+A2*(y0+alpha*dy)-b|/sigma -// -> |(A1*dx+A2*dy)*alpha-(b-A1*x0-A2*y0)|/sigma -/* ************************************************************************* */ -GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const Symbol& key, const VectorConfig& x, - const VectorConfig& d) const { - - // Calculate A matrix - size_t m = b_.size(); - Vector A = zero(m); - FOREACH_PAIR(j, Aj, As_) - A += *Aj * d[*j]; - - // calculate the value of the factor for RHS - Vector b = - unweighted_error(x); - - // construct factor - shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,model_)); - return factor; -} - /* ************************************************************************* */ namespace gtsam { diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 093001ee8..ae0d3c385 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -230,13 +230,6 @@ public: boost::tuple, std::list, std::list > sparse(const Dimensions& columnIndices) const; - /** - * Create a GaussianFactor on one variable 'alpha' (step size), in direction d - * @param x: starting point for search - * @param d: search direction - */ - shared_ptr alphaFactor(const Symbol& key, const VectorConfig& x, const VectorConfig& d) const; - /* ************************************************************************* */ // MUTABLE functions. FD:on the path to being eradicated /* ************************************************************************* */ diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 4947deb05..09843308d 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -86,8 +86,10 @@ VectorConfig GaussianFactorGraph::operator^(const Errors& e) const { VectorConfig x; // For each factor add the gradient contribution Errors::const_iterator it = e.begin(); - BOOST_FOREACH(sharedFactor Ai,factors_) - x += (*Ai)^(*(it++)); + BOOST_FOREACH(sharedFactor Ai,factors_) { + VectorConfig xi = (*Ai)^(*(it++)); + x.insertAdd(xi); + } return x; } @@ -103,8 +105,10 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, /* ************************************************************************* */ VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const { - const GaussianFactorGraph& A = *this; - return A^errors(x); + // It is crucial for performance to make a zero-valued clone of x + VectorConfig g = VectorConfig::zero(x); + transposeMultiplyAdd(1.0, errors(x), g); + return g; } /* ************************************************************************* */ @@ -389,27 +393,6 @@ Matrix GaussianFactorGraph::sparse(const Dimensions& indices) const { return ijs; } -/* ************************************************************************* */ -VectorConfig GaussianFactorGraph::optimalUpdate(const VectorConfig& x, - const VectorConfig& d) const { - - // create a new graph on step-size - GaussianFactorGraph alphaGraph; - Symbol alphaKey('\224', 1); - BOOST_FOREACH(sharedFactor factor,factors_) { - sharedFactor alphaFactor = factor->alphaFactor(alphaKey, x,d); - alphaGraph.push_back(alphaFactor); - } - - // solve it for optimal step-size alpha - GaussianConditional::shared_ptr gc = alphaGraph.eliminateOne(alphaKey); - double alpha = gc->get_d()(0); - cout << alpha << endl; - - // return updated estimate by stepping in direction d - return expmap(x, d.scale(alpha)); -} - /* ************************************************************************* */ VectorConfig GaussianFactorGraph::steepestDescent(const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const { diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 8583f0163..eeffc37ce 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -224,14 +224,6 @@ namespace gtsam { */ Matrix sparse(const Dimensions& indices) const; - - /** - * Take an optimal step in direction d by calculating optimal step-size - * @param x: starting point for search - * @param d: search direction - */ - VectorConfig optimalUpdate(const VectorConfig& x0, const VectorConfig& d) const; - /** * Find solution using gradient descent * @param x0: VectorConfig specifying initial estimate diff --git a/cpp/GaussianISAM.cpp b/cpp/GaussianISAM.cpp index aa7ff3026..bce605142 100644 --- a/cpp/GaussianISAM.cpp +++ b/cpp/GaussianISAM.cpp @@ -24,7 +24,7 @@ void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result) { Vector x = cg->solve(result); // Solve for that variable result.insert(cg->key(), x); // store result in partial solution } - BOOST_FOREACH(GaussianISAM::sharedClique child, clique->children_) { + 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/cpp/GaussianISAM2.cpp b/cpp/GaussianISAM2.cpp index 6fcd7b3a3..111336ff2 100644 --- a/cpp/GaussianISAM2.cpp +++ b/cpp/GaussianISAM2.cpp @@ -31,7 +31,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, Vect result.insert(cg->key(), x); // store result in partial solution } if (process_children) { - BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) { + BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { optimize2(child, threshold, result); } } diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index 4372d8a10..03f613ede 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -13,6 +13,7 @@ #include "Vector.h" #include "Testable.h" +#include "VectorConfig.h" namespace boost { template class optional; } namespace gtsam { class VectorConfig; } diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 457d107cd..f898768db 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -40,7 +40,13 @@ testMatrix_LDADD = libgtsam.la # The header files will be installed in ~/include/gtsam headers = gtsam.h Value.h Testable.h Factor.h Conditional.h headers += Ordering.h IndexTable.h numericalDerivative.h +headers += BTree.h sources += Ordering.cpp smallExample.cpp +check_PROGRAMS += testOrdering testBTree +testOrdering_SOURCES = testOrdering.cpp +testOrdering_LDADD = libgtsam.la +testBTree_SOURCES = testBTree.cpp +testBTree_LDADD = libgtsam.la # Symbolic Inference headers += SymbolicConditional.h @@ -70,8 +76,6 @@ testFactorgraph_SOURCES = testFactorgraph.cpp testInference_SOURCES = testInference.cpp testFactorgraph_LDADD = libgtsam.la testInference_LDADD = libgtsam.la -testOrdering_SOURCES = testOrdering.cpp -testOrdering_LDADD = libgtsam.la testBayesTree_SOURCES = testBayesTree.cpp testBayesTree_LDADD = libgtsam.la testGaussianISAM_SOURCES = testGaussianISAM.cpp @@ -88,11 +92,13 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp testBinaryBayesNet_LDADD = libgtsam.la # Gaussian inference -headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h -sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp -check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel -testVectorConfig_SOURCES = testVectorConfig.cpp -testVectorConfig_LDADD = libgtsam.la +headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h VectorConfig.h +sources += NoiseModel.cpp Errors.cpp VectorMap.cpp VectorBTree.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp +check_PROGRAMS += testVectorMap testVectorBTree testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel +testVectorMap_SOURCES = testVectorMap.cpp +testVectorMap_LDADD = libgtsam.la +testVectorBTree_SOURCES = testVectorBTree.cpp +testVectorBTree_LDADD = libgtsam.la testGaussianFactor_SOURCES = testGaussianFactor.cpp testGaussianFactor_LDADD = libgtsam.la testGaussianFactorGraph_SOURCES = testGaussianFactorGraph.cpp diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index ad5f4a0f4..ade536bff 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -187,7 +187,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector } /* ************************************************************************* */ -void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector& x) { +void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { // ublas x += prod(trans(A),e) is terribly slow // TODO: use BLAS size_t m = A.size1(), n = A.size2(); diff --git a/cpp/Matrix.h b/cpp/Matrix.h index 0538a3cfe..56764650f 100644 --- a/cpp/Matrix.h +++ b/cpp/Matrix.h @@ -105,7 +105,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector /** * BLAS Level-2 style x <- x + alpha*A'*e */ -void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector& x); +void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x); /** * overload * for vector*matrix multiplication (as BOOST does not) diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index f75598c32..aee7cb5ea 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -81,7 +81,7 @@ namespace gtsam { const Matrix R = R_.matrix(), T = Matrix_(3,1, t_.vector()); const Matrix A34 = collect(2, &R, &T); const Matrix A14 = Matrix_(1,4, 0.0, 0.0, 0.0, 1.0); - return stack(2, &A34, &A14); + return gtsam::stack(2, &A34, &A14); } /* ************************************************************************* */ @@ -140,7 +140,7 @@ namespace gtsam { Matrix DR_t1 = zeros(3, 3); Matrix DR = collect(2, &DR_R1, &DR_t1); Matrix Dt = Dtransform_from1(p1, p2.translation()); - return stack(2, &DR, &Dt); + return gtsam::stack(2, &DR, &Dt); } Matrix Dcompose2(const Pose3& p1, const Pose3& p2) { @@ -149,7 +149,7 @@ namespace gtsam { const static Matrix Z3 = zeros(3, 3); Matrix DR = collect(2, &I, &Z3); Matrix Dt = collect(2, &Z3, &R1); - return stack(2, &DR, &Dt); + return gtsam::stack(2, &DR, &Dt); } /* ************************************************************************* */ @@ -162,7 +162,7 @@ namespace gtsam { Matrix Dt_R1 = -skewSymmetric(unrotate(p.rotation(),p.translation()).vector()); Matrix Dt_t1 = -Rt; Matrix Dt = collect(2, &Dt_R1, &Dt_t1); - return stack(2, &DR, &Dt); + return gtsam::stack(2, &DR, &Dt); } /* ************************************************************************* */ diff --git a/cpp/SubgraphPreconditioner-inl.h b/cpp/SubgraphPreconditioner-inl.h index 425d48b75..bf6f1f56c 100644 --- a/cpp/SubgraphPreconditioner-inl.h +++ b/cpp/SubgraphPreconditioner-inl.h @@ -68,9 +68,7 @@ namespace gtsam { /* ************************************************************************* */ template VectorConfig SubgraphPCG::optimize(SubgraphPreconditioner& system) const { - //TODO: 3 is hard coded here - VectorConfig zeros; - BOOST_FOREACH(const Symbol& j, *ordering_) zeros.insert(j,zero(Pose::dim())); + VectorConfig zeros = system.zero(); // Solve the subgraph PCG VectorConfig ybar = conjugateGradientserrors_(*xbar)) { } /* ************************************************************************* */ // x = xbar + inv(R1)*y VectorConfig SubgraphPreconditioner::x(const VectorConfig& y) const { +#ifdef VECTORBTREE + if (!y.cloned(*xbar_)) throw + invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar"); +#endif VectorConfig x = y; backSubstituteInPlace(*Rc1_,x); x += *xbar_; @@ -48,7 +53,9 @@ namespace gtsam { // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), VectorConfig SubgraphPreconditioner::gradient(const VectorConfig& y) const { VectorConfig x = this->x(y); // x = inv(R1)*y - VectorConfig gx2 = *Ab2_ ^ Ab2_->errors(x); + Errors e2 = Ab2_->errors(x); + VectorConfig gx2 = VectorConfig::zero(y); + Ab2_->transposeMultiplyAdd(1.0,e2,gx2); // A2'*e2; VectorConfig gy2 = gtsam::backSubstituteTranspose(*Rc1_, gx2); // inv(R1')*gx2 return y + gy2; } @@ -107,21 +114,14 @@ namespace gtsam { y.insert(j,ej); } - // create e2 with what's left of e - Errors e2; - while (it != e.end()) - e2.push_back(*(it++)); - - // get A2 part, - VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2 - y += gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; + // get A2 part + transposeMultiplyAdd2(1.0,it,e.end(),y); return y; } /* ************************************************************************* */ // y += alpha*A'*e - // TODO avoid code duplication void SubgraphPreconditioner::transposeMultiplyAdd (double alpha, const Errors& e, VectorConfig& y) const { @@ -130,18 +130,31 @@ namespace gtsam { BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { const Symbol& j = cg->key(); const Vector& ej = *(it++); - Vector& yj = y.getReference(j); - axpy(alpha,ej,yj); + axpy(alpha,ej,y[j]); } + // get A2 part + transposeMultiplyAdd2(alpha,it,e.end(),y); + } + + /* ************************************************************************* */ + // y += alpha*inv(R1')*A2'*e2 + void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, + Errors::const_iterator it, Errors::const_iterator end, VectorConfig& y) const { + // create e2 with what's left of e + // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? Errors e2; - while (it != e.end()) + while (it != end) e2.push_back(*(it++)); - // get A2 part, - VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2 - y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; + // Old code: + // VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2 + // y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; + // New Code: + VectorConfig x = VectorConfig::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/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index 02603edba..8fe158e61 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/cpp/SubgraphPreconditioner.h @@ -55,6 +55,9 @@ namespace gtsam { /* x = xbar + inv(R1)*y */ VectorConfig x(const VectorConfig& y) const; + /* A zero VectorConfig with the structure of xbar */ + VectorConfig zero() const { return VectorConfig::zero(*xbar_);} + /* error, given y */ double error(const VectorConfig& y) const; @@ -70,10 +73,21 @@ namespace gtsam { /** Apply operator A' */ VectorConfig operator^(const Errors& e) const; - /** y += alpha*A'*e */ + /** + * Add A'*e to y + * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] + */ void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& y) const; - /** print the object */ + /** + * Add constraint part of the error only, used in both calls above + * y += alpha*inv(R1')*A2'*e2 + * Takes a range indicating e2 !!!! + */ + void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, + Errors::const_iterator end, VectorConfig& y) const; + + /** print the object */ void print(const std::string& s = "SubgraphPreconditioner") const; }; diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp index 29afb2b6c..89b1ed767 100644 --- a/cpp/Vector.cpp +++ b/cpp/Vector.cpp @@ -152,7 +152,25 @@ namespace gtsam { print(actual, "actual"); return false; } - + + /* ************************************************************************* */ + bool assert_equal(SubVector expected, SubVector actual, double tol) { + if (equal_with_abs_tol(expected,actual,tol)) return true; + cout << "not equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; + } + + /* ************************************************************************* */ + bool assert_equal(ConstSubVector expected, ConstSubVector actual, double tol) { + if (equal_with_abs_tol(expected,actual,tol)) return true; + cout << "not equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; + } + /* ************************************************************************* */ Vector sub(const Vector &v, size_t i1, size_t i2) { size_t n = i2-i1; @@ -263,7 +281,7 @@ namespace gtsam { } /* ************************************************************************* */ - void axpy(double alpha, const Vector& x, SubVector& y) { + void axpy(double alpha, const Vector& x, SubVector y) { size_t n = x.size(); assert (y.size()==n); for (size_t i = 0; i < n; i++) diff --git a/cpp/Vector.h b/cpp/Vector.h index 1fe332f73..a06d573c1 100644 --- a/cpp/Vector.h +++ b/cpp/Vector.h @@ -20,6 +20,7 @@ typedef boost::numeric::ublas::vector Vector; #endif typedef boost::numeric::ublas::vector_range SubVector; +typedef boost::numeric::ublas::vector_range ConstSubVector; namespace gtsam { @@ -132,6 +133,16 @@ inline bool equal(const Vector& vec1, const Vector& vec2) { */ bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9); +/** + * Same, prints if error + * @param vec1 Vector + * @param vec2 Vector + * @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); + /** * extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2 * @param v Vector @@ -220,7 +231,7 @@ void scal(double alpha, Vector& x); * BLAS Level 1 axpy: y <- alpha*x + y */ void axpy(double alpha, const Vector& x, Vector& y); -void axpy(double alpha, const Vector& x, SubVector& y); +void axpy(double alpha, const Vector& x, SubVector y); /** * Divide every element of a Vector into a scalar diff --git a/cpp/VectorBTree.cpp b/cpp/VectorBTree.cpp new file mode 100644 index 000000000..40d7c0286 --- /dev/null +++ b/cpp/VectorBTree.cpp @@ -0,0 +1,208 @@ +/** + * @file VectorConfig.cpp + * @brief Factor Graph Configuration + * @brief VectorConfig + * @author Frank Dellaert + */ + +#include +#include +#include "VectorBTree.h" + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + // Check if two VectorConfigs are compatible, throw exception if not + static void check_compatible(const string& s, const VectorBTree& a, + const VectorBTree& b) { + if (!a.compatible(b)) + throw invalid_argument(s + ": incompatible VectorBTrees"); + } + + /* ************************************************************************* */ + // Check if two VectorBTrees are cloned, throw exception if not + // The 2 configs need exactly the same ranges tree + static void check_cloned(const string& s, const VectorBTree& a, + const VectorBTree& b) { + if (!a.cloned(b)) + throw invalid_argument(s + ": only defined for cloned VectorBTrees"); + } + + /* ************************************************************************* */ + void VectorBTree::print(const string& name) const { + odprintf("VectorBTree %s\n", name.c_str()); + odprintf("size: %d\n", size_); + BOOST_FOREACH(const Pair& p, ranges_) { + const Range& r = p.second; + odprintf("%s: %d:%d", string(p.first).c_str(), r.start(), r.size()); + gtsam::print(get(r)); + } + } + + /* ************************************************************************* */ + bool VectorBTree::equals(const VectorBTree& expected, double tol) const { + BOOST_FOREACH(const Pair& p, ranges_) { + const Symbol& j = p.first; + if (!expected.contains(j)) return false; + if (!equal_with_abs_tol(expected[j], get(p.second), tol)) + return false; + } + return true; + } + + /* ************************************************************************* */ + VectorBTree& VectorBTree::insert(const Symbol& j, const Vector& v) { + if (contains(j)) throw invalid_argument("VectorBTree::insert: key already exists"); + // calculate new range from current dimensionality and dim(a) + size_t n1 = values_.size(), n2 = n1 + v.size(); + ranges_ = ranges_.add(j,Range(n1,n2)); + // resize vector and COPY a into it + values_.resize(n2,true); + std::copy(v.begin(),v.end(),values_.begin()+n1); + // increment size + ++size_; + return *this; + } + + /* ************************************************************************* */ + VectorBTree& VectorBTree::insertAdd(const Symbol& j, const Vector& v) { + if (!contains(j)) return insert(j,v); + const Range& r = ranges_.find(j); + SubVector(values_,r) += v; + return *this; + } + + /* ************************************************************************* */ + void VectorBTree::insert(const VectorBTree& config) { + BOOST_FOREACH(const Pair& p, config.ranges_) + insert(p.first,config.get(p.second)); + } + + /* ************************************************************************* */ + void VectorBTree::insertAdd(const VectorBTree& config) { + BOOST_FOREACH(const Pair& p, config.ranges_) + insertAdd(p.first,config.get(p.second)); + } + + /* ************************************************************************* */ + std::vector VectorBTree::get_names() const { + std::vector names; + BOOST_FOREACH(const Pair& p, ranges_) + names.push_back(p.first); + return names; + } + + /* ************************************************************************* */ + SubVector VectorBTree::operator[](const Symbol& j) { + const Range& r = ranges_.find(j); + return SubVector(values_,r); + } + + /* ************************************************************************* */ + ConstSubVector VectorBTree::operator[](const Symbol& j) const { + const Range& r = ranges_.find(j); + return ConstSubVector(values_,r); + } + + /* ************************************************************************* */ + double VectorBTree::max() const { + double m = -std::numeric_limits::infinity(); + BOOST_FOREACH(const Pair& p, ranges_) + m = std::max(m, gtsam::max(get(p.second))); + return m; + } + + /* ************************************************************************* */ + VectorBTree VectorBTree::scale(double s) const { + VectorBTree scaled = *this; + scaled.values_ *= s; + return scaled; + } + + /* ************************************************************************* */ + VectorBTree VectorBTree::operator*(double s) const { + return scale(s); + } + + /* ************************************************************************* */ + VectorBTree VectorBTree::operator-() const { + VectorBTree result = *this; + result.values_ = - values_; + return result; + } + + /* ************************************************************************* */ + void VectorBTree::operator+=(const VectorBTree& b) { + check_compatible("VectorBTree:operator+=", *this, b); + values_ += b.values_; + } + + /* ************************************************************************* */ + VectorBTree VectorBTree::operator+(const VectorBTree& b) const { + check_compatible("VectorBTree:operator+", *this, b); + VectorBTree result = *this; + result += b; + return result; + } + + /* ************************************************************************* */ + void VectorBTree::operator-=(const VectorBTree& b) { + check_compatible("VectorBTree:operator-=", *this, b); + values_ -= b.values_; + } + + /* ************************************************************************* */ + VectorBTree VectorBTree::operator-(const VectorBTree& b) const { + VectorBTree result = *this; + result -= b; + return result; + } + + /* ************************************************************************* */ + double VectorBTree::dot(const VectorBTree& b) const { + check_compatible("VectorBTree:dot", *this, b); + return gtsam::dot(values_,b.values_); + } + + /* ************************************************************************* */ + VectorBTree& VectorBTree::zero() { + std::fill(values_.begin(), values_.end(), 0.0); + return *this; + } + + /* ************************************************************************* */ + VectorBTree VectorBTree::zero(const VectorBTree& x) { + VectorBTree cloned(x); + return cloned.zero(); + } + + /* ************************************************************************* */ + VectorBTree expmap(const VectorBTree& original, const VectorBTree& delta) { + check_compatible("VectorBTree:expmap", original, delta); + return original + delta; + } + + /* ************************************************************************* */ + VectorBTree expmap(const VectorBTree& original, const Vector& delta) + { + VectorBTree result = original; + result.values_ += delta; + return result; + } + + /* ************************************************************************* */ + void scal(double alpha, VectorBTree& x) { + scal(alpha, x.values_); + } + + /* ************************************************************************* */ + void axpy(double alpha, const VectorBTree& x, VectorBTree& y) { + check_cloned("VectorBTree:axpy", x, y); + axpy(alpha, x.values_, y.values_); + } + + /* ************************************************************************* */ + +} // gtsam diff --git a/cpp/VectorBTree.h b/cpp/VectorBTree.h new file mode 100644 index 000000000..0ce5767c4 --- /dev/null +++ b/cpp/VectorBTree.h @@ -0,0 +1,258 @@ +/** + * @file VectorBTree.h + * @brief Factor Graph Configuration + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include +#include + +#include "Testable.h" +#include "Vector.h" +#include "Key.h" +#include "BTree.h" + +namespace gtsam { + + /** Factor Graph Configuration */ + class VectorBTree: public Testable { + + private: + + /** dictionary from Symbol to Range */ + typedef boost::numeric::ublas::range Range; + typedef BTree Ranges; + typedef Ranges::value_type Pair; + Ranges ranges_; + + /** Actual vector */ + Vector values_; + + /** size_ is number of vectors */ + size_t size_; + + /** private get from symbol pair */ + Vector get(const Range& r) const { + return sub(values_,r.start(),r.start()+r.size()); + } + +public: + + /** + * Default constructor + */ + VectorBTree() : + size_(0) { + } + + /** + * Copy constructor + */ + VectorBTree(const VectorBTree& c) : + ranges_(c.ranges_), values_(c.values_), size_(c.size_) { + } + + /** + * Construct with a single vector + */ + VectorBTree(const Symbol& j, const Vector& a) : + size_(0) { + insert(j, a); + } + + virtual ~VectorBTree() { + } + + /** print */ + void print(const std::string& name = "") const; + + /** equals, for unit testing */ + bool equals(const VectorBTree& expected, double tol = 1e-9) const; + + /** Insert a value into the configuration with a given index: O(n) */ + VectorBTree& insert(const Symbol& j, const Vector& v); + + /** Insert or add a value with given index: O(n) if does not exist */ + VectorBTree& insertAdd(const Symbol& j, const Vector& v); + + /** Insert a config into another config, replace if key already exists */ + void insert(const VectorBTree& config); + + /** Insert a config into another config, add if key already exists */ + void insertAdd(const VectorBTree& config); + + /** Nr of vectors */ + inline size_t size() const { return size_; } + + /** Total dimensionality */ + inline size_t dim() const { return values_.size(); } + + /** Check whether Symbol j exists in config */ + inline bool contains(const Symbol& j) const { return ranges_.mem(j); } + + /** return all the nodes in the graph **/ + std::vector get_names() const; + + /** Vector access in VectorBtree is via the SubVector type */ + SubVector operator[](const Symbol& j); + ConstSubVector operator[](const Symbol& j) const; + + /** max of the vectors */ + double max() const; + + /** + * Check if compatible with other config, which is only + * guaranteed if vectors are inserted in exactly the same order, + * or if one config was created from the other using assignment. + * In the latter case, comparison is O(1), otherwise can be O(n). + */ + inline bool compatible(const VectorBTree& other) const { + return ranges_ == other.ranges_; + } + + /** + * O(1) check if structure of config is *physically* the same. + * i.e., configs were created through some assignment chain. + */ + inline bool cloned(const VectorBTree& other) const { + return ranges_.same(other.ranges_); + } + + /** Math operators */ + VectorBTree scale(double s) const; + VectorBTree operator*(double s) const; + VectorBTree operator-() const; + void operator+=(const VectorBTree &b); + VectorBTree operator+(const VectorBTree &b) const; + void operator-=(const VectorBTree &b); + VectorBTree operator-(const VectorBTree &b) const; + double dot(const VectorBTree& b) const; + + /** Set all vectors to zero */ + VectorBTree& zero(); + + /** Create a clone of x with exactly same structure, except with zero values */ + static VectorBTree zero(const VectorBTree& x); + + /** + * Add a delta config, needed for use in NonlinearOptimizer + * For VectorBTree, this is just addition. + */ + friend VectorBTree expmap(const VectorBTree& original, const VectorBTree& delta); + + /** + * Add a delta vector (not a config) + * Will use the ordering that map uses to loop over vectors + */ + friend VectorBTree expmap(const VectorBTree& original, const Vector& delta); + + /** + * BLAS Level 1 scal: x <- alpha*x + */ + friend void scal(double alpha, VectorBTree& x); + + /** + * BLAS Level 1 axpy: y <- alpha*x + y + * UNSAFE !!!! Only works if x and y laid out in exactly same shape + * Used in internal loop in iterative for fast conjugate gradients + * Consider using other functions if this is not in hotspot + */ + friend void axpy(double alpha, const VectorBTree& x, VectorBTree& y); + + /** @brief Const iterator */ + class const_iterator { + + public: + + // traits for playing nice with STL + typedef ptrdiff_t difference_type; // correct ? + typedef std::forward_iterator_tag iterator_category; + typedef std::pair value_type; + typedef const value_type* pointer; + typedef const value_type& reference; + + bool operator==(const const_iterator& __x) const { return it_ == __x.it_;} + bool operator!=(const const_iterator& __x) const { return it_ != __x.it_;} + + reference operator*() const { return value_;} + pointer operator->() const { return &value_;} + + const_iterator& operator++() { increment(); return *this; } + const_iterator operator++(int) { + const_iterator __tmp = *this; increment(); return __tmp; + } + + private: + + Ranges::const_iterator it_, end_; + const VectorBTree& config_; + value_type value_; + + const_iterator(const VectorBTree& config, const Ranges::const_iterator& it) : + it_(it), end_(config_.ranges_.end()), config_(config) { + update(); + } + + void update() { + if (it_ != end_) value_ = std::make_pair(it_->first, config_.get(it_->second)); + } + + void increment() { it_++; update();} + + friend class VectorBTree; + }; // const_iterator + + // We do not have a non-const iterator right now + typedef const_iterator iterator; + + /** return iterators */ + const_iterator begin() const { return const_iterator(*this,ranges_.begin());} + const_iterator end () const { return const_iterator(*this,ranges_.end());} + +#ifdef UNTESTED + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(values); + } + }; // VectorBTree + +#endif + + }; // VectorBTree + + /** scalar product */ + inline VectorBTree operator*(double s, const VectorBTree& x) { + return x * s; + } + + /** dim function (for iterative::CGD) */ + inline double dim(const VectorBTree& x) { + return x.dim(); + } + + /** max of the vectors */ + inline double max(const VectorBTree& x) { + return x.max(); + } + + /* dot product */ + inline double dot(const VectorBTree& a, const VectorBTree& b) { + return a.dot(b); + } + + /** print with optional string */ + inline void print(const VectorBTree& v, const std::string& s = "") { + v.print(s); + } + +} // gtsam diff --git a/cpp/VectorConfig.h b/cpp/VectorConfig.h index 96637d537..b729b3a78 100644 --- a/cpp/VectorConfig.h +++ b/cpp/VectorConfig.h @@ -1,165 +1,41 @@ /** * @file VectorConfig.h * @brief Factor Graph Configuration - * @author Carlos Nieto - * @author Christian Potthast + * @author Frank Dellaert */ -// \callgraph - #pragma once -#include -#include +/* + * There are two interchangeable implementations of VectorConfig. + * + * VectorMap uses a straightforward stl::map of Vectors. It has O(log n) + * insert and access, and is fairly fast at both. However, it has high overhead + * for arithmetic operations such as +, scale, axpy etc... + * + * VectorBTree uses a functional BTree as a way to access SubVectors + * in an ordinary Vector. Inserting is O(n) and much slower, but accessing, + * is O(log n) and might be a bit slower than VectorMap. Arithmetic operations + * are blindingly fast, however. The cost is it is not as KISS as VectorMap. + * + * Access to vectors is now exclusively via operator[] + * Vector access in VectorMap is via a Vector reference + * Vector access in VectorBtree is via the SubVector type (see Vector.h) + * + * Feb 16 2010: FD: I made VectorMap the default, because I decided to try + * and speed up conjugate gradients by using Sparse FactorGraphs all the way. + */ -#include "Testable.h" -#include "Vector.h" -#include "Key.h" -#include "SymbolMap.h" +// we use define and not typedefs as typdefs cannot be forward declared +#ifdef VECTORBTREE -namespace gtsam { - - /** Factor Graph Configuration */ - class VectorConfig : public Testable { +#include "VectorBTree.h" +#define VectorConfig VectorBTree - protected: - /** Map from string indices to values */ - SymbolMap values; +#else - public: - typedef SymbolMap::iterator iterator; - typedef SymbolMap::const_iterator const_iterator; +#include "VectorMap.h" +#define VectorConfig VectorMap - VectorConfig() {} - VectorConfig(const VectorConfig& cfg_in): values(cfg_in.values) {} - VectorConfig(const Symbol& j, const Vector& a) { add(j,a); } - - virtual ~VectorConfig() {} +#endif - /** return all the nodes in the graph **/ - std::vector get_names() const; - - /** Insert a value into the configuration with a given index */ - inline VectorConfig& insert(const Symbol& name, const Vector& val) { - values.insert(std::make_pair(name,val)); - return *this; - } - - /** Insert a config into another config */ - void insert(const VectorConfig& config); - - /** Add to vector at position j */ - void add(const Symbol& j, const Vector& a); - - const_iterator begin() const {return values.begin();} - const_iterator end() const {return values.end();} - iterator begin() {return values.begin();} - iterator end() {return values.end();} - - /** get a vector in the configuration by name */ - const Vector& get(const Symbol& name) const; - - /** get a vector reference by name */ - Vector& getReference(const Symbol& name); - - /** operator[] syntax for get */ - inline const Vector& operator[](const Symbol& name) const { - return get(name); - } - - bool contains(const Symbol& name) const { - const_iterator it = values.find(name); - return (it!=values.end()); - } - - /** Nr of vectors */ - size_t size() const { return values.size();} - - /** Total dimensionality */ - size_t dim() const; - - /** max of the vectors */ - inline double max() const { - double m = -std::numeric_limits::infinity(); - for(const_iterator it=begin(); it!=end(); it++) - m = std::max(m, gtsam::max(it->second)); - return m; - } - - /** Scale */ - VectorConfig scale(double s) const; - VectorConfig operator*(double s) const; - - /** Negation */ - VectorConfig operator-() const; - - /** Add in place */ - void operator+=(const VectorConfig &b); - - /** Add */ - VectorConfig operator+(const VectorConfig &b) const; - - /** Subtract */ - VectorConfig operator-(const VectorConfig &b) const; - - /** print */ - void print(const std::string& name = "") const; - - /** equals, for unit testing */ - bool equals(const VectorConfig& expected, double tol=1e-9) const; - - void clear() {values.clear();} - - /** Dot product */ - double dot(const VectorConfig& b) const; - - /** - * Add a delta config, needed for use in NonlinearOptimizer - * For VectorConfig, this is just addition. - */ - friend VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta); - /** - * Add a delta vector (not a config) - * Will use the ordering that map uses to loop over vectors - */ - friend VectorConfig expmap(const VectorConfig& original, const Vector& delta); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(values); - } - }; // VectorConfig - - /** scalar product */ - inline VectorConfig operator*(double s, const VectorConfig& x) {return x*s;} - - /** Dot product */ - double dot(const VectorConfig&, const VectorConfig&); - - /** - * BLAS Level 1 scal: x <- alpha*x - */ - void scal(double alpha, VectorConfig& x); - - /** - * BLAS Level 1 axpy: y <- alpha*x + y - * UNSAFE !!!! Only works if x and y laid out in exactly same shape - * Used in internal loop in iterative for fast conjugate gradients - * Consider using other functions if this is not in hotspot - */ - void axpy(double alpha, const VectorConfig& x, VectorConfig& y); - - /** dim function (for iterative::CGD) */ - inline double dim(const VectorConfig& x) { return x.dim();} - - /** max of the vectors */ - inline double max(const VectorConfig& x) { return x.max();} - - /** print with optional string */ - void print(const VectorConfig& v, const std::string& s = ""); - -} // gtsam diff --git a/cpp/VectorConfig.cpp b/cpp/VectorMap.cpp similarity index 53% rename from cpp/VectorConfig.cpp rename to cpp/VectorMap.cpp index c37875c1b..ae56ea1ba 100644 --- a/cpp/VectorConfig.cpp +++ b/cpp/VectorMap.cpp @@ -1,14 +1,14 @@ /** - * @file VectorConfig.cpp + * @file VectorMap.cpp * @brief Factor Graph Configuration - * @brief VectorConfig + * @brief VectorMap * @author Carlos Nieto * @author Christian Potthast */ #include #include -#include "VectorConfig.h" +#include "VectorMap.h" // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) @@ -23,12 +23,12 @@ void check_size(const Symbol& key, const Vector & vj, const Vector & dj) { cout << "For key \"" << (string)key << "\"" << endl; cout << "vj.size = " << vj.size() << endl; cout << "dj.size = " << dj.size() << endl; - throw(std::invalid_argument("VectorConfig::+ mismatched dimensions")); + throw(std::invalid_argument("VectorMap::+ mismatched dimensions")); } } /* ************************************************************************* */ -std::vector VectorConfig::get_names() const { +std::vector VectorMap::get_names() const { std::vector names; for(const_iterator it=values.begin(); it!=values.end(); it++) names.push_back(it->first); @@ -36,81 +36,106 @@ std::vector VectorConfig::get_names() const { } /* ************************************************************************* */ -void VectorConfig::insert(const VectorConfig& config) { - for (const_iterator it = config.begin(); it!=config.end(); it++) { - insert(it->first, it->second); - } +VectorMap& VectorMap::insert(const Symbol& name, const Vector& v) { + values.insert(std::make_pair(name,v)); + return *this; } /* ************************************************************************* */ -void VectorConfig::add(const Symbol& j, const Vector& a) { +VectorMap& VectorMap::insertAdd(const Symbol& j, const Vector& a) { Vector& vj = values[j]; if (vj.size()==0) vj = a; else vj += a; + return *this; } /* ************************************************************************* */ -size_t VectorConfig::dim() const { +void VectorMap::insert(const VectorMap& config) { + for (const_iterator it = config.begin(); it!=config.end(); it++) + insert(it->first, it->second); +} + +/* ************************************************************************* */ +void VectorMap::insertAdd(const VectorMap& config) { + for (const_iterator it = config.begin(); it!=config.end(); it++) + insertAdd(it->first,it->second); +} + +/* ************************************************************************* */ +size_t VectorMap::dim() const { size_t result=0; - Symbol key; Vector v; // rtodo: copying vector? - FOREACH_PAIR(key, v, values) result += v.size(); + for (const_iterator it = begin(); it != end(); it++) + result += it->second.size(); return result; } /* ************************************************************************* */ -VectorConfig VectorConfig::scale(double s) const { - VectorConfig scaled; - Symbol key; Vector val; // rtodo: copying vector? - FOREACH_PAIR(key, val, values) - scaled.insert(key, s*val); +const Vector& VectorMap::operator[](const Symbol& name) const { + return values.at(name); +} + +/* ************************************************************************* */ +Vector& VectorMap::operator[](const Symbol& name) { + return values.at(name); +} + +/* ************************************************************************* */ +VectorMap VectorMap::scale(double s) const { + VectorMap scaled; + for (const_iterator it = begin(); it != end(); it++) + scaled.insert(it->first, s*it->second); return scaled; } /* ************************************************************************* */ -VectorConfig VectorConfig::operator*(double s) const { +VectorMap VectorMap::operator*(double s) const { return scale(s); } /* ************************************************************************* */ -VectorConfig VectorConfig::operator-() const { - VectorConfig result; - Symbol j; Vector v; // rtodo: copying vector? - FOREACH_PAIR(j, v, values) - result.insert(j, -v); +VectorMap VectorMap::operator-() const { + VectorMap result; + for (const_iterator it = begin(); it != end(); it++) + result.insert(it->first, - it->second); return result; } /* ************************************************************************* */ -void VectorConfig::operator+=(const VectorConfig& b) { - Symbol j; Vector b_j; // rtodo: copying vector? - FOREACH_PAIR(j, b_j, b.values) { - iterator it = values.find(j); - if (it==values.end()) - insert(j, b_j); - else - it->second += b_j; - } +void VectorMap::operator+=(const VectorMap& b) { + insertAdd(b); } /* ************************************************************************* */ -VectorConfig VectorConfig::operator+(const VectorConfig& b) const { - VectorConfig result = *this; +VectorMap VectorMap::operator+(const VectorMap& b) const { + VectorMap result = *this; result += b; return result; } /* ************************************************************************* */ -VectorConfig VectorConfig::operator-(const VectorConfig& b) const { - VectorConfig result; - Symbol j; Vector v; // rtodo: copying vector? - FOREACH_PAIR(j, v, values) - result.insert(j, v - b.get(j)); +VectorMap VectorMap::operator-(const VectorMap& b) const { + VectorMap result; + for (const_iterator it = begin(); it != end(); it++) + result.insert(it->first, it->second - b[it->first]); return result; } /* ************************************************************************* */ -VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta) +VectorMap& VectorMap::zero() { + for (iterator it = begin(); it != end(); it++) + std::fill(it->second.begin(), it->second.end(), 0.0); + return *this; +} + +/* ************************************************************************* */ +VectorMap VectorMap::zero(const VectorMap& x) { + VectorMap cloned(x); + return cloned.zero(); +} + +/* ************************************************************************* */ +VectorMap expmap(const VectorMap& original, const VectorMap& delta) { - VectorConfig newConfig; + VectorMap newConfig; Symbol j; Vector vj; // rtodo: copying vector? FOREACH_PAIR(j, vj, original.values) { if (delta.contains(j)) { @@ -125,9 +150,9 @@ VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta) } /* ************************************************************************* */ -VectorConfig expmap(const VectorConfig& original, const Vector& delta) +VectorMap expmap(const VectorMap& original, const Vector& delta) { - VectorConfig newConfig; + VectorMap newConfig; size_t i = 0; Symbol j; Vector vj; // rtodo: copying vector? FOREACH_PAIR(j, vj, original.values) { @@ -140,70 +165,59 @@ VectorConfig expmap(const VectorConfig& original, const Vector& delta) } /* ************************************************************************* */ -const Vector& VectorConfig::get(const Symbol& name) const { - return values.at(name); -} - -/* ************************************************************************* */ -Vector& VectorConfig::getReference(const Symbol& name) { - return values.at(name); -} - -/* ************************************************************************* */ -void VectorConfig::print(const string& name) const { - odprintf("VectorConfig %s\n", name.c_str()); +void VectorMap::print(const string& name) const { + odprintf("VectorMap %s\n", name.c_str()); odprintf("size: %d\n", values.size()); - Symbol j; Vector v; // rtodo: copying vector - FOREACH_PAIR(j, v, values) { - odprintf("%s:", ((string)j).c_str()); - gtsam::print(v); + for (const_iterator it = begin(); it != end(); it++) { + odprintf("%s:", ((string)it->first).c_str()); + gtsam::print(it->second); } } /* ************************************************************************* */ -bool VectorConfig::equals(const VectorConfig& expected, double tol) const { - Symbol j; Vector vActual; // rtodo: copying vector +bool VectorMap::equals(const VectorMap& expected, double tol) const { if( values.size() != expected.size() ) return false; // iterate over all nodes - FOREACH_PAIR(j, vActual, values) { - Vector vExpected = expected[j]; - if(!equal_with_abs_tol(vExpected,vActual,tol)) + for (const_iterator it = begin(); it != end(); it++) { + Vector vExpected = expected[it->first]; + if(!equal_with_abs_tol(vExpected,it->second,tol)) return false; } return true; } /* ************************************************************************* */ -double VectorConfig::dot(const VectorConfig& b) const { - Symbol j; Vector v; double result = 0.0; // rtodo: copying vector - FOREACH_PAIR(j, v, values) result += gtsam::dot(v,b.get(j)); +double VectorMap::dot(const VectorMap& b) const { + double result = 0.0; // rtodo: copying vector + for (const_iterator it = begin(); it != end(); it++) + result += gtsam::dot(it->second,b[it->first]); return result; } /* ************************************************************************* */ -double dot(const VectorConfig& a, const VectorConfig& b) { +double dot(const VectorMap& a, const VectorMap& b) { return a.dot(b); } /* ************************************************************************* */ -void scal(double alpha, VectorConfig& x) { - for (VectorConfig::iterator xj = x.begin(); xj != x.end(); xj++) +void scal(double alpha, VectorMap& x) { + for (VectorMap::iterator xj = x.begin(); xj != x.end(); xj++) scal(alpha, xj->second); } /* ************************************************************************* */ -void axpy(double alpha, const VectorConfig& x, VectorConfig& y) { - VectorConfig::const_iterator xj = x.begin(); - for (VectorConfig::iterator yj = y.begin(); yj != y.end(); yj++, xj++) +void axpy(double alpha, const VectorMap& x, VectorMap& y) { + VectorMap::const_iterator xj = x.begin(); + for (VectorMap::iterator yj = y.begin(); yj != y.end(); yj++, xj++) axpy(alpha, xj->second, yj->second); } /* ************************************************************************* */ - -void print(const VectorConfig& v, const std::string& s){ +void print(const VectorMap& v, const std::string& s){ v.print(s); } + /* ************************************************************************* */ } // gtsam diff --git a/cpp/VectorMap.h b/cpp/VectorMap.h new file mode 100644 index 000000000..8c2ec1e10 --- /dev/null +++ b/cpp/VectorMap.h @@ -0,0 +1,165 @@ +/** + * @file VectorMap.h + * @brief Factor Graph Configuration + * @author Carlos Nieto + * @author Christian Potthast + */ + +// \callgraph + +#pragma once + +#include +#include + +#include "Testable.h" +#include "Vector.h" +#include "Key.h" +#include "SymbolMap.h" + +namespace gtsam { + + /** Factor Graph Configuration */ + class VectorMap : public Testable { + + protected: + /** Map from string indices to values */ + SymbolMap values; + + public: + typedef SymbolMap::iterator iterator; + typedef SymbolMap::const_iterator const_iterator; + + VectorMap() {} + VectorMap(const VectorMap& cfg_in): values(cfg_in.values) {} + VectorMap(const Symbol& j, const Vector& a) { insert(j,a); } + + virtual ~VectorMap() {} + + /** return all the nodes in the graph **/ + std::vector get_names() const; + + /** Insert a value into the configuration with a given index */ + VectorMap& insert(const Symbol& name, const Vector& v); + + /** Insert or add a value with given index */ + VectorMap& insertAdd(const Symbol& j, const Vector& v); + + /** Insert a config into another config */ + void insert(const VectorMap& config); + + /** Insert a config into another config, add if key already exists */ + void insertAdd(const VectorMap& config); + + const_iterator begin() const {return values.begin();} + const_iterator end() const {return values.end();} + iterator begin() {return values.begin();} + iterator end() {return values.end();} + + /** Vector access in VectorMap is via a Vector reference */ + Vector& operator[](const Symbol& j); + const Vector& operator[](const Symbol& j) const; + + bool contains(const Symbol& name) const { + const_iterator it = values.find(name); + return (it!=values.end()); + } + + /** Nr of vectors */ + size_t size() const { return values.size();} + + /** Total dimensionality */ + size_t dim() const; + + /** max of the vectors */ + inline double max() const { + double m = -std::numeric_limits::infinity(); + for(const_iterator it=begin(); it!=end(); it++) + m = std::max(m, gtsam::max(it->second)); + return m; + } + + /** Scale */ + VectorMap scale(double s) const; + VectorMap operator*(double s) const; + + /** Negation */ + VectorMap operator-() const; + + /** Add in place */ + void operator+=(const VectorMap &b); + + /** Add */ + VectorMap operator+(const VectorMap &b) const; + + /** Subtract */ + VectorMap operator-(const VectorMap &b) const; + + /** print */ + void print(const std::string& name = "") const; + + /** equals, for unit testing */ + bool equals(const VectorMap& expected, double tol=1e-9) const; + + void clear() {values.clear();} + + /** Dot product */ + double dot(const VectorMap& b) const; + + /** Set all vectors to zero */ + VectorMap& zero(); + + /** Create a clone of x with exactly same structure, except with zero values */ + static VectorMap zero(const VectorMap& x); + + /** + * Add a delta config, needed for use in NonlinearOptimizer + * For VectorMap, this is just addition. + */ + friend VectorMap expmap(const VectorMap& original, const VectorMap& delta); + + /** + * Add a delta vector (not a config) + * Will use the ordering that map uses to loop over vectors + */ + friend VectorMap expmap(const VectorMap& original, const Vector& delta); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(values); + } + }; // VectorMap + + /** scalar product */ + inline VectorMap operator*(double s, const VectorMap& x) {return x*s;} + + /** Dot product */ + double dot(const VectorMap&, const VectorMap&); + + /** + * BLAS Level 1 scal: x <- alpha*x + */ + void scal(double alpha, VectorMap& x); + + /** + * BLAS Level 1 axpy: y <- alpha*x + y + * UNSAFE !!!! Only works if x and y laid out in exactly same shape + * Used in internal loop in iterative for fast conjugate gradients + * Consider using other functions if this is not in hotspot + */ + void axpy(double alpha, const VectorMap& x, VectorMap& y); + + /** dim function (for iterative::CGD) */ + inline double dim(const VectorMap& x) { return x.dim();} + + /** max of the vectors */ + inline double max(const VectorMap& x) { return x.max();} + + /** print with optional string */ + void print(const VectorMap& v, const std::string& s = ""); + +} // gtsam diff --git a/cpp/testBayesNetPreconditioner.cpp b/cpp/testBayesNetPreconditioner.cpp index aa7a3c1db..abac336bb 100644 --- a/cpp/testBayesNetPreconditioner.cpp +++ b/cpp/testBayesNetPreconditioner.cpp @@ -89,9 +89,26 @@ TEST( BayesNetPreconditioner, conjugateGradients ) BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2); VectorConfig y1 = y0; - y1.getReference("x2003") = Vector_(2, 1.0, -1.0); + y1["x2003"] = Vector_(2, 1.0, -1.0); VectorConfig x1 = system.x(y1); + // Check gradient for y0 + VectorConfig expectedGradient0; + expectedGradient0.insert("x1001", Vector_(2,-1000.,-1000.)); + expectedGradient0.insert("x1002", Vector_(2, 0., -300.)); + expectedGradient0.insert("x1003", Vector_(2, 0., -300.)); + expectedGradient0.insert("x2001", Vector_(2, -100., 200.)); + expectedGradient0.insert("x2002", Vector_(2, -100., 0.)); + expectedGradient0.insert("x2003", Vector_(2, -100., -200.)); + expectedGradient0.insert("x3001", Vector_(2, -100., 100.)); + expectedGradient0.insert("x3002", Vector_(2, -100., 0.)); + expectedGradient0.insert("x3003", Vector_(2, -100., -100.)); + VectorConfig actualGradient0 = system.gradient(y0); + CHECK(assert_equal(expectedGradient0,actualGradient0)); +#ifdef VECTORBTREE + CHECK(actualGradient0.cloned(y0)); +#endif + // Solve using PCG bool verbose = false; double epsilon = 1e-6; // had to crank this down !!! diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index a243023ba..c7a220f4c 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -670,26 +670,6 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional ) CHECK(assert_equal(expectedLF,actualLF,1e-5)); } -/* ************************************************************************* */ -TEST( GaussianFactor, alphaFactor ) -{ - GaussianFactorGraph fg = createGaussianFactorGraph(); - - // get alphafactor for first factor in fg at zero, in gradient direction - Symbol alphaKey(ALPHA, 1); - VectorConfig x = createZeroDelta(); - VectorConfig d = fg.gradient(x); - GaussianFactor::shared_ptr factor = fg[0]; - GaussianFactor::shared_ptr actual = factor->alphaFactor(alphaKey,x,d); - - // calculate expected - Matrix A = Matrix_(2,1,300.0,50.0); - Vector b = Vector_(2,-1.0,-1.0); - GaussianFactor expected(alphaKey,A,b,noiseModel::Unit::Create(2)); - - CHECK(assert_equal(expected,*actual)); -} - /* ************************************************************************* */ TEST ( GaussianFactor, constraint_eliminate1 ) { @@ -817,6 +797,7 @@ TEST ( GaussianFactor, combine_matrix ) { // CHECK(true); // } //} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index a19ab2e5c..b035e86b3 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -31,7 +31,7 @@ using namespace example; double tol=1e-5; /* ************************************************************************* */ -/* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */ +/* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */ /* ************************************************************************* */ TEST( GaussianFactorGraph, equals ){ diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 0629b753c..35d451cde 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -14,7 +14,7 @@ using namespace boost::assign; #define GTSAM_MAGIC_KEY #include "Ordering.h" -#include "iterative.h" +#include "VectorConfig.h" #include "smallExample.h" #include "pose2SLAM.h" #include "SubgraphPreconditioner.h" @@ -92,7 +92,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) Pose2Graph graph; Matrix cov = eye(3); graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov))); - graph.addConstraint(1, config[1]); + graph.addHardConstraint(1, config[1]); VectorConfig zeros; zeros.insert("x1",zero(3)); @@ -144,10 +144,6 @@ TEST( Iterative, subgraphPCG ) graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); - VectorConfig zeros; - zeros.insert("x1",zero(3)); - zeros.insert("x2",zero(3)); - // generate spanning tree and create ordering PredecessorMap tree = graph.findMinimumSpanningTree(); list keys = predecessorMap2Keys(tree); @@ -168,6 +164,8 @@ TEST( Iterative, subgraphPCG ) SubgraphPreconditioner::sharedConfig xbar = optimize_(*Rc1); SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); + VectorConfig zeros = VectorConfig::zero(*xbar); + // Solve the subgraph PCG VectorConfig ybar = conjugateGradients (system, zeros, verbose, 1e-5, 1e-5, 100); @@ -178,6 +176,7 @@ TEST( Iterative, subgraphPCG ) expected.insert("x2", Vector_(3, -0.5, 0., 0.)); CHECK(assert_equal(expected, actual)); } + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 942566d92..4976a34bc 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -205,25 +205,34 @@ TEST( NonlinearOptimizer, Factorization ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SubgraphPCG ) { - typedef NonlinearOptimizer > Optimizer; - - boost::shared_ptr config(new Pose2Config); - config->insert(1, Pose2(0.,0.,0.)); - config->insert(2, Pose2(1.5,0.,0.)); + typedef NonlinearOptimizer > Optimizer; + // Create a graph boost::shared_ptr graph(new Pose2Graph); - graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); - graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); + graph->addPrior(1, Pose2(0., 0., 0.), Isotropic::Sigma(3, 1e-10)); + graph->addConstraint(1, 2, Pose2(1., 0., 0.), Isotropic::Sigma(3, 1)); + // Create an initial config + boost::shared_ptr config(new Pose2Config); + config->insert(1, Pose2(0., 0., 0.)); + config->insert(2, Pose2(1.5, 0., 0.)); + + // Create solver and optimizer + Optimizer::shared_solver solver + (new SubgraphPCG (*graph, *config)); + Optimizer optimizer(graph, config, solver); + + // Optimize !!!! double relativeThreshold = 1e-5; double absoluteThreshold = 1e-5; - Optimizer::shared_solver solver(new SubgraphPCG(*graph, *config)); - Optimizer optimizer(graph, config, solver); - Optimizer optimized = optimizer.gaussNewton(relativeThreshold, absoluteThreshold, Optimizer::SILENT); + Optimizer optimized = optimizer.gaussNewton(relativeThreshold, + absoluteThreshold, Optimizer::SILENT); + // Check solution Pose2Config expected; - expected.insert(1, Pose2(0.,0.,0.)); - expected.insert(2, Pose2(1.,0.,0.)); + expected.insert(1, Pose2(0., 0., 0.)); + expected.insert(2, Pose2(1., 0., 0.)); CHECK(assert_equal(expected, *optimized.config(), 1e-5)); } diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 6beab698e..86d36f183 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -50,7 +50,8 @@ TEST( Pose2Factor, error ) CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); // Check error after increasing p2 - VectorConfig plus = delta + VectorConfig("x2", Vector_(3, 0.1, 0.0, 0.0)); + VectorConfig plus = delta; + plus.insertAdd("x2", Vector_(3, 0.1, 0.0, 0.0)); Pose2Config x1 = expmap(x0, plus); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); diff --git a/cpp/testSubgraphPreconditioner.cpp b/cpp/testSubgraphPreconditioner.cpp index e5054ff4f..8203306a0 100644 --- a/cpp/testSubgraphPreconditioner.cpp +++ b/cpp/testSubgraphPreconditioner.cpp @@ -66,29 +66,6 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree ) CHECK(assert_equal(xtrue,xbar)); } -/* ************************************************************************* */ -double error(const VectorConfig& x) { - // Build a planar graph - GaussianFactorGraph Ab; - VectorConfig xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); - - // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_.eliminate_(ordering); // R1*x-c1 - SubgraphPreconditioner::sharedConfig xbar = optimize_(*Rc1); // xbar = inv(R1)*c1 - - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); - return system.error(x); -} - /* ************************************************************************* */ TEST( SubgraphPreconditioner, system ) { @@ -113,21 +90,19 @@ TEST( SubgraphPreconditioner, system ) SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); // Create zero config - VectorConfig zeros; - Vector z2 = zero(2); - BOOST_FOREACH(const Symbol& j, ordering) zeros.insert(j,z2); + VectorConfig zeros = VectorConfig::zero(*xbar); // Set up y0 as all zeros VectorConfig y0 = zeros; // y1 = perturbed y0 VectorConfig y1 = zeros; - y1.getReference("x2003") = Vector_(2, 1.0, -1.0); + y1["x2003"] = Vector_(2, 1.0, -1.0); // Check corresponding x values VectorConfig expected_x1 = xtrue, x1 = system.x(y1); - expected_x1.getReference("x2003") = Vector_(2, 2.01, 2.99); - expected_x1.getReference("x3003") = Vector_(2, 3.01, 2.99); + expected_x1["x2003"] = Vector_(2, 2.01, 2.99); + expected_x1["x3003"] = Vector_(2, 3.01, 2.99); CHECK(assert_equal(xtrue, system.x(y0))); CHECK(assert_equal(expected_x1,system.x(y1))); @@ -141,30 +116,30 @@ TEST( SubgraphPreconditioner, system ) VectorConfig expected_gx0 = zeros; VectorConfig expected_gx1 = zeros; CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue))); - expected_gx1.getReference("x1003") = Vector_(2, -100., 100.); - expected_gx1.getReference("x2002") = Vector_(2, -100., 100.); - expected_gx1.getReference("x2003") = Vector_(2, 200., -200.); - expected_gx1.getReference("x3002") = Vector_(2, -100., 100.); - expected_gx1.getReference("x3003") = Vector_(2, 100., -100.); + expected_gx1["x1003"] = Vector_(2, -100., 100.); + expected_gx1["x2002"] = Vector_(2, -100., 100.); + expected_gx1["x2003"] = Vector_(2, 200., -200.); + expected_gx1["x3002"] = Vector_(2, -100., 100.); + expected_gx1["x3003"] = Vector_(2, 100., -100.); CHECK(assert_equal(expected_gx1,Ab.gradient(x1))); // Test gradient in y VectorConfig expected_gy0 = zeros; VectorConfig expected_gy1 = zeros; - expected_gy1.getReference("x1003") = Vector_(2, 2., -2.); - expected_gy1.getReference("x2002") = Vector_(2, -2., 2.); - expected_gy1.getReference("x2003") = Vector_(2, 3., -3.); - expected_gy1.getReference("x3002") = Vector_(2, -1., 1.); - expected_gy1.getReference("x3003") = Vector_(2, 1., -1.); + expected_gy1["x1003"] = Vector_(2, 2., -2.); + expected_gy1["x2002"] = Vector_(2, -2., 2.); + expected_gy1["x2003"] = Vector_(2, 3., -3.); + expected_gy1["x3002"] = Vector_(2, -1., 1.); + expected_gy1["x3003"] = Vector_(2, 1., -1.); CHECK(assert_equal(expected_gy0,system.gradient(y0))); CHECK(assert_equal(expected_gy1,system.gradient(y1))); // Check it numerically for good measure // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) - Vector numerical_g1 = numericalGradient (error, y1, 0.001); - Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., - 3., -3., 0., 0., -1., 1., 1., -1.); - CHECK(assert_equal(expected_g1,numerical_g1)); +// Vector numerical_g1 = numericalGradient (error, y1, 0.001); +// Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., +// 3., -3., 0., 0., -1., 1., 1., -1.); +// CHECK(assert_equal(expected_g1,numerical_g1)); } /* ************************************************************************* */ @@ -191,12 +166,10 @@ TEST( SubgraphPreconditioner, conjugateGradients ) SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); // Create zero config y0 and perturbed config y1 - VectorConfig y0; - Vector z2 = zero(2); - BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2); + VectorConfig y0 = VectorConfig::zero(*xbar); VectorConfig y1 = y0; - y1.getReference("x2003") = Vector_(2, 1.0, -1.0); + y1["x2003"] = Vector_(2, 1.0, -1.0); VectorConfig x1 = system.x(y1); // Solve for the remaining constraints using PCG @@ -209,7 +182,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) // Compare with non preconditioned version: VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon, - maxIterations); + epsilon, maxIterations); CHECK(assert_equal(xtrue,actual2,1e-4)); } diff --git a/cpp/testVectorBTree.cpp b/cpp/testVectorBTree.cpp new file mode 100644 index 000000000..0f115eba8 --- /dev/null +++ b/cpp/testVectorBTree.cpp @@ -0,0 +1,310 @@ +/** + * @file testVectorBTree.cpp + * @brief Unit tests for Factor Graph Configuration + * @author Frank Dellaert + **/ + +#include +#include +#include + +#include +#include +using namespace boost::assign; // bring 'operator+=()' into scope + +//#include TEST_AC_DEFINE + +#ifdef HAVE_BOOST_SERIALIZATION +#include +#include +#endif //HAVE_BOOST_SERIALIZATION + +#include +#include "Matrix.h" +#include "VectorBTree.h" + +using namespace std; +using namespace gtsam; + +static Symbol l1('l',1), x1('x',1), x2('x',2); +static double inf = std::numeric_limits::infinity(); + +/* ************************************************************************* */ +VectorBTree smallVectorBTree() { + VectorBTree c; + c.insert(l1, Vector_(2, 0.0, -1.0)); + c.insert(x1, Vector_(2, 0.0, 0.0)); + c.insert(x2, Vector_(2, 1.5, 0.0)); + return c; +} + +/* ************************************************************************* */ +TEST( VectorBTree, constructor_insert_get ) +{ + VectorBTree expected; + Vector v = Vector_(3, 5.0, 6.0, 7.0); + expected.insert(x1, v); + VectorBTree actual(x1, v); + LONGS_EQUAL(1,actual.size()) + CHECK(assert_equal(expected,actual)) + CHECK(equal_with_abs_tol(v,actual[x1])) +} + +/* ************************************************************************* */ +TEST( VectorBTree, dim) { + VectorBTree c = smallVectorBTree(); + LONGS_EQUAL(6,c.dim()); +} + +/* ************************************************************************* */ +TEST( VectorBTree, insertAdd) { + VectorBTree expected; + expected.insert(l1, Vector_(2, 0.0, -2.0)); + expected.insert(x1, Vector_(2, 0.0, 0.0)); + expected.insert(x2, Vector_(2, 3.0, 0.0)); + VectorBTree actual = smallVectorBTree(); + actual.insertAdd(actual); + CHECK(assert_equal(expected,actual)) +} + +/* ************************************************************************* */ +TEST( VectorBTree, zero) { + VectorBTree expected; + expected.insert(l1, Vector_(2, 0.0, 0.0)); + expected.insert(x1, Vector_(2, 0.0, 0.0)); + expected.insert(x2, Vector_(2, 0.0, 0.0)); + VectorBTree actual = smallVectorBTree(); + CHECK(assert_equal(expected,VectorBTree::zero(actual))); + CHECK(assert_equal(expected,actual.zero())); +} + +/* ************************************************************************* */ +TEST( VectorBTree, insert_config) { + VectorBTree expected = smallVectorBTree(); + VectorBTree actual, toAdd = smallVectorBTree(); + actual.insert(toAdd); + CHECK(assert_equal(expected,actual)) +} + +/* ************************************************************************* */ +TEST( VectorBTree, get_names) { + VectorBTree c = smallVectorBTree(); + std::vector expected, actual = c.get_names(); + expected += l1, x1, x2; + CHECK(expected==actual) +} + +/* ************************************************************************* */ +TEST( VectorBTree, const_iterator) { + VectorBTree c = smallVectorBTree(); + VectorBTree::const_iterator it = c.begin(); + CHECK(assert_equal(l1,it->first)); + CHECK(assert_equal(Vector_(2, 0.0,-1.0),(it++)->second)); + CHECK(assert_equal(x1,it->first)); + CHECK(assert_equal(Vector_(2, 0.0, 0.0),(it++)->second)); + CHECK(assert_equal(x2,it->first)); + CHECK(assert_equal(Vector_(2, 1.5, 0.0),(it++)->second)); + CHECK(it==c.end()); +} + +/* ************************************************************************* */ +TEST( VectorBTree, equals ) +{ + VectorBTree cfg1; + cfg1.insert(x1, Vector_(3, 5.0, 6.0, 7.0)); + CHECK(cfg1.equals(cfg1)); + CHECK(cfg1.compatible(cfg1)); + CHECK(cfg1.cloned(cfg1)); + + VectorBTree cfg2; + cfg2.insert(x1, Vector_(3, 5.0, 6.0, 7.0)); + CHECK(cfg1.equals(cfg2)); + CHECK(cfg1.compatible(cfg2)); + CHECK(!cfg1.cloned(cfg2)); + + VectorBTree cfg3 = cfg1; + CHECK(cfg1.equals(cfg3)); + CHECK(cfg1.compatible(cfg3)); + CHECK(cfg1.cloned(cfg3)); + + VectorBTree cfg4; + cfg4.insert(x1, Vector_(3, 5.0, 6.0, 8.0)); + CHECK(!cfg1.equals(cfg4)); + CHECK(cfg1.compatible(cfg4)); + CHECK(!cfg1.cloned(cfg4)); +} + +/* ************************************************************************* */ +TEST( VectorBTree, equals_nan ) + { + VectorBTree cfg1, cfg2; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, inf, inf, inf); + cfg1.insert(x1, v1); + cfg2.insert(x1, v2); + CHECK(!cfg1.equals(cfg2)); + CHECK(!cfg2.equals(cfg1)); + } + +/* ************************************************************************* */ +TEST( VectorBTree, contains) +{ + VectorBTree fg; + Vector v = Vector_(3, 5.0, 6.0, 7.0); + fg.insert(x1, v); + CHECK(fg.contains(x1)); + CHECK(!fg.contains(x2)); +} + +/* ************************************************************************* */ +TEST( VectorBTree, max) { + VectorBTree c = smallVectorBTree(); + DOUBLES_EQUAL(1.5,c.max(),1e-9); +} + +/* ************************************************************************* */ +TEST( VectorBTree, scale) { + VectorBTree cfg; + cfg.insert(x1, Vector_(2, 1.0, 2.0)); + cfg.insert(x2, Vector_(2,-1.0,-2.0)); + + VectorBTree actual = cfg.scale(2.0); + + VectorBTree expected; + expected.insert(x1, Vector_(2, 2.0, 4.0)); + expected.insert(x2, Vector_(2,-2.0,-4.0)); + + CHECK(assert_equal(actual, expected)); +} + +/* ************************************************************************* */ +TEST( VectorBTree, plus) +{ + VectorBTree c; + Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0); + c.insert(x1,vx).insert(x2,vy); + + VectorBTree delta; + Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0); + delta.insert(x1, dx).insert(x2,dy); + CHECK(delta.compatible(c)); + + // operator + + VectorBTree expected; + Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0); + expected.insert(x1, wx).insert(x2,wy); + CHECK(assert_equal(expected,c+delta)); + + // operator - + VectorBTree expected2; + Vector wx2 = Vector_(3, -5.0, -6.0, -7.0), wy2 = Vector_(2, -8.0, -9.0); + expected2.insert(x1, wx2).insert(x2,wy2); + CHECK(assert_equal(expected2,-c)); + + // expmap + VectorBTree actual = expmap(c,delta); + CHECK(assert_equal(expected,actual)); + + // in-place (although + already tests that, really) + c += delta; + CHECK(assert_equal(expected,c)); +} + +/* ************************************************************************* */ +TEST( VectorBTree, operators) { + VectorBTree c; c.insert(x1, Vector_(2, 1.1, 2.2)); + VectorBTree expected1; expected1.insert(x1, Vector_(2, 2.2, 4.4)); + CHECK(assert_equal(expected1,c*2)); + CHECK(assert_equal(expected1,c+c)); + VectorBTree expected2; expected2.insert(x1, Vector_(2, 0.0, 0.0)); + CHECK(assert_equal(expected2,c-c)); +} + +/* ************************************************************************* */ +TEST( VectorBTree, dot) { + VectorBTree c = smallVectorBTree(); + DOUBLES_EQUAL(3.25,dot(c,c),1e-9); +} + +/* ************************************************************************* */ +TEST( VectorBTree, expmap) +{ + VectorBTree c = smallVectorBTree(); + Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2 + CHECK(assert_equal(expmap(c,c),expmap(c,v))); +} + +/* ************************************************************************* */ +TEST( VectorBTree, scal) { + VectorBTree x,expected; + x.insert(x1,Vector_(3, 1.0, 2.0, 3.0)); + x.insert(x2,Vector_(2, 4.0, 5.0)); + expected.insert(x1,Vector_(3, 10.0, 20.0, 30.0)); + expected.insert(x2,Vector_(2, 40.0, 50.0)); + scal(10,x); + CHECK(assert_equal(expected,x)); +} + +/* ************************************************************************* */ +TEST( VectorBTree, axpy) { + VectorBTree x; + x.insert(x1,Vector_(3, 1.0, 1.0, 1.0)); + x.insert(x2,Vector_(2, -1.0, -1.0)); + + // axpy will only work on cloned configs - enforced for speed + VectorBTree y = VectorBTree::zero(x); + y[x1] = Vector_(3, 5.0, 6.0, 7.0); + y[x2] = Vector_(2, 8.0, 9.0); +// axpy(10,x,y); +// +// // Check result +// VectorBTree expected; +// expected.insert(x1,Vector_(3, 15.0, 16.0, 17.0)); +// expected.insert(x2,Vector_(2, -2.0, -1.0)); +// CHECK(assert_equal(expected,y)); +} + +/* ************************************************************************* */ +TEST( VectorBTree, subVector) { + VectorBTree c; c.insert(x1, Vector_(2, 1.1, 2.2)); + SubVector cx = c[x1]; + for (size_t i = 0; i < 2; i++) + cx(i) = cx(i)*2.0; + VectorBTree expected; expected.insert(x1, Vector_(2, 2.2, 4.4)); + CHECK(assert_equal(expected,c)); +} + +/* ************************************************************************* */ +#ifdef HAVE_BOOST_SERIALIZATION +TEST( VectorBTree, serialize) +{ + //DEBUG: + cout << "VectorBTree: Running Serialization Test" << endl; + + //create an VectorBTree + VectorBTree fg = createConfig(); + + //serialize the config + std::ostringstream in_archive_stream; + boost::archive::text_oarchive in_archive(in_archive_stream); + in_archive << fg; + std::string serialized_fgc = in_archive_stream.str(); + + //deserialize the config + std::istringstream out_archive_stream(serialized_fgc); + boost::archive::text_iarchive out_archive(out_archive_stream); + VectorBTree output; + out_archive >> output; + + //check for equality + CHECK(fg.equals(output)); +} +#endif //HAVE_BOOST_SERIALIZATION + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} + +/* ************************************************************************* */ diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorMap.cpp similarity index 66% rename from cpp/testVectorConfig.cpp rename to cpp/testVectorMap.cpp index 43d06cd70..e5b9ce674 100644 --- a/cpp/testVectorConfig.cpp +++ b/cpp/testVectorMap.cpp @@ -1,5 +1,5 @@ /** - * @file testVectorConfig.cpp + * @file testVectorMap.cpp * @brief Unit tests for Factor Graph Configuration * @author Carlos Nieto **/ @@ -19,28 +19,37 @@ #include #include "Matrix.h" -#include "VectorConfig.h" -#include "smallExample.cpp" +#include "VectorMap.h" using namespace std; using namespace gtsam; -using namespace example; + +static Symbol l1('l',1), x1('x',1), x2('x',2); /* ************************************************************************* */ -TEST( VectorConfig, equals1 ) +VectorMap smallVectorMap() { + VectorMap c; + c.insert(l1, Vector_(2, 0.0, -1.0)); + c.insert(x1, Vector_(2, 0.0, 0.0)); + c.insert(x2, Vector_(2, 1.5, 0.0)); + return c; +} + +/* ************************************************************************* */ +TEST( VectorMap, equals1 ) { - VectorConfig expected; + VectorMap expected; Vector v = Vector_(3, 5.0, 6.0, 7.0); expected.insert("a",v); - VectorConfig actual; + VectorMap actual; actual.insert("a",v); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( VectorConfig, equals2 ) +TEST( VectorMap, equals2 ) { - VectorConfig cfg1, cfg2; + VectorMap cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 5.0, 6.0, 8.0); cfg1.insert("x", v1); @@ -54,9 +63,9 @@ TEST( VectorConfig, equals2 ) #include double inf = std::numeric_limits::infinity(); -TEST( VectorConfig, equals_nan ) +TEST( VectorMap, equals_nan ) { - VectorConfig cfg1, cfg2; + VectorMap cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, inf, inf, inf); cfg1.insert("x", v1); @@ -66,9 +75,9 @@ TEST( VectorConfig, equals_nan ) } /* ************************************************************************* */ -TEST( VectorConfig, contains) +TEST( VectorMap, contains) { - VectorConfig fg; + VectorMap fg; Vector v = Vector_(3, 5.0, 6.0, 7.0); fg.insert("a", v); CHECK(fg.contains("a")); @@ -76,43 +85,43 @@ TEST( VectorConfig, contains) } /* ************************************************************************* */ -TEST( VectorConfig, expmap) +TEST( VectorMap, expmap) { - VectorConfig c = createVectorConfig(); + VectorMap c = smallVectorMap(); Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2 CHECK(assert_equal(expmap(c,c),expmap(c,v))); } /* ************************************************************************* */ -TEST( VectorConfig, plus) +TEST( VectorMap, plus) { - VectorConfig c; + VectorMap c; Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0); - c += VectorConfig("x",vx); - c += VectorConfig("y",vy); + c += VectorMap("x",vx); + c += VectorMap("y",vy); - VectorConfig delta; + VectorMap delta; Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0); delta.insert("x", dx).insert("y",dy); - VectorConfig expected; + VectorMap expected; Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0); expected.insert("x", wx).insert("y",wy); // functional - VectorConfig actual = expmap(c,delta); + VectorMap actual = expmap(c,delta); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( VectorConfig, scale) { - VectorConfig cfg; +TEST( VectorMap, scale) { + VectorMap cfg; cfg.insert("x", Vector_(2, 1.0, 2.0)); cfg.insert("y", Vector_(2,-1.0,-2.0)); - VectorConfig actual = cfg.scale(2.0); + VectorMap actual = cfg.scale(2.0); - VectorConfig expected; + VectorMap expected; expected.insert("x", Vector_(2, 2.0, 4.0)); expected.insert("y", Vector_(2,-2.0,-4.0)); @@ -120,42 +129,42 @@ TEST( VectorConfig, scale) { } /* ************************************************************************* */ -TEST( VectorConfig, axpy) { - VectorConfig x,y,expected; - x += VectorConfig("x",Vector_(3, 1.0, 1.0, 1.0)); - x += VectorConfig("y",Vector_(2, -1.0, -1.0)); - y += VectorConfig("x",Vector_(3, 5.0, 6.0, 7.0)); - y += VectorConfig("y",Vector_(2, 8.0, 9.0)); - expected += VectorConfig("x",Vector_(3, 15.0, 16.0, 17.0)); - expected += VectorConfig("y",Vector_(2, -2.0, -1.0)); +TEST( VectorMap, axpy) { + VectorMap x,y,expected; + x += VectorMap("x",Vector_(3, 1.0, 1.0, 1.0)); + x += VectorMap("y",Vector_(2, -1.0, -1.0)); + y += VectorMap("x",Vector_(3, 5.0, 6.0, 7.0)); + y += VectorMap("y",Vector_(2, 8.0, 9.0)); + expected += VectorMap("x",Vector_(3, 15.0, 16.0, 17.0)); + expected += VectorMap("y",Vector_(2, -2.0, -1.0)); axpy(10,x,y); CHECK(assert_equal(expected,y)); } /* ************************************************************************* */ -TEST( VectorConfig, scal) { - VectorConfig x,expected; - x += VectorConfig("x",Vector_(3, 1.0, 2.0, 3.0)); - x += VectorConfig("y",Vector_(2, 4.0, 5.0)); - expected += VectorConfig("x",Vector_(3, 10.0, 20.0, 30.0)); - expected += VectorConfig("y",Vector_(2, 40.0, 50.0)); +TEST( VectorMap, scal) { + VectorMap x,expected; + x += VectorMap("x",Vector_(3, 1.0, 2.0, 3.0)); + x += VectorMap("y",Vector_(2, 4.0, 5.0)); + expected += VectorMap("x",Vector_(3, 10.0, 20.0, 30.0)); + expected += VectorMap("y",Vector_(2, 40.0, 50.0)); scal(10,x); CHECK(assert_equal(expected,x)); } /* ************************************************************************* */ -TEST( VectorConfig, update_with_large_delta) { +TEST( VectorMap, update_with_large_delta) { // this test ensures that if the update for delta is larger than // the size of the config, it only updates existing variables - VectorConfig init, delta; + VectorMap init, delta; init.insert("x", Vector_(2, 1.0, 2.0)); init.insert("y", Vector_(2, 3.0, 4.0)); delta.insert("x", Vector_(2, 0.1, 0.1)); delta.insert("y", Vector_(2, 0.1, 0.1)); delta.insert("p", Vector_(2, 0.1, 0.1)); - VectorConfig actual = expmap(init,delta); - VectorConfig expected; + VectorMap actual = expmap(init,delta); + VectorMap expected; expected.insert("x", Vector_(2, 1.1, 2.1)); expected.insert("y", Vector_(2, 3.1, 4.1)); @@ -163,45 +172,45 @@ TEST( VectorConfig, update_with_large_delta) { } /* ************************************************************************* */ -TEST( VectorConfig, dot) { - VectorConfig c = createVectorConfig(); +TEST( VectorMap, dot) { + VectorMap c = smallVectorMap(); DOUBLES_EQUAL(3.25,dot(c,c),1e-9); } /* ************************************************************************* */ -TEST( VectorConfig, dim) { - VectorConfig c = createVectorConfig(); +TEST( VectorMap, dim) { + VectorMap c = smallVectorMap(); LONGS_EQUAL(6,c.dim()); } /* ************************************************************************* */ -TEST( VectorConfig, operators) { - VectorConfig c; c.insert("x", Vector_(2, 1.1, 2.2)); - VectorConfig expected1; expected1.insert("x", Vector_(2, 2.2, 4.4)); +TEST( VectorMap, operators) { + VectorMap c; c.insert("x", Vector_(2, 1.1, 2.2)); + VectorMap expected1; expected1.insert("x", Vector_(2, 2.2, 4.4)); CHECK(assert_equal(expected1,c*2)); CHECK(assert_equal(expected1,c+c)); - VectorConfig expected2; expected2.insert("x", Vector_(2, 0.0, 0.0)); + VectorMap expected2; expected2.insert("x", Vector_(2, 0.0, 0.0)); CHECK(assert_equal(expected2,c-c)); } /* ************************************************************************* */ -TEST( VectorConfig, getReference) { - VectorConfig c; c.insert("x", Vector_(2, 1.1, 2.2)); - Vector& cx = c.getReference("x"); +TEST( VectorMap, getReference) { + VectorMap c; c.insert("x", Vector_(2, 1.1, 2.2)); + Vector& cx = c["x"]; cx = cx*2.0; - VectorConfig expected; expected.insert("x", Vector_(2, 2.2, 4.4)); + VectorMap expected; expected.insert("x", Vector_(2, 2.2, 4.4)); CHECK(assert_equal(expected,c)); } /* ************************************************************************* */ #ifdef HAVE_BOOST_SERIALIZATION -TEST( VectorConfig, serialize) +TEST( VectorMap, serialize) { //DEBUG: - cout << "VectorConfig: Running Serialization Test" << endl; + cout << "VectorMap: Running Serialization Test" << endl; - //create an VectorConfig - VectorConfig fg = createConfig(); + //create an VectorMap + VectorMap fg = createConfig(); //serialize the config std::ostringstream in_archive_stream; @@ -212,7 +221,7 @@ TEST( VectorConfig, serialize) //deserialize the config std::istringstream out_archive_stream(serialized_fgc); boost::archive::text_iarchive out_archive(out_archive_stream); - VectorConfig output; + VectorMap output; out_archive >> output; //check for equality diff --git a/cpp/timeVectorConfig.cpp b/cpp/timeVectorConfig.cpp index 70d29a156..fa21f908e 100644 --- a/cpp/timeVectorConfig.cpp +++ b/cpp/timeVectorConfig.cpp @@ -6,29 +6,18 @@ #include #include -#include "VectorConfig.h" +#include "VectorBTree.h" +#include "VectorMap.h" using namespace std; using namespace gtsam; -#define TIME(STATEMENT) { boost::timer t; \ - for (int j = 0; j < n; ++j) STATEMENT; \ +#define TIME1(STATEMENT) { boost::timer t; \ + STATEMENT; \ double time = t.elapsed(); \ cout << "Average elapsed time :" << 10e3 * time / n << "ms." << endl; } -/* ************************************************************************* */ -void unsafe_assign(VectorConfig& a, const VectorConfig& b) { - VectorConfig::const_iterator bp = b.begin(); - for (VectorConfig::iterator ap = a.begin(); ap != a.end(); ap++, bp++) - ap->second = bp->second; -} - -/* ************************************************************************* */ -void unsafe_add(VectorConfig& a, const VectorConfig& b) { - VectorConfig::const_iterator bp = b.begin(); - for (VectorConfig::iterator ap = a.begin(); ap != a.end(); ap++, bp++) - ap->second += bp->second; -} +#define TIME(STATEMENT) TIME1(for (int j = 0; j < n; ++j) STATEMENT;) /* ************************************************************************* */ int main(int argc, char ** argv) { @@ -39,35 +28,42 @@ int main(int argc, char ** argv) { // n = number of times to loop // m = number of vectors // r = rows per vector - size_t n = 100, m = 10000, r = 3, alpha = 0.1; + size_t n = 100, m = 30000, r = 3, alpha = 0.1; - // Create 2 VectorConfigs - VectorConfig x, y; - for (int i = 0; i < m; ++i) { - Vector v = zero(r); - Symbol key('x', i); - x.add(key, v); - y.add(key, v); + { + cout << "Vector:" << endl; + Vector v = zero(m * r), w = zero(m * r); + TIME(v=w); + TIME(v+=w); + TIME(v+=alpha*w); + TIME(axpy(alpha,v,w)); } - cout << "Convenient VectorConfig:" << endl; - TIME(x=y); - TIME(x+=y); - TIME(x+=alpha*y); - //TIME(a=a+b); + { + // Create 2 VectorBTrees and one VectorMap + VectorBTree p, q; + VectorMap old; + cout << "Creating VectorBTree:" << endl; + TIME1(for (int i = 0; i < m; ++i) { + Vector v = zero(r); + Symbol key('x', i); + p.insert(key, v); + q.insert(key, v); + old.insert(key, v); + }) - cout << "Unsafe VectorConfig:" << endl; - TIME(unsafe_assign(x,y)); - TIME(unsafe_add(x,y)); - TIME(axpy(alpha,x,y)); + cout << "VectorBTree:" << endl; + TIME(p=q); + TIME(p+=q); + TIME(p+=alpha*q); + TIME(axpy(alpha,p,q)); - cout << "Compares with Vector:" << endl; - Vector v = zero(m * r), w = zero(m * r); - TIME(v=w); - TIME(v+=w); - TIME(v+=alpha*w); - TIME(axpy(alpha,v,w)); - // TIME(v=v+w); + cout << "VectorBTree get:" << endl; + TIME1(for (int i = 0; i < m; ++i) p[Symbol('x', i)]); + + cout << "VectorMap get:" << endl; + TIME1(for (int i = 0; i < m; ++i) old[Symbol('x', i)]); +} return 0; }