diff --git a/.cproject b/.cproject index a668ce85f..50eef337c 100644 --- a/.cproject +++ b/.cproject @@ -322,14 +322,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -356,6 +348,7 @@ make + tests/testBayesTree.run true false @@ -363,6 +356,7 @@ make + testBinaryBayesNet.run true false @@ -410,6 +404,7 @@ make + testSymbolicBayesNet.run true false @@ -417,6 +412,7 @@ make + tests/testSymbolicFactor.run true false @@ -424,6 +420,7 @@ make + testSymbolicFactorGraph.run true false @@ -439,11 +436,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -478,7 +484,6 @@ make - testGraph.run true false @@ -574,7 +579,6 @@ make - testInference.run true false @@ -582,7 +586,6 @@ make - testGaussianBayesNet.run true false @@ -590,7 +593,6 @@ make - testGaussianFactor.run true false @@ -598,7 +600,6 @@ make - testJunctionTree.run true false @@ -606,7 +607,6 @@ make - testSymbolicBayesNet.run true false @@ -614,7 +614,6 @@ make - testSymbolicFactorGraph.run true false @@ -676,14 +675,6 @@ true true - - make - -j2 - check - true - true - true - make -j2 @@ -716,6 +707,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -772,6 +771,14 @@ true true + + make + -j2 + vSFMexample.run + true + true + true + make -j2 @@ -796,14 +803,6 @@ true true - - make - -j2 - vSFMexample.run - true - true - true - make -j2 @@ -1014,6 +1013,7 @@ make + testErrors.run true false @@ -1373,7 +1373,6 @@ make - testSimulated2DOriented.run true false @@ -1413,7 +1412,6 @@ make - testSimulated2D.run true false @@ -1421,7 +1419,6 @@ make - testSimulated3D.run true false @@ -1557,7 +1554,6 @@ make - tests/testGaussianISAM2 true false @@ -1675,54 +1671,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - make -j2 @@ -1755,6 +1703,14 @@ true true + + make + -j2 + check + true + true + true + @@ -2077,14 +2033,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2111,6 +2059,7 @@ make + tests/testBayesTree.run true false @@ -2118,6 +2067,7 @@ make + testBinaryBayesNet.run true false @@ -2165,6 +2115,7 @@ make + testSymbolicBayesNet.run true false @@ -2172,6 +2123,7 @@ make + tests/testSymbolicFactor.run true false @@ -2179,6 +2131,7 @@ make + testSymbolicFactorGraph.run true false @@ -2194,11 +2147,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2233,7 +2195,6 @@ make - testGraph.run true false @@ -2329,7 +2290,6 @@ make - testInference.run true false @@ -2337,7 +2297,6 @@ make - testGaussianBayesNet.run true false @@ -2345,7 +2304,6 @@ make - testGaussianFactor.run true false @@ -2353,7 +2311,6 @@ make - testJunctionTree.run true false @@ -2361,7 +2318,6 @@ make - testSymbolicBayesNet.run true false @@ -2369,7 +2325,6 @@ make - testSymbolicFactorGraph.run true false @@ -2431,14 +2386,6 @@ true true - - make - -j2 - check - true - true - true - make -j2 @@ -2471,6 +2418,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -2527,6 +2482,14 @@ true true + + make + -j2 + vSFMexample.run + true + true + true + make -j2 @@ -2551,14 +2514,6 @@ true true - - make - -j2 - vSFMexample.run - true - true - true - make -j2 @@ -2769,6 +2724,7 @@ make + testErrors.run true false @@ -3128,7 +3084,6 @@ make - testSimulated2DOriented.run true false @@ -3168,7 +3123,6 @@ make - testSimulated2D.run true false @@ -3176,7 +3130,6 @@ make - testSimulated3D.run true false @@ -3312,7 +3265,6 @@ make - tests/testGaussianISAM2 true false @@ -3430,54 +3382,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - make -j2 @@ -3510,6 +3414,14 @@ true true + + make + -j2 + check + true + true + true + diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 85742388f..bc4e4051c 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -47,11 +47,22 @@ namespace gtsam { template class Conditional: public gtsam::Factor, boost::noncopyable, public Testable > { -protected: +private: /** The first nFrontal variables are frontal and the rest are parents. */ size_t nrFrontals_; + /** Create keys by adding key in front */ + template + static std::vector MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) { + std::vector keys((lastParent - firstParent) + 1); + std::copy(firstParent, lastParent, keys.begin() + 1); + keys[0] = key; + return keys; + } + +protected: + // Calls the base class assertInvariants, which checks for unique keys void assertInvariants() const { Factor::assertInvariants(); } @@ -99,33 +110,16 @@ public: Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); } /** Constructor from a frontal variable and a vector of parents */ - Conditional(Key key, const std::vector& parents) : nrFrontals_(1) { - FactorType::keys_.resize(1 + parents.size()); - *(beginFrontals()) = key; - std::copy(parents.begin(), parents.end(), beginParents()); - assertInvariants(); - } + Conditional(Key key, const std::vector& parents) : + FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) { + assertInvariants(); + } - /** Constructor from a frontal variable and an iterator range of parents */ - template - static typename DERIVED::shared_ptr FromRange(Key key, ITERATOR firstParent, ITERATOR lastParent) { - typename DERIVED::shared_ptr conditional(new DERIVED); - conditional->nrFrontals_ = 1; - conditional->keys_.push_back(key); - std::copy(firstParent, lastParent, back_inserter(conditional->keys_)); - conditional->This::assertInvariants(); - return conditional; - } - - /** Named constructor from any number of frontal variables and parents */ - template - static typename DERIVED::shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { - typename DERIVED::shared_ptr conditional(new DERIVED); - conditional->nrFrontals_ = nrFrontals; - std::copy(firstKey, lastKey, back_inserter(conditional->keys_)); - conditional->This::assertInvariants(); - return conditional; - } + /** Constructor from keys and nr of frontal variables */ + Conditional(const std::vector& keys, size_t nrFrontals) : + FactorType(keys), nrFrontals_(nrFrontals) { + assertInvariants(); + } /** check equality */ template @@ -136,22 +130,22 @@ public: size_t nrFrontals() const { return nrFrontals_; } /** return the number of parents */ - size_t nrParents() const { return FactorType::keys_.size() - nrFrontals_; } + size_t nrParents() const { return FactorType::size() - nrFrontals_; } /** Special accessor when there is only one frontal variable. */ - Key key() const { assert(nrFrontals_==1); return FactorType::keys_[0]; } + Key key() const { assert(nrFrontals_==1); return FactorType::front(); } /** Iterators over frontal and parent variables. */ - const_iterator beginFrontals() const { return FactorType::keys_.begin(); } - const_iterator endFrontals() const { return FactorType::keys_.begin()+nrFrontals_; } - const_iterator beginParents() const { return FactorType::keys_.begin()+nrFrontals_; } - const_iterator endParents() const { return FactorType::keys_.end(); } + const_iterator beginFrontals() const { return FactorType::begin(); } + const_iterator endFrontals() const { return FactorType::begin()+nrFrontals_; } + const_iterator beginParents() const { return FactorType::begin()+nrFrontals_; } + const_iterator endParents() const { return FactorType::end(); } /** Mutable iterators and accessors */ - iterator beginFrontals() { return FactorType::keys_.begin(); } - iterator endFrontals() { return FactorType::keys_.begin()+nrFrontals_; } - iterator beginParents() { return FactorType::keys_.begin()+nrFrontals_; } - iterator endParents() { return FactorType::keys_.end(); } + iterator beginFrontals() { return FactorType::begin(); } + iterator endFrontals() { return FactorType::begin()+nrFrontals_; } + iterator beginParents() { return FactorType::begin()+nrFrontals_; } + iterator endParents() { return FactorType::end(); } boost::iterator_range frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); } boost::iterator_range parents() { return boost::make_iterator_range(beginParents(), endParents()); } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index ea394ca9d..bc56dfb31 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -70,11 +70,16 @@ public: /** Const iterator over keys */ typedef typename std::vector::const_iterator const_iterator; -protected: +private: // The keys involved in this factor std::vector keys_; + friend class JacobianFactor; + friend class HessianFactor; + +protected: + // Internal check to make sure keys are unique. If NDEBUG is defined, this // is empty and optimized out. void assertInvariants() const; @@ -111,10 +116,15 @@ public: keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } /** Construct n-way factor */ - Factor(const std::set& keys) { - BOOST_FOREACH(const Key& key, keys) - keys_.push_back(key); - assertInvariants(); } + Factor(const std::set& keys) { + BOOST_FOREACH(const Key& key, keys) keys_.push_back(key); + assertInvariants(); + } + + /** Construct n-way factor */ + Factor(const std::vector& keys) : keys_(keys) { + assertInvariants(); + } #ifdef TRACK_ELIMINATE /** @@ -157,6 +167,7 @@ public: /** * @return keys involved in this factor */ + std::vector& keys() { return keys_; } const std::vector& keys() const { return keys_; } /** diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index 7ade6ca11..a7e4d0890 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -68,7 +68,7 @@ namespace gtsam { } } #endif - BOOST_FOREACH(Index& key, keys_) + BOOST_FOREACH(Index& key, keys()) key = inversePermutation[key]; assertInvariants(); } diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index 2f379b030..8037d9e42 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -63,21 +63,15 @@ namespace gtsam { IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); } /** Constructor from a frontal variable and a vector of parents */ - IndexConditional(Index j, const std::vector& parents) : Base(j, parents) { assertInvariants(); } + IndexConditional(Index j, const std::vector& parents) : Base(j, parents) { + assertInvariants(); + } - /** Constructor from a frontal variable and an iterator range of parents */ - template - static shared_ptr FromRange(Index j, ITERATOR firstParent, ITERATOR lastParent) { - shared_ptr result(Base::FromRange(j, firstParent, lastParent)); - result->assertInvariants(); - return result; } - - /** Named constructor from any number of frontal variables and parents */ - template - static shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { - shared_ptr result(Base::FromRange(firstKey, lastKey, nrFrontals)); - result->assertInvariants(); - return result; } + /** Constructor from keys and nr of frontal variables */ + IndexConditional(const std::vector& keys, size_t nrFrontals) : + Base(keys, nrFrontals) { + assertInvariants(); + } /** Convert to a factor */ IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); } diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp index 6b49ff471..6f71599a3 100644 --- a/gtsam/inference/IndexFactor.cpp +++ b/gtsam/inference/IndexFactor.cpp @@ -64,7 +64,7 @@ namespace gtsam { /* ************************************************************************* */ void IndexFactor::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(Index& key, keys_) + BOOST_FOREACH(Index& key, keys()) key = inversePermutation[key]; assertInvariants(); } diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index 08f78cb17..bfe270eb5 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -103,6 +103,12 @@ namespace gtsam { assertInvariants(); } + /** Construct n-way factor */ + IndexFactor(const std::vector& js) : + Base(js) { + assertInvariants(); + } + #ifdef TRACK_ELIMINATE /** * eliminate the first variable involved in this factor diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index f23ddcd54..33bce0e0c 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -16,6 +16,8 @@ * Author: Frank Dellaert */ +#include + #include #include #include @@ -92,8 +94,10 @@ namespace gtsam { pair::shared_ptr, IndexFactor::shared_ptr> result; result.first.reset(new BayesNet ()); FastSet::const_iterator it; - for (it = keys.begin(); result.first->size() < nrFrontals; ++it) - result.first->push_back(IndexConditional::FromRange(it, keys.end(), 1)); + for (it = keys.begin(); result.first->size() < nrFrontals; ++it) { + std::vector newKeys(it,keys.end()); + result.first->push_back(boost::make_shared(newKeys, 1)); + } result.second.reset(new IndexFactor(it, keys.end())); return result; diff --git a/gtsam/inference/tests/testConditional.cpp b/gtsam/inference/tests/testConditional.cpp index 4b1c39d8b..33c1e3236 100644 --- a/gtsam/inference/tests/testConditional.cpp +++ b/gtsam/inference/tests/testConditional.cpp @@ -79,9 +79,9 @@ TEST( IndexConditional, fourParents ) /* ************************************************************************* */ TEST( IndexConditional, FromRange ) { - list keys; + vector keys; keys += 1,2,3,4,5; - IndexConditional::shared_ptr c0 = IndexConditional::FromRange(keys.begin(),keys.end(),2); + IndexConditional::shared_ptr c0(new IndexConditional(keys,2)); LONGS_EQUAL(2,c0->nrFrontals()) LONGS_EQUAL(3,c0->nrParents()) } diff --git a/gtsam/inference/tests/testSymbolicFactor.cpp b/gtsam/inference/tests/testSymbolicFactor.cpp index be37b921d..1be53aee0 100644 --- a/gtsam/inference/tests/testSymbolicFactor.cpp +++ b/gtsam/inference/tests/testSymbolicFactor.cpp @@ -33,7 +33,7 @@ TEST(SymbolicFactor, constructor) { // Frontals sorted, parents not sorted vector keys1; keys1 += 3, 4, 5, 9, 7, 8; - (void)IndexConditional::FromRange(keys1.begin(), keys1.end(), 3); + (void)IndexConditional(keys1, 3); // // Frontals not sorted // vector keys2; keys2 += 3, 5, 4, 9, 7, 8; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index c1649dfd4..30e402189 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -73,20 +73,16 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri } /* ************************************************************************* */ -GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const list >& parents, const Vector& sigmas) : - rsd_(matrix_), sigmas_(sigmas) { + GaussianConditional::GaussianConditional(Index key, const Vector& d, + const Matrix& R, const list >& parents, const Vector& sigmas) : + IndexConditional(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) { assert(R.size1() <= R.size2()); - IndexConditional::nrFrontals_ = 1; - keys_.resize(1+parents.size()); size_t dims[1+parents.size()+1]; dims[0] = R.size2(); - keys_[0] = key; size_t j=1; - for(std::list >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) { - keys_[j] = parent->first; + std::list >::const_iterator parent=parents.begin(); + for(; parent!=parents.end(); ++parent,++j) dims[j] = parent->second.size2(); - ++ j; - } dims[j] = 1; rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+1+parents.size()+1, d.size())); ublas::noalias(rsd_(0)) = ublas::triangular_adaptor(R); diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index e6f9a80c1..e1cd98e8b 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include #include @@ -157,19 +158,19 @@ private: // ar & BOOST_SERIALIZATION_NVP(d_); // ar & BOOST_SERIALIZATION_NVP(sigmas_); // } -}; +}; // GaussianConditional /* ************************************************************************* */ - template - GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, - size_t nrFrontals, const VerticalBlockView& matrices, - const Vector& sigmas) : - rsd_(matrix_), sigmas_(sigmas) { - nrFrontals_ = nrFrontals; - std::copy(firstKey, lastKey, back_inserter(keys_)); - rsd_.assignNoalias(matrices); - } - - /* ************************************************************************* */ - +// TODO: constructor outside class??? +template +GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, + size_t nrFrontals, const VerticalBlockView& matrices, + const Vector& sigmas) : + IndexConditional(std::vector(firstKey, lastKey), nrFrontals), rsd_( + matrix_), sigmas_(sigmas) { + rsd_.assignNoalias(matrices); } + +/* ************************************************************************* */ + +} // gtsam diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 579f2d4f4..9886d144a 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -70,6 +70,8 @@ namespace gtsam { /** Construct n-way factor */ GaussianFactor(const std::set& js) : IndexFactor(js) {} + /** Construct n-way factor */ + GaussianFactor(const std::vector& js) : IndexFactor(js) {} public: @@ -94,4 +96,13 @@ namespace gtsam { }; // GaussianFactor + /** make keys from list, vector, or map of matrices */ + template + static std::vector GetKeys(size_t n, ITERATOR begin, ITERATOR end) { + std::vector keys; + keys.reserve(n); + for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first); + return keys; + } + } // namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 9b783d84f..b90253c89 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -53,7 +53,7 @@ namespace gtsam { inline void JacobianFactor::assertInvariants() const { #ifndef NDEBUG IndexFactor::assertInvariants(); // The base class checks for sorted keys - assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks()); + assert((size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks()); assert(firstNonzeroBlocks_.size() == Ab_.size1()); for(size_t i=0; i > &terms, - const Vector &b, const SharedDiagonal& model) : - model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { - keys_.resize(terms.size()); + const Vector &b, const SharedDiagonal& model) : + GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), + model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) + { size_t dims[terms.size()+1]; - for(size_t j=0; j > &terms, const Vector &b, const SharedDiagonal& model) : - model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { - keys_.resize(terms.size()); + GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), + model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) + { size_t dims[terms.size()+1]; size_t j=0; - for(std::list >::const_iterator term=terms.begin(); term!=terms.end(); ++term) { - keys_[j] = term->first; + std::list >::const_iterator term=terms.begin(); + for(; term!=terms.end(); ++term,++j) dims[j] = term->second.size2(); - ++ j; - } dims[j] = 1; - firstNonzeroBlocks_.resize(b.size(), 0); Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size())); j = 0; - for(std::list >::const_iterator term=terms.begin(); term!=terms.end(); ++term) { + for(term=terms.begin(); term!=terms.end(); ++term,++j) Ab_(j) = term->second; - ++ j; - } getb() = b; assertInvariants(); } @@ -183,7 +178,7 @@ namespace gtsam { // Sort keys set vars; - for(size_t j=0; j(f_)); if (empty()) return (f.empty()); - if(keys_!=f.keys_ /*|| !model_->equals(lf->model_, tol)*/) + if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/) return false; assert(Ab_.size1() == f.Ab_.size1() && Ab_.size2() == f.Ab_.size2()); @@ -244,20 +239,20 @@ namespace gtsam { // Build a map from the new variable indices to the old slot positions. typedef FastMap SourceSlots; SourceSlots sourceSlots; - for(size_t j=0; j dimensions(keys_.size() + 1); + vector dimensions(size() + 1); size_t j = 0; BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { dimensions[j++] = Ab_(sourceSlot.second).size2(); } - assert(j == keys_.size()); + assert(j == size()); dimensions.back() = 1; // Copy the variables and matrix into the new order - vector oldKeys(keys_.size()); + vector oldKeys(size()); keys_.swap(oldKeys); AbMatrix oldMatrix; BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.size1()); @@ -279,7 +274,7 @@ namespace gtsam { Vector JacobianFactor::unweighted_error(const VectorValues& c) const { Vector e = -getb(); if (empty()) return e; - for(size_t pos=0; poswhiten(Ax); @@ -316,13 +311,13 @@ namespace gtsam { VectorValues& x) const { Vector E = alpha * model_->whiten(e); // Just iterate over all A matrices and insert Ai^e into VectorValues - for(size_t pos=0; pos JacobianFactor::matrix(bool weight) const { - Matrix A(Ab_.range(0, keys_.size())); + Matrix A(Ab_.range(0, size())); Vector b(getb()); // divide in sigma so error is indeed 0.5*|Ax-b| if (weight) model_->WhitenSystem(A,b); @@ -377,7 +372,7 @@ namespace gtsam { GaussianBayesNet::shared_ptr JacobianFactor::eliminate(size_t nrFrontals) { assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0); - assert(keys_.size() >= nrFrontals); + assert(size() >= nrFrontals); assertInvariants(); const bool debug = ISDEBUG("JacobianFactor::eliminate"); @@ -437,7 +432,7 @@ namespace gtsam { if(noiseModel->dim() < frontalDim) { throw domain_error((boost::format( "JacobianFactor is singular in variable %1%, discovered while attempting\n" - "to eliminate this variable.") % keys_.front()).str()); + "to eliminate this variable.") % front()).str()); } // Extract conditionals @@ -449,7 +444,7 @@ namespace gtsam { size_t varDim = Ab_(0).size2(); Ab_.rowEnd() = Ab_.rowStart() + varDim; const ublas::vector_range sigmas(noiseModel->sigmas(), ublas::range(Ab_.rowStart(), Ab_.rowEnd())); - conditionals->push_back(boost::make_shared(keys_.begin()+j, keys_.end(), 1, Ab_, sigmas)); + conditionals->push_back(boost::make_shared(begin()+j, end(), 1, Ab_, sigmas)); if(debug) conditionals->back()->print("Extracted conditional: "); Ab_.rowStart() += varDim; Ab_.firstBlock() += 1; @@ -461,7 +456,7 @@ namespace gtsam { tic(4, "remaining factor"); // Take lower-right block of Ab to get the new factor Ab_.rowEnd() = noiseModel->dim(); - keys_.assign(keys_.begin() + nrFrontals, keys_.end()); + keys_.assign(begin() + nrFrontals, end()); // Set sigmas with the right model if (noiseModel->isConstrained()) model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); @@ -477,7 +472,7 @@ namespace gtsam { firstNonzeroBlocks_.resize(this->size1()); for(size_t row=0; rowkeys_.push_back(key_); + NonlinearFactor1(const SharedGaussian& noiseModel, const KEY& key1) : + Base(noiseModel,key1), key_(key1) { } /* print */ @@ -291,12 +307,8 @@ namespace gtsam { * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1, - KEY2 j2) : - Base(noiseModel), key1_(j1), key2_(j2) { - this->keys_.reserve(2); - this->keys_.push_back(key1_); - this->keys_.push_back(key2_); + NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2) : + Base(noiseModel,j1,j2), key1_(j1), key2_(j2) { } virtual ~NonlinearFactor2() {}