diff --git a/gtsam.h b/gtsam.h index f5d163141..defc88c20 100644 --- a/gtsam.h +++ b/gtsam.h @@ -724,7 +724,6 @@ class Permutation { // FIXME: Cannot currently wrap std::vector //static gtsam::Permutation PullToFront(const vector& toFront, size_t size, bool filterDuplicates); //static gtsam::Permutation PushToBack(const vector& toBack, size_t size, bool filterDuplicates = false); - gtsam::Permutation* partialPermutation(const gtsam::Permutation& selector, const gtsam::Permutation& partialPermutation) const; }; class IndexFactor { @@ -772,7 +771,6 @@ class IndexConditional { gtsam::IndexFactor* toFactor() const; //Advanced interface - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); void permuteWithInverse(const gtsam::Permutation& inversePermutation); }; @@ -795,7 +793,6 @@ virtual class BayesNet { void push_front(This& conditional); void pop_front(); void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; #include @@ -840,7 +837,6 @@ virtual class BayesTreeClique { // derived_ptr parent() const { return parent_.lock(); } void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); // FIXME: need wrapped versions graphs, BayesNet // BayesNet shortcut(derived_ptr root, Eliminate function) const; @@ -868,7 +864,6 @@ virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { //Advanced Interface void pop_front(); void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; typedef gtsam::BayesTreeClique SymbolicBayesTreeClique; @@ -1050,12 +1045,11 @@ class VectorValues { //Standard Interface size_t size() const; size_t dim(size_t j) const; - size_t dim() const; bool exists(size_t j) const; void print(string s) const; bool equals(const gtsam::VectorValues& expected, double tol) const; void insert(size_t j, Vector value); - Vector vector() const; + Vector asVector() const; Vector at(size_t j) const; //Advanced Interface @@ -1821,7 +1815,6 @@ virtual class ISAM2Clique { void print(string s); void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; class ISAM2Result { diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 0fe78fbed..903826e3a 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -117,18 +117,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - template - bool BayesNet::permuteSeparatorWithInverse( - const Permutation& inversePermutation) { - bool separatorChanged = false; - BOOST_FOREACH(sharedConditional conditional, conditionals_) { - if (conditional->permuteSeparatorWithInverse(inversePermutation)) - separatorChanged = true; - } - return separatorChanged; - } - /* ************************************************************************* */ template void BayesNet::push_back(const BayesNet& bn) { diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 1077c1cad..d3998656a 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -220,13 +220,6 @@ public: /** Permute the variables in the BayesNet */ void permuteWithInverse(const Permutation& inversePermutation); - /** - * Permute the variables when only separator variables need to be permuted. - * Returns true if any reordered variables appeared in the separator and - * false if not. - */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); - iterator begin() {return conditionals_.begin();} /// Nodes; + typedef std::vector Nodes; protected: diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index c9d8e1f5c..5f01aa395 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -134,22 +134,19 @@ namespace gtsam { /* ************************************************************************* */ template - bool BayesTreeCliqueBase::permuteSeparatorWithInverse( - const Permutation& inversePermutation) { - bool changed = conditional_->permuteSeparatorWithInverse( - inversePermutation); + bool BayesTreeCliqueBase::reduceSeparatorWithInverse( + const internal::Reduction& inverseReduction) + { + bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); #ifndef NDEBUG if(!changed) { - BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);} BOOST_FOREACH(const derived_ptr& child, children_) { - assert(child->permuteSeparatorWithInverse(inversePermutation) == false); - } + assert(child->reduceSeparatorWithInverse(inverseReduction) == false); } } #endif - if (changed) { + if(changed) { BOOST_FOREACH(const derived_ptr& child, children_) { - (void) child->permuteSeparatorWithInverse(inversePermutation); - } + (void) child->reduceSeparatorWithInverse(inverseReduction); } } assertInvariants(); return changed; diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 487d10a0d..3a9a1ef78 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -183,7 +183,7 @@ namespace gtsam { * traversing the whole tree. Returns whether any separator variables in * this subtree were reordered. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); /** return the conditional P(S|Root) on the separator given the root */ BayesNet shortcut(derived_ptr root, Eliminate function) const; diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index e0993a9a1..4167e56f2 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -44,16 +44,16 @@ namespace gtsam { } /* ************************************************************************* */ - bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - #ifndef NDEBUG - BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } - #endif + bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { +#ifndef NDEBUG + BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); } +#endif bool parentChanged = false; BOOST_FOREACH(KeyType& parent, parents()) { - KeyType newParent = inversePermutation[parent]; - if(parent != newParent) { + internal::Reduction::const_iterator it = inverseReduction.find(parent); + if(it != inverseReduction.end()) { parentChanged = true; - parent = newParent; + parent = it->second; } } assertInvariants(); diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index 895592547..9643b1a84 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -112,7 +112,13 @@ namespace gtsam { * Returns true if any reordered variables appeared in the separator and * false if not. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** Permute the variables when only separator variables need to be permuted. + * Returns true if any reordered variables appeared in the separator and + * false if not. + */ + bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); /** * Permutes the Conditional, but for efficiency requires the permutation diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp index 883041b86..94528f64f 100644 --- a/gtsam/inference/Permutation.cpp +++ b/gtsam/inference/Permutation.cpp @@ -123,18 +123,6 @@ Permutation::shared_ptr Permutation::permute(const Permutation& permutation) con return result; } -/* ************************************************************************* */ -Permutation::shared_ptr Permutation::partialPermutation( - const Permutation& selector, const Permutation& partialPermutation) const { - assert(selector.size() == partialPermutation.size()); - Permutation::shared_ptr result(new Permutation(*this)); - - for(Index subsetPos=0; subsetPossize())); @@ -161,6 +149,16 @@ namespace internal { return result; } + /* ************************************************************************* */ + Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) { + if(selector.size() != p.size()) + throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes"); + Reduction result; + for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot) + result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]])); + return result; + } + /* ************************************************************************* */ void Reduction::applyInverse(std::vector& js) const { BOOST_FOREACH(Index& j, js) { @@ -183,10 +181,10 @@ namespace internal { } /* ************************************************************************* */ - Index& Reduction::operator[](const Index& j) { + const Index& Reduction::operator[](const Index& j) { iterator it = this->find(j); if(it == this->end()) - throw std::out_of_range("Index to Reduction::operator[] not present"); + return j; else return it->second; } @@ -195,7 +193,7 @@ namespace internal { const Index& Reduction::operator[](const Index& j) const { const_iterator it = this->find(j); if(it == this->end()) - throw std::out_of_range("Index to Reduction::operator[] not present"); + return j; else return it->second; } @@ -207,6 +205,11 @@ namespace internal { cout << " " << p.first << " : " << p.second << endl; } + /* ************************************************************************* */ + bool Reduction::equals(const Reduction& other, double tol) const { + return (const Base&)(*this) == (const Base&)other; + } + /* ************************************************************************* */ Permutation createReducingPermutation(const std::set& indices) { Permutation p(indices.size()); diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 6014d704e..985cc192f 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -167,14 +167,6 @@ public: /// @name Advanced Interface /// @{ - - /** - * A partial permutation, reorders the variables selected by selector through - * partialPermutation. selector and partialPermutation should have the same - * size, this is checked if NDEBUG is not defined. - */ - Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const; - iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices @@ -192,12 +184,14 @@ namespace internal { typedef gtsam::FastMap Base; static Reduction CreateAsInverse(const Permutation& p); + static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p); void applyInverse(std::vector& js) const; Permutation inverse() const; - Index& operator[](const Index& j); + const Index& operator[](const Index& j); const Index& operator[](const Index& j) const; void print(const std::string& s="") const; + bool equals(const Reduction& other, double tol = 1e-9) const; }; // Reduce the variable indices so that those in the set are mapped to start at zero diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index e6e84cf1c..11ce68ec6 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -68,7 +68,7 @@ void VariableIndex::outputMetisFormat(ostream& os) const { /* ************************************************************************* */ void VariableIndex::permuteInPlace(const Permutation& permutation) { // Create new index and move references to data into it in permuted order - deque newIndex(this->size()); + vector newIndex(this->size()); for(Index i = 0; i < newIndex.size(); ++i) newIndex[i].swap(this->index_[permutation[i]]); @@ -76,6 +76,20 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) { index_.swap(newIndex); } +/* ************************************************************************* */ +void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) { + if(selector.size() != permutation.size()) + throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); + // Create new index the size of the permuted entries + vector newIndex(selector.size()); + // Permute the affected entries into the new index + for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) + newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]); + // Put the affected entries back in the new order + for(size_t slot = 0; slot < selector.size(); ++slot) + this->index_[selector[slot]].swap(newIndex[slot]); +} + /* ************************************************************************* */ void VariableIndex::removeUnusedAtEnd(size_t nToRemove) { #ifndef NDEBUG diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index de863454d..149f8eaaf 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -48,7 +48,7 @@ public: typedef Factors::const_iterator Factor_const_iterator; protected: - std::deque index_; + std::vector index_; size_t nFactors_; // Number of factors in the original factor graph. size_t nEntries_; // Sum of involved variable counts of each factor. @@ -135,6 +135,9 @@ public: /// Permute the variables in the VariableIndex according to the given permutation void permuteInPlace(const Permutation& permutation); + /// Permute the variables in the VariableIndex according to the given partial permutation + void permuteInPlace(const Permutation& selector, const Permutation& permutation); + /** Remove unused empty variables at the end of the ordering (in debug mode * verifies they are empty). * @param nToRemove The number of unused variables at the end to remove diff --git a/gtsam/inference/tests/testPermutation.cpp b/gtsam/inference/tests/testPermutation.cpp index 1ccafb1a2..98e94b4f7 100644 --- a/gtsam/inference/tests/testPermutation.cpp +++ b/gtsam/inference/tests/testPermutation.cpp @@ -25,6 +25,7 @@ using namespace gtsam; using namespace std; +/* ************************************************************************* */ TEST(Permutation, Identity) { Permutation expected(5); expected[0] = 0; @@ -38,6 +39,7 @@ TEST(Permutation, Identity) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ TEST(Permutation, PullToFront) { Permutation expected(5); expected[0] = 4; @@ -55,6 +57,7 @@ TEST(Permutation, PullToFront) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ TEST(Permutation, PushToBack) { Permutation expected(5); expected[0] = 1; @@ -72,6 +75,7 @@ TEST(Permutation, PushToBack) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ TEST(Permutation, compose) { Permutation p1(5); p1[0] = 1; @@ -104,6 +108,25 @@ TEST(Permutation, compose) { LONGS_EQUAL(p1[p2[4]], actual[4]); } +/* ************************************************************************* */ +TEST(Reduction, CreateFromPartialPermutation) { + Permutation selector(3); + selector[0] = 2; + selector[1] = 4; + selector[2] = 6; + Permutation p(3); + p[0] = 2; + p[1] = 0; + p[2] = 1; + + internal::Reduction expected; + expected.insert(make_pair(2,6)); + expected.insert(make_pair(4,2)); + expected.insert(make_pair(6,4)); + + internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 8f7d4234e..5a78b35c7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -607,11 +607,11 @@ break; /* ************************************************************************* */ void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - r.vector() = Vector::Zero(r.dim()); + r.setZero(); Index i = 0; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - SubVector &y = r[i]; + Vector &y = r[i]; for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { y += Ai->getA(j) * x[*j]; } @@ -621,7 +621,7 @@ break; /* ************************************************************************* */ void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { - x.vector() = Vector::Zero(x.dim()); + x.setZero(); Index i = 0; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index ea1529e16..2d8e15099 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -30,11 +30,11 @@ namespace gtsam { class GaussianISAM : public ISAM { typedef ISAM Super; - std::deque > dims_; + std::vector dims_; public: - typedef std::deque > Dims; + typedef std::vector Dims; /** Create an empty Bayes Tree */ GaussianISAM() : Super() {} diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 528a16b4a..814d4eede 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -325,17 +325,11 @@ double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) const double f = constantTerm(); double xtg = 0, xGx = 0; - if (c.dim() == this->rows()) { - // If using the full vector values, this will reduce copying - xtg = c.vector().dot(linearTerm()); - xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * c.vector(); - } else { - // extract the relevant subset of the VectorValues - // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); - xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; - } + // extract the relevant subset of the VectorValues + // NOTE may not be as efficient + const Vector x = c.vector(this->keys()); + xtg = x.dot(linearTerm()); + xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; return 0.5 * (f - 2.0 * xtg + xGx); } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index efe642d93..3c6c7133f 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -20,29 +20,19 @@ #include #include +#include + using namespace std; namespace gtsam { -/* ************************************************************************* */ -VectorValues::VectorValues(const VectorValues& other) { - *this = other; -} - -/* ************************************************************************* */ -VectorValues& VectorValues::operator=(const VectorValues& rhs) { - if(this != &rhs) { - resizeLike(rhs); // Copy structure - values_ = rhs.values_; // Copy values - } - return *this; -} - /* ************************************************************************* */ VectorValues VectorValues::Zero(const VectorValues& x) { - VectorValues cloned(SameStructure(x)); - cloned.setZero(); - return cloned; + VectorValues result; + result.values_.resize(x.size()); + for(size_t j=0; j dimensions(size()); - for(size_t k=0; k= size()) - dimensions.insert(dimensions.end(), j+1-size(), 0); + values_.resize(j+1); - // Set correct dimension for j - dimensions[j] = value.rows(); - - // Make a copy to make assignment easier - VectorValues original(*this); - - // Resize to accomodate new variable - resize(dimensions); - - // Copy original variables - for(Index k = 0; k < original.size(); ++k) - if(k != j && exists(k)) - operator[](k) = original[k]; - - // Copy new variable - operator[](j) = value; + // Assign value + values_[j] = value; } /* ************************************************************************* */ void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const { std::cout << str << ": " << size() << " elements\n"; for (Index var = 0; var < size(); ++var) - std::cout << " " << formatter(var) << ": \n" << operator[](var) << "\n"; + std::cout << " " << formatter(var) << ": \n" << (*this)[var] << "\n"; std::cout.flush(); } /* ************************************************************************* */ bool VectorValues::equals(const VectorValues& x, double tol) const { - return hasSameStructure(x) && equal_with_abs_tol(values_, x.values_, tol); + if(this->size() != x.size()) + return false; + for(Index j=0; j < size(); ++j) + if(!equal_with_abs_tol(values_[j], x.values_[j], tol)) + return false; + return true; } /* ************************************************************************* */ void VectorValues::resize(Index nVars, size_t varDim) { - maps_.clear(); - maps_.reserve(nVars); - values_.resize(nVars * varDim); - int varStart = 0; - for (Index j = 0; j < nVars; ++j) { - maps_.push_back(values_.segment(varStart, varDim)); - varStart += varDim; - } + values_.resize(nVars); + for(Index j = 0; j < nVars; ++j) + values_[j] = Vector(varDim); } /* ************************************************************************* */ void VectorValues::resizeLike(const VectorValues& other) { - values_.resize(other.dim()); - // Create SubVectors referencing our values_ vector - maps_.clear(); - maps_.reserve(other.size()); - int varStart = 0; - BOOST_FOREACH(const SubVector& value, other) { - maps_.push_back(values_.segment(varStart, value.rows())); - varStart += value.rows(); - } + values_.resize(other.size()); + for(Index j = 0; j < other.size(); ++j) + values_[j].resize(other.values_[j].size()); } /* ************************************************************************* */ @@ -140,91 +105,133 @@ VectorValues VectorValues::Zero(Index nVars, size_t varDim) { /* ************************************************************************* */ void VectorValues::setZero() { - values_.setZero(); + BOOST_FOREACH(Vector& v, *this) { + v.setZero(); + } +} + +/* ************************************************************************* */ +const Vector VectorValues::asVector() const { + return internal::extractVectorValuesSlices(*this, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true); +} + +/* ************************************************************************* */ +const Vector VectorValues::vector(const std::vector& indices) const { + return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end()); } /* ************************************************************************* */ bool VectorValues::hasSameStructure(const VectorValues& other) const { if(this->size() != other.size()) return false; - for(size_t j=0; jmaps_[j].rows() != other.maps_[j].rows()) + if(this->values_[j].rows() != other.values_[j].rows()) return false; return true; } /* ************************************************************************* */ -VectorValues VectorValues::operator+(const VectorValues& c) const { - assert(this->hasSameStructure(c)); - VectorValues result(SameStructure(c)); - result.values_ = this->values_ + c.values_; - return result; +void VectorValues::permuteInPlace(const Permutation& permutation) { + // Allocate new values + Values newValues(this->size()); + + // Swap values from this VectorValues to the permuted VectorValues + for(size_t i = 0; i < this->size(); ++i) + newValues[i].swap(this->at(permutation[i])); + + // Swap the values into this VectorValues + values_.swap(newValues); } /* ************************************************************************* */ -VectorValues VectorValues::operator-(const VectorValues& c) const { - assert(this->hasSameStructure(c)); - VectorValues result(SameStructure(c)); - result.values_ = this->values_ - c.values_; - return result; -} - -/* ************************************************************************* */ -void VectorValues::operator+=(const VectorValues& c) { - assert(this->hasSameStructure(c)); - this->values_ += c.values_; -} - -/* ************************************************************************* */ -VectorValues VectorValues::permute(const Permutation& permutation) const { - // Create result and allocate space - VectorValues lhs; - lhs.values_.resize(this->dim()); - lhs.maps_.reserve(this->size()); - - // Copy values from this VectorValues to the permuted VectorValues - size_t lhsPos = 0; - for(size_t i = 0; i < this->size(); ++i) { - // Map the next LHS subvector to the next slice of the LHS vector - lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size())); - // Copy the data from the RHS subvector to the LHS subvector - lhs.maps_[i] = this->at(permutation[i]); - // Increment lhs position - lhsPos += lhs.maps_[i].size(); - } - - return lhs; +void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation) { + if(selector.size() != permutation.size()) + throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); + // Create new index the size of the permuted entries + Values reorderedEntries(selector.size()); + // Permute the affected entries into the new index + for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) + reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]); + // Put the affected entries back in the new order + for(size_t slot = 0; slot < selector.size(); ++slot) + values_[selector[slot]].swap(reorderedEntries[slot]); } /* ************************************************************************* */ void VectorValues::swap(VectorValues& other) { this->values_.swap(other.values_); - this->maps_.swap(other.maps_); } /* ************************************************************************* */ -Vector VectorValues::vector(const std::vector& indices) const { - if (indices.empty()) - return Vector(); - - // find dimensions - size_t d = 0; - BOOST_FOREACH(const Index& idx, indices) - d += dim(idx); - - // copy out values - Vector result(d); - size_t curHead = 0; - BOOST_FOREACH(const Index& j, indices) { - const SubVector& vj = at(j); - size_t dj = (size_t) vj.rows(); - result.segment(curHead, dj) = vj; - curHead += dj; - } +double VectorValues::dot(const VectorValues& v) const { + double result = 0.0; + if(this->size() != v.size()) + throw invalid_argument("VectorValues::dot called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == v.values_[j].size()) + result += this->values_[j].dot(v.values_[j]); + else + throw invalid_argument("VectorValues::dot called with different vector sizes"); return result; } +/* ************************************************************************* */ +double VectorValues::norm() const { + return std::sqrt(this->squaredNorm()); +} + +/* ************************************************************************* */ +double VectorValues::squaredNorm() const { + double sumSquares = 0.0; + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + sumSquares += this->values_[j].squaredNorm(); + return sumSquares; +} + +/* ************************************************************************* */ +VectorValues VectorValues::operator+(const VectorValues& c) const { + VectorValues result = SameStructure(*this); + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+ called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == c.values_[j].size()) + result.values_[j] = this->values_[j] + c.values_[j]; + else + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + return result; +} + +/* ************************************************************************* */ +VectorValues VectorValues::operator-(const VectorValues& c) const { + VectorValues result = SameStructure(*this); + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == c.values_[j].size()) + result.values_[j] = this->values_[j] - c.values_[j]; + else + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + return result; +} + +/* ************************************************************************* */ +void VectorValues::operator+=(const VectorValues& c) { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+= called with different vector sizes"); + for(Index j = 0; j < this->size(); ++j) + // Directly accessing maps instead of using VV::dim in case some values are empty + if(this->values_[j].size() == c.values_[j].size()) + this->values_[j] += c.values_[j]; + else + throw invalid_argument("VectorValues::operator+= called with different vector sizes"); +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 1f6e4cd1d..1e63c5bb5 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -93,15 +93,14 @@ namespace gtsam { */ class VectorValues { protected: - Vector values_; ///< The underlying vector storing the values - typedef std::vector ValueMaps; ///< Collection of SubVector s - ValueMaps maps_; ///< SubVector s referencing each vector variable in values_ + typedef std::vector Values; ///< Typedef for the collection of Vectors making up a VectorValues + Values values_; ///< Collection of Vectors making up this VectorValues public: - typedef ValueMaps::iterator iterator; ///< Iterator over vector values - typedef ValueMaps::const_iterator const_iterator; ///< Const iterator over vector values - typedef ValueMaps::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - typedef ValueMaps::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values + typedef Values::iterator iterator; ///< Iterator over vector values + typedef Values::const_iterator const_iterator; ///< Const iterator over vector values + typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values + typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class /// @name Standard Constructors @@ -112,11 +111,8 @@ namespace gtsam { */ VectorValues() {} - /** Copy constructor */ - VectorValues(const VectorValues &other); - /** Named constructor to create a VectorValues of the same structure of the - * specifed one, but filled with zeros. + * specified one, but filled with zeros. * @return */ static VectorValues Zero(const VectorValues& model); @@ -127,55 +123,49 @@ namespace gtsam { /** Number of variables stored, always 1 more than the highest variable index, * even if some variables with lower indices are not present. */ - Index size() const { return maps_.size(); } + Index size() const { return values_.size(); } /** Return the dimension of variable \c j. */ size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } - /** Return the summed dimensionality of all variables. */ - size_t dim() const { return values_.rows(); } - /** Return the dimension of each vector in this container */ std::vector dims() const; /** Check whether a variable with index \c j exists. */ - bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } + bool exists(Index j) const { return j < size() && values_[j].rows() > 0; } /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ - SubVector& at(Index j) { checkExists(j); return maps_[j]; } + Vector& at(Index j) { checkExists(j); return values_[j]; } /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ - const SubVector& at(Index j) const { checkExists(j); return maps_[j]; } + const Vector& at(Index j) const { checkExists(j); return values_[j]; } /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */ - SubVector& operator[](Index j) { return at(j); } + Vector& operator[](Index j) { return at(j); } /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */ - const SubVector& operator[](Index j) const { return at(j); } + const Vector& operator[](Index j) const { return at(j); } /** Insert a vector \c value with index \c j. - * Causes reallocation. Can be used to insert values in any order, but - * throws an invalid_argument exception if the index \c j is already used. + * Causes reallocation, but can insert values in any order. + * Throws an invalid_argument exception if the index \c j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ void insert(Index j, const Vector& value); - /** Assignment */ - VectorValues& operator=(const VectorValues& rhs); - - iterator begin() { chk(); return maps_.begin(); } ///< Iterator over variables - const_iterator begin() const { chk(); return maps_.begin(); } ///< Iterator over variables - iterator end() { chk(); return maps_.end(); } ///< Iterator over variables - const_iterator end() const { chk(); return maps_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables - const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables - const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Reverse iterator over variables + iterator begin() { return values_.begin(); } ///< Iterator over variables + const_iterator begin() const { return values_.begin(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables + const_iterator end() const { return values_.end(); } ///< Iterator over variables + reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables + const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables + reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables + const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables /** print required by Testable for unit testing */ void print(const std::string& str = "VectorValues: ", - const IndexFormatter& formatter =DefaultIndexFormatter) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; @@ -187,10 +177,10 @@ namespace gtsam { /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ template - explicit VectorValues(const CONTAINER& dimensions) { append(dimensions); } + explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); } /** Construct to hold nVars vectors of varDim dimension each. */ - VectorValues(Index nVars, size_t varDim) { resize(nVars, varDim); } + VectorValues(Index nVars, size_t varDim) { this->resize(nVars, varDim); } /** Named constructor to create a VectorValues that matches the structure of * the specified VectorValues, but do not initialize the new values. */ @@ -222,17 +212,16 @@ namespace gtsam { void resizeLike(const VectorValues& other); /** Resize the VectorValues to hold \c nVars variables, each of dimension - * \c varDim, not preserving any data. After calling this function, all - * variables will be uninitialized. + * \c varDim. Any individual vectors that do not change size will keep + * their values, but any new or resized vectors will be uninitialized. * @param nVars The number of variables to create * @param varDim The dimension of each variable */ void resize(Index nVars, size_t varDim); /** Resize the VectorValues to contain variables of the dimensions stored - * in \c dimensions, not preserving any data. The new variables are - * uninitialized, but this function is used to pre-allocate space for - * performance. After calling this function all variables will be uninitialized. + * in \c dimensions. Any individual vectors that do not change size will keep + * their values, but any new or resized vectors will be uninitialized. * @param dimensions A container of the dimension of each variable to create. */ template @@ -251,14 +240,11 @@ namespace gtsam { /** Set all entries to zero, does not modify the size. */ void setZero(); - /** Reference the entire solution vector (const version). */ - const Vector& vector() const { chk(); return values_; } - - /** Reference the entire solution vector. */ - Vector& vector() { chk(); return values_; } + /** Retrieve the entire solution as a single vector */ + const Vector asVector() const; /** Access a vector that is a subset of relevant indices */ - Vector vector(const std::vector& indices) const; + const Vector vector(const std::vector& indices) const; /** Check whether this VectorValues has the same structure, meaning has the * same number of variables and that all variables are of the same dimension, @@ -268,16 +254,34 @@ namespace gtsam { */ bool hasSameStructure(const VectorValues& other) const; + /** + * Permute the variables in the VariableIndex according to the given partial permutation + */ + void permuteInPlace(const Permutation& selector, const Permutation& permutation); + + /** + * Permute the entries of this VectorValues in place + */ + void permuteInPlace(const Permutation& permutation); + + /** + * Swap the data in this VectorValues with another. + */ + void swap(VectorValues& other); + + /// @} + /// @name Linear algebra operations + /// @{ + /** Dot product with another VectorValues, interpreting both as vectors of * their concatenated values. */ - double dot(const VectorValues& V) const { - return gtsam::dot(this->values_, V.values_); - } + double dot(const VectorValues& v) const; /** Vector L2 norm */ - inline double norm() const { - return this->vector().norm(); - } + double norm() const; + + /** Squared vector L2 norm */ + double squaredNorm() const; /** * + operator does element-wise addition. Both VectorValues must have the @@ -297,26 +301,11 @@ namespace gtsam { */ void operator+=(const VectorValues& c); - /** - * Permute the entries of this VectorValues, returns a new VectorValues as - * the result. - */ - VectorValues permute(const Permutation& permutation) const; - - /** - * Swap the data in this VectorValues with another. - */ - void swap(VectorValues& other); - /// @} private: - // Verifies that the underlying Vector is consistent with the collection of SubVectors - void chk() const; - // Throw an exception if j does not exist - inline void checkExists(Index j) const { - chk(); + void checkExists(Index j) const { if(!exists(j)) { const std::string msg = (boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str(); @@ -329,73 +318,63 @@ namespace gtsam { /** * scale a vector by a scalar */ - friend VectorValues operator*(const double a, const VectorValues &V) { - VectorValues result(VectorValues::SameStructure(V)); - result.values_ = a * V.values_; + friend VectorValues operator*(const double a, const VectorValues &v) { + VectorValues result = VectorValues::SameStructure(v); + for(Index j = 0; j < v.size(); ++j) + result.values_[j] = a * v.values_[j]; return result; } - /// TODO: linear algebra interface seems to have been added for SPCG. - friend size_t dim(const VectorValues& V) { - return V.dim(); - } - /// TODO: linear algebra interface seems to have been added for SPCG. - friend double dot(const VectorValues& V1, const VectorValues& V2) { - return gtsam::dot(V1.values_, V2.values_); - } /// TODO: linear algebra interface seems to have been added for SPCG. friend void scal(double alpha, VectorValues& x) { - gtsam::scal(alpha, x.values_); + for(Index j = 0; j < x.size(); ++j) + x.values_[j] *= alpha; } /// TODO: linear algebra interface seems to have been added for SPCG. friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { - gtsam::axpy(alpha, x.values_, y.values_); + if(x.size() != y.size()) + throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + for(Index j = 0; j < x.size(); ++j) + if(x.values_[j].size() == y.values_[j].size()) + y.values_[j] += alpha * x.values_[j]; + else + throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); } /// TODO: linear algebra interface seems to have been added for SPCG. friend void sqrt(VectorValues &x) { - Vector y = gtsam::esqrt(x.values_); - x.values_ = y; + for(Index j = 0; j < x.size(); ++j) + x.values_[j] = x.values_[j].cwiseSqrt(); } /// TODO: linear algebra interface seems to have been added for SPCG. - friend void ediv(const VectorValues& numerator, - const VectorValues& denominator, VectorValues &result) { - assert( - numerator.dim() == denominator.dim() && denominator.dim() == result.dim()); - const size_t sz = result.dim(); - for (size_t i = 0; i < sz; ++i) - result.values_[i] = numerator.values_[i] / denominator.values_[i]; + friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { + if(numerator.size() != denominator.size() || numerator.size() != result.size()) + throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + for(Index j = 0; j < numerator.size(); ++j) + if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) + result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); + else + throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); } /// TODO: linear algebra interface seems to have been added for SPCG. friend void edivInPlace(VectorValues& x, const VectorValues& y) { - assert(x.dim() == y.dim()); - const size_t sz = x.dim(); - for (size_t i = 0; i < sz; ++i) - x.values_[i] /= y.values_[i]; + if(x.size() != y.size()) + throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + for(Index j = 0; j < x.size(); ++j) + if(x.values_[j].size() == y.values_[j].size()) + x.values_[j].array() /= y.values_[j].array(); + else + throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); } private: /** Serialization function */ friend class boost::serialization::access; template - void save(ARCHIVE & ar, const unsigned int version) const { - // The maps_ stores pointers, so we serialize dimensions instead - std::vector dimensions(size()); - for(size_t j=0; j - void load(ARCHIVE & ar, const unsigned int version) { - std::vector dimensions; - ar & BOOST_SERIALIZATION_NVP(dimensions); // Load dimensions - resize(dimensions); // Allocate space for everything - ar & BOOST_SERIALIZATION_NVP(values_); // Load values - chk(); - } - BOOST_SERIALIZATION_SPLIT_MEMBER() }; // VectorValues definition // Implementations of template and inline functions @@ -403,86 +382,75 @@ namespace gtsam { /* ************************************************************************* */ template void VectorValues::resize(const CONTAINER& dimensions) { - maps_.clear(); - values_.resize(0); + values_.clear(); append(dimensions); } /* ************************************************************************* */ template void VectorValues::append(const CONTAINER& dimensions) { - chk(); - int newDim = std::accumulate(dimensions.begin(), dimensions.end(), 0); // Sum of dimensions - values_.conservativeResize(dim() + newDim); - // Relocate existing maps - int varStart = 0; - for(size_t j = 0; j < maps_.size(); ++j) { - new (&maps_[j]) SubVector(values_.segment(varStart, maps_[j].rows())); - varStart += maps_[j].rows(); - } - maps_.reserve(maps_.size() + dimensions.size()); + size_t i = size(); + values_.resize(size() + dimensions.size()); BOOST_FOREACH(size_t dim, dimensions) { - maps_.push_back(values_.segment(varStart, dim)); - varStart += (int)dim; // varStart is continued from first for loop + values_[i] = Vector(dim); + ++ i; } } /* ************************************************************************* */ template VectorValues VectorValues::Zero(const CONTAINER& dimensions) { - VectorValues ret(dimensions); - ret.setZero(); - return ret; - } - - /* ************************************************************************* */ - inline void VectorValues::chk() const { -#ifndef NDEBUG - // Check that the first SubVector points to the beginning of the Vector - if(maps_.size() > 0) { - assert(values_.data() == maps_[0].data()); - // Check that the end of the last SubVector points to the end of the Vector - assert(values_.rows() == maps_.back().data() + maps_.back().rows() - maps_.front().data()); + VectorValues ret; + ret.values_.resize(dimensions.size()); + size_t i = 0; + BOOST_FOREACH(size_t dim, dimensions) { + ret.values_[i] = Vector::Zero(dim); + ++ i; } -#endif + return ret; } namespace internal { - /* ************************************************************************* */ - // Helper function, extracts vectors with variable indices - // in the first and last iterators, and concatenates them in that order into the - // output. - template - Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) { - // Find total dimensionality - int dim = 0; - for(ITERATOR j = first; j != last; ++j) - dim += values[*j].rows(); + /* ************************************************************************* */ + // Helper function, extracts vectors with variable indices + // in the first and last iterators, and concatenates them in that order into the + // output. + template + const Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) { + // Find total dimensionality + int dim = 0; + for(ITERATOR j = first; j != last; ++j) + // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) + if(!allowNonexistant || values.exists(*j)) + dim += values.dim(*j); - // Copy vectors - Vector ret(dim); - int varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - ret.segment(varStart, values[*j].rows()) = values[*j]; - varStart += values[*j].rows(); + // Copy vectors + Vector ret(dim); + int varStart = 0; + for(ITERATOR j = first; j != last; ++j) { + // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) + if(!allowNonexistant || values.exists(*j)) { + ret.segment(varStart, values.dim(*j)) = values[*j]; + varStart += values.dim(*j); + } + } + return ret; } - return ret; - } - /* ************************************************************************* */ - // Helper function, writes to the variables in values - // with indices iterated over by first and last, interpreting vector as the - // concatenated vectors to write. - template - void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) { - // Copy vectors - int varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - values[*j] = vector.segment(varStart, values[*j].rows()); - varStart += values[*j].rows(); + /* ************************************************************************* */ + // Helper function, writes to the variables in values + // with indices iterated over by first and last, interpreting vector as the + // concatenated vectors to write. + template + void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) { + // Copy vectors + int varStart = 0; + for(ITERATOR j = first; j != last; ++j) { + values[*j] = vector.segment(varStart, values[*j].rows()); + varStart += values[*j].rows(); + } + assert(varStart == vector.rows()); } - assert(varStart == vector.rows()); - } } } // \namespace gtsam diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 9507b55b9..8bfd42781 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -39,7 +39,6 @@ TEST(VectorValues, insert) { // Check dimensions LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(7, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -59,7 +58,7 @@ TEST(VectorValues, insert) { EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector())); + EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); // Check exceptions CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); @@ -80,7 +79,6 @@ TEST(VectorValues, dimsConstructor) { // Check dimensions LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(5, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -89,7 +87,7 @@ TEST(VectorValues, dimsConstructor) { EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector())); + EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); } /* ************************************************************************* */ @@ -106,7 +104,6 @@ TEST(VectorValues, copyConstructor) { // Check dimensions LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(7, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -126,7 +123,7 @@ TEST(VectorValues, copyConstructor) { EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector())); + EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); // Check exceptions CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); @@ -149,7 +146,6 @@ TEST(VectorValues, assignment) { // Check dimensions LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(7, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -169,7 +165,7 @@ TEST(VectorValues, assignment) { EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector())); + EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); // Check exceptions CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); @@ -188,7 +184,6 @@ TEST(VectorValues, SameStructure) { // Check dimensions LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(7, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -220,7 +215,6 @@ TEST(VectorValues, Zero_fromModel) { // Check dimensions LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(7, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -256,7 +250,6 @@ TEST(VectorValues, Zero_fromDims) { // Check dimensions LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(5, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -273,7 +266,6 @@ TEST(VectorValues, Zero_fromUniform) { // Check dimensions LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(6, actual.dim()); LONGS_EQUAL(2, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -298,7 +290,6 @@ TEST(VectorValues, resizeLike) { // Check dimensions LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(7, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -328,7 +319,6 @@ TEST(VectorValues, resize_fromUniform) { // Check dimensions LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(6, actual.dim()); LONGS_EQUAL(2, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -337,7 +327,7 @@ TEST(VectorValues, resize_fromUniform) { EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actual[0])); EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.vector())); + EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); } /* ************************************************************************* */ @@ -355,7 +345,6 @@ TEST(VectorValues, resize_fromDims) { // Check dimensions LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(5, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -364,7 +353,7 @@ TEST(VectorValues, resize_fromDims) { EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.vector())); + EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); } /* ************************************************************************* */ @@ -383,7 +372,6 @@ TEST(VectorValues, append) { // Check dimensions LONGS_EQUAL(5, actual.size()); - LONGS_EQUAL(13, actual.dim()); LONGS_EQUAL(1, actual.dim(0)); LONGS_EQUAL(2, actual.dim(1)); LONGS_EQUAL(2, actual.dim(2)); @@ -443,7 +431,8 @@ TEST(VectorValues, permute) { permutation[2] = 3; permutation[3] = 1; - VectorValues actual = original.permute(permutation); + VectorValues actual = original; + actual.permuteInPlace(permutation); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index b46fc6a3f..671dfe8f8 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -28,13 +28,12 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint( // Get magnitude of each update and find out which segment Delta falls in assert(Delta >= 0.0); double DeltaSq = Delta*Delta; - double x_u_norm_sq = dx_u.vector().squaredNorm(); - double x_n_norm_sq = dx_n.vector().squaredNorm(); + double x_u_norm_sq = dx_u.squaredNorm(); + double x_n_norm_sq = dx_n.squaredNorm(); if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl; if(DeltaSq < x_u_norm_sq) { // Trust region is smaller than steepest descent update - VectorValues x_d = VectorValues::SameStructure(dx_u); - x_d.vector() = dx_u.vector() * std::sqrt(DeltaSq / x_u_norm_sq); + VectorValues x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u; if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl; return x_d; } else if(DeltaSq < x_n_norm_sq) { @@ -79,8 +78,7 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues& // Compute blended point if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl; - VectorValues blend = VectorValues::SameStructure(x_u); - blend.vector() = (1. - tau) * x_u.vector() + tau * x_n.vector(); + VectorValues blend = (1. - tau) * x_u; axpy(tau, x_n, blend); return blend; } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 340c32318..31c761f88 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -177,7 +177,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose); gttoc(Dog_leg_point); - if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << std::endl; + if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl; gttic(retract); // Compute expmapped solution @@ -208,7 +208,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(rho >= 0.75) { // M agrees very well with f, so try to increase lambda - const double dx_d_norm = result.dx_d.vector().norm(); + const double dx_d_norm = result.dx_d.norm(); const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index bfc486fbf..da0a997c2 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -39,15 +39,15 @@ void ISAM2::Impl::AddVariables( // Add the new keys onto the ordering, add zeros to the delta for the new variables std::vector dims(newTheta.dims(*newTheta.orderingArbitrary())); if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; - const size_t newDim = accumulate(dims.begin(), dims.end(), 0); - const size_t originalDim = delta.dim(); const size_t originalnVars = delta.size(); delta.append(dims); - delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); deltaNewton.append(dims); - deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); RgProd.append(dims); - RgProd.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + for(Index j = originalnVars; j < delta.size(); ++j) { + delta[j].setZero(); + deltaNewton[j].setZero(); + RgProd[j].setZero(); + } { Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { @@ -55,10 +55,9 @@ void ISAM2::Impl::AddVariables( if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; ++ nextVar; } - assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); } - replacedKeys.resize(ordering.nVars(), false); + replacedKeys.resize(ordering.size(), false); } /* ************************************************************************* */ @@ -115,9 +114,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli } // Reorder and remove from ordering and solution - ordering.permuteWithInverse(unusedToEndInverse); + ordering.permuteInPlace(unusedToEnd); BOOST_REVERSE_FOREACH(Key key, unusedKeys) { - ordering.pop_back(key); + Ordering::value_type removed = ordering.pop_back(); + assert(removed.first == key); theta.erase(key); } @@ -188,14 +188,14 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; BOOST_FOREACH(Index var, clique->conditional()->keys()) { // Lookup the key associated with this index - Key key = decoder.at(var); + Key key = ordering.key(var); // Find the threshold for this variable type const Vector& threshold = thresholds.find(Symbol(key).chr())->second; @@ -214,7 +214,7 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMapchildren()) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child); + CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, ordering, child); } } } @@ -229,8 +229,7 @@ FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::share if(relinearizeThreshold.type() == typeid(double)) { CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); } else if(relinearizeThreshold.type() == typeid(FastMap)) { - Ordering::InvertedMap decoder = ordering.invert(); - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, decoder, root); + CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, ordering, root); } } @@ -267,8 +266,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const invalidateIfDebug = boost::none; #endif - assert(values.size() == ordering.nVars()); - assert(delta.size() == ordering.nVars()); + assert(values.size() == ordering.size()); + assert(delta.size() == ordering.size()); Values::iterator key_value; Ordering::const_iterator key_index; for(key_value = values.begin(), key_index = ordering.begin(); @@ -353,11 +352,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); if(debug) affectedColamd->print("affectedColamd: "); if(debug) affectedColamdInverse->print("affectedColamdInverse: "); - result.fullReordering = - *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamd); - result.fullReorderingInverse = - *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamdInverse); - if(debug) result.fullReordering.print("partialReordering: "); + result.reorderingSelector = affectedKeysSelector; + result.reorderingPermutation = *affectedColamd; + result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse); gttoc(ccolamd_permutations); gttic(permute_affected_variable_index); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 4ce16de19..a39b321d8 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -26,8 +26,9 @@ struct ISAM2::Impl { struct PartialSolveResult { ISAM2::sharedClique bayesTree; - Permutation fullReordering; - Permutation fullReorderingInverse; + Permutation reorderingSelector; + Permutation reorderingPermutation; + internal::Reduction reorderingInverse; }; struct ReorderingMode { diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index ba5a7bd27..d1b70f094 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -29,7 +29,7 @@ namespace gtsam { template VALUE ISAM2::calculateEstimate(Key key) const { const Index index = getOrdering()[key]; - const SubVector delta = getDelta()[index]; + const Vector& delta = getDelta()[index]; return theta_.at(key).retract(delta); } @@ -158,7 +158,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { if(!valuesChanged) { const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); - const SubVector& newValue(delta[*it]); + const Vector& newValue(delta[*it]); if((oldValue - newValue).lpNorm() >= threshold) { valuesChanged = true; break; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index be82fadc3..256a3485a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -348,12 +348,12 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark variableIndex_.permuteInPlace(*colamd); gttoc(permute_global_variable_index); gttic(permute_delta); - delta_ = delta_.permute(*colamd); - deltaNewton_ = deltaNewton_.permute(*colamd); - RgProd_ = RgProd_.permute(*colamd); + delta_.permuteInPlace(*colamd); + deltaNewton_.permuteInPlace(*colamd); + RgProd_.permuteInPlace(*colamd); gttoc(permute_delta); gttic(permute_ordering); - ordering_.permuteWithInverse(*colamdInverse); + ordering_.permuteInPlace(*colamd); gttoc(permute_ordering); gttoc(reorder); @@ -412,7 +412,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // Reeliminated keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, affectedAndNewKeys) { - result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; + result.detail->variableStatus[ordering_.key(index)].isReeliminated = true; } } @@ -450,7 +450,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(PartialSolve); Impl::ReorderingMode reorderingMode; - reorderingMode.nFullSystemVars = ordering_.nVars(); + reorderingMode.nFullSystemVars = ordering_.size(); reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; if(constrainKeys) { @@ -481,22 +481,22 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // re-eliminate. The reordered variables are also mentioned in the // orphans and the leftover cached factors. gttic(permute_global_variable_index); - variableIndex_.permuteInPlace(partialSolveResult.fullReordering); + variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_global_variable_index); gttic(permute_delta); - delta_ = delta_.permute(partialSolveResult.fullReordering); - deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering); - RgProd_ = RgProd_.permute(partialSolveResult.fullReordering); + delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); + deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); + RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_delta); gttic(permute_ordering); - ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_ordering); if(params_.cacheLinearizedFactors) { gttic(permute_cached_linear); //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse); FastList permuteLinearIndices = getAffectedFactors(affectedAndNewKeys); BOOST_FOREACH(size_t idx, permuteLinearIndices) { - linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse); + linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse); } gttoc(permute_cached_linear); } @@ -514,7 +514,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(orphans); gttic(permute); BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); + (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse); } gttoc(permute); gttic(insert); @@ -538,7 +538,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // Root clique variables for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { - result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; + result.detail->variableStatus[ordering_.key(index)].inRootClique = true; } } @@ -628,7 +628,6 @@ ISAM2Result ISAM2::update( Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_); // New keys for detailed results if(params_.enableDetailedResults) { - inverseOrdering_ = ordering_.invert(); BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } gttoc(add_new_variables); @@ -649,7 +648,7 @@ ISAM2Result ISAM2::update( // Observed keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, markedKeys) { - result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; + result.detail->variableStatus[ordering_.key(index)].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this @@ -666,7 +665,7 @@ ISAM2Result ISAM2::update( FastSet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); - vector markedRelinMask(ordering_.nVars(), false); + vector markedRelinMask(ordering_.size(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. if(params_.enablePartialRelinearizationCheck) relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold); @@ -677,8 +676,8 @@ ISAM2Result ISAM2::update( // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, relinKeys) { - result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true; - result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true; + result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } @@ -696,9 +695,9 @@ ISAM2Result ISAM2::update( FastSet involvedRelinKeys; Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask); BOOST_FOREACH(Index index, involvedRelinKeys) { - if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) { - result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true; - result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) { + result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true; + result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } } } gttoc(fluid_find_all); @@ -818,7 +817,7 @@ Values ISAM2::calculateEstimate() const { const VectorValues& delta(getDelta()); gttoc(getDelta); gttic(Expmap); - vector mask(ordering_.nVars(), true); + vector mask(ordering_.size(), true); Impl::ExpmapMasked(ret, delta, ordering_, mask); gttoc(Expmap); return ret; @@ -905,13 +904,13 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { gttic(Compute_minimizing_step_size); // Compute minimizing step size - double RgNormSq = isam.RgProd_.vector().squaredNorm(); + double RgNormSq = isam.RgProd_.asVector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; gttoc(Compute_minimizing_step_size); gttic(Compute_point); // Compute steepest descent point - grad.vector() *= step; + scal(step, grad); gttoc(Compute_point); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 7cc2c0518..5243efd27 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -405,9 +405,15 @@ public: Base::permuteWithInverse(inversePermutation); } - bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = Base::permuteSeparatorWithInverse(inversePermutation); - if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { + // bool changed = Base::permuteSeparatorWithInverse(inversePermutation); + // if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + // return changed; + //} + + bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { + bool changed = Base::reduceSeparatorWithInverse(inverseReduction); + if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction); return changed; } @@ -496,9 +502,6 @@ protected: /** The current Dogleg Delta (trust region radius) */ mutable boost::optional doglegDelta_; - /** The inverse ordering, only used for creating ISAM2Result::DetailedResults */ - boost::optional inverseOrdering_; - public: typedef ISAM2 This; ///< This class diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 7dd711a15..8505834b5 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -110,7 +110,7 @@ void LevenbergMarquardtOptimizer::iterate() { // Solve Damped Gaussian Factor Graph const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl; + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); // update values diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 732c807b3..c0c072571 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -13,14 +13,8 @@ namespace gtsam { /* ************************************************************************* */ void LinearContainerFactor::rekeyFactor(const Ordering& ordering) { - Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert - rekeyFactor(invOrdering); -} - -/* ************************************************************************* */ -void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) { BOOST_FOREACH(Index& idx, factor_->keys()) { - Key fullKey = invOrdering.find(idx)->second; + Key fullKey = ordering.key(idx); idx = fullKey; keys_.push_back(fullKey); } @@ -86,34 +80,6 @@ LinearContainerFactor::LinearContainerFactor( initializeLinearizationPoint(linearizationPoint); } -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint) -: factor_(factor.clone()) { - rekeyFactor(inverted_ordering); - initializeLinearizationPoint(linearizationPoint); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint) -: factor_(factor.clone()) { - rekeyFactor(inverted_ordering); - initializeLinearizationPoint(linearizationPoint); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor( - const GaussianFactor::shared_ptr& factor, - const Ordering::InvertedMap& ordering, - const Values& linearizationPoint) -: factor_(factor->clone()) { - rekeyFactor(ordering); - initializeLinearizationPoint(linearizationPoint); -} - /* ************************************************************************* */ void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { Base::print(s+"LinearContainerFactor", keyFormatter); @@ -198,6 +164,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize( // Determine delta between linearization points using new ordering VectorValues delta = linearizationPoint_->localCoordinates(subsetC, localOrdering); + Vector deltaVector = delta.asVector(); // clone and reorder linear factor to final ordering GaussianFactor::shared_ptr linFactor = order(localOrdering); @@ -208,15 +175,14 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize( HessianFactor::shared_ptr hesFactor = boost::shared_dynamic_cast(linFactor); size_t dim = hesFactor->linearTerm().size(); Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); - Vector G_delta = Gview.selfadjointView() * delta.vector(); - hesFactor->constantTerm() += delta.vector().dot(G_delta) + delta.vector().dot(hesFactor->linearTerm()); + Vector G_delta = Gview.selfadjointView() * deltaVector; + hesFactor->constantTerm() += deltaVector.dot(G_delta) + deltaVector.dot(hesFactor->linearTerm()); hesFactor->linearTerm() += G_delta; } // reset ordering - Ordering::InvertedMap invLocalOrdering = localOrdering.invert(); BOOST_FOREACH(Index& idx, linFactor->keys()) - idx = ordering[invLocalOrdering[idx] ]; + idx = ordering[localOrdering.key(idx)]; return linFactor; } @@ -258,18 +224,11 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( const GaussianFactorGraph& linear_graph, const Ordering& ordering, const Values& linearizationPoint) { - return convertLinearGraph(linear_graph, ordering.invert()); -} - -/* ************************************************************************* */ -NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering, - const Values& linearizationPoint) { NonlinearFactorGraph result; BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) if (f) result.push_back(NonlinearFactorGraph::sharedFactor( - new LinearContainerFactor(f, invOrdering, linearizationPoint))); + new LinearContainerFactor(f, ordering, linearizationPoint))); return result; } diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index b2b3f9645..30a0c33d7 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -45,21 +45,6 @@ public: LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); - /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ - LinearContainerFactor(const JacobianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint = Values()); - - /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ - LinearContainerFactor(const HessianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint = Values()); - - /** Constructor from shared_ptr with inverted ordering*/ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, - const Ordering::InvertedMap& ordering, - const Values& linearizationPoint = Values()); - // Access const GaussianFactor::shared_ptr& factor() const { return factor_; } @@ -130,19 +115,13 @@ public: /** * Utility function for converting linear graphs to nonlinear graphs - * consisting of LinearContainerFactors. Two versions are available, using - * either the ordering the linear graph was linearized around, or the inverse ordering. - * If the inverse ordering is present, it will be faster. + * consisting of LinearContainerFactors. */ static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, const Ordering& ordering, const Values& linearizationPoint = Values()); - static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, - const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values()); - protected: void rekeyFactor(const Ordering& ordering); - void rekeyFactor(const Ordering::InvertedMap& invOrdering); void initializeLinearizationPoint(const Values& linearizationPoint); }; // \class LinearContainerFactor diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index da14d8982..90f663639 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -44,7 +44,7 @@ NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradient } NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; - step.vector() *= alpha; + scal(alpha, step); return current.retract(step, ordering_); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 0479b06a5..b84d39a09 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -223,7 +223,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( variableIndex)); // Permute the Ordering with the COLAMD ordering - ordering->permuteWithInverse(*colamdPerm->inverse()); + ordering->permuteInPlace(*colamdPerm); return ordering; } @@ -254,7 +254,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Value variableIndex, index_constraints)); // Permute the Ordering with the COLAMD ordering - ordering->permuteWithInverse(*colamdPerm->inverse()); + ordering->permuteInPlace(*colamdPerm); return ordering; } diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 6cd917212..214c1c887 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -55,10 +55,8 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, // Augment ordering // TODO: allow for ordering constraints within the new variables - // FIXME: should just loop over new values - BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors) - BOOST_FOREACH(Key key, factor->keys()) - ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues) + ordering_.insert(key_value.key, ordering_.size()); boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_, ordering_); diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 1f7c5375b..4ab3a2b75 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -26,17 +26,53 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Ordering::Ordering(const std::list & L):nVars_(0) { +Ordering::Ordering(const std::list & L) { int i = 0; BOOST_FOREACH( Key s, L ) insert(s, i++) ; } /* ************************************************************************* */ -void Ordering::permuteWithInverse(const Permutation& inversePermutation) { - gttic(Ordering_permuteWithInverse); - BOOST_FOREACH(Ordering::value_type& key_order, *this) { - key_order.second = inversePermutation[key_order.second]; +Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) { + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; +} + +/* ************************************************************************* */ +Ordering& Ordering::operator=(const Ordering& rhs) { + order_ = rhs.order_; + orderingIndex_.resize(rhs.size()); + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; + return *this; +} + +/* ************************************************************************* */ +void Ordering::permuteInPlace(const Permutation& permutation) { + gttic(Ordering_permuteInPlace); + // Allocate new index and permute in map iterators + OrderingIndex newIndex(permutation.size()); + for(size_t j = 0; j < newIndex.size(); ++j) { + newIndex[j] = orderingIndex_[permutation[j]]; // Assign the iterator + newIndex[j]->second = j; // Change the stored index to its permuted value + } + // Swap the new index into this Ordering class + orderingIndex_.swap(newIndex); +} + +/* ************************************************************************* */ +void Ordering::permuteInPlace(const Permutation& selector, const Permutation& permutation) { + if(selector.size() != permutation.size()) + throw invalid_argument("Ordering::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); + // Create new index the size of the permuted entries + OrderingIndex newIndex(selector.size()); + // Permute the affected entries into the new index + for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) + newIndex[dstSlot] = orderingIndex_[selector[permutation[dstSlot]]]; + // Put the affected entries back in the new order and fix the indices + for(size_t slot = 0; slot < selector.size(); ++slot) { + orderingIndex_[selector[slot]] = newIndex[slot]; + orderingIndex_[selector[slot]]->second = selector[slot]; } } @@ -44,16 +80,15 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) { void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str; // Print ordering in index order - Ordering::InvertedMap inverted = this->invert(); // Print the ordering with varsPerLine ordering entries printed on each line, // for compactness. static const size_t varsPerLine = 10; bool endedOnNewline = false; - BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) { - if(index_key.first % varsPerLine != 0) + BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) { + if(index_key->second % varsPerLine != 0) cout << ", "; - cout << index_key.first << ":" << keyFormatter(index_key.second); - if(index_key.first % varsPerLine == varsPerLine - 1) { + cout << index_key->second<< ":" << keyFormatter(index_key->first); + if(index_key->second % varsPerLine == varsPerLine - 1) { cout << "\n"; endedOnNewline = true; } else { @@ -72,40 +107,11 @@ bool Ordering::equals(const Ordering& rhs, double tol) const { /* ************************************************************************* */ Ordering::value_type Ordering::pop_back() { - // FIXME: is there a way of doing this without searching over the entire structure? - for (iterator it=begin(); it!=end(); ++it) { - if (it->second == nVars_ - 1) { - value_type result = *it; - order_.erase(it); - --nVars_; - return result; - } - } - return value_type(); -} - -/* ************************************************************************* */ -Index Ordering::pop_back(Key key) { - Map::iterator item = order_.find(key); - if(item == order_.end()) { - throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key"); - } else { - if(item->second != nVars_ - 1) { - throw invalid_argument("Attempting to remove a key from an ordering in which that key is not last"); - } else { - order_.erase(item); - -- nVars_; - return nVars_; - } - } -} - -/* ************************************************************************* */ -Ordering::InvertedMap Ordering::invert() const { - InvertedMap result; - BOOST_FOREACH(const value_type& p, *this) - result.insert(make_pair(p.second, p.first)); - return result; + iterator lastItem = orderingIndex_.back(); // Get the map iterator to the highest-index entry + value_type removed = *lastItem; // Save the key-index pair to return + order_.erase(lastItem); // Erase the entry from the map + orderingIndex_.pop_back(); // Erase the entry from the index + return removed; // Return the removed item } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index ddffe787e..3ee2da832 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -35,8 +35,9 @@ namespace gtsam { class Ordering { protected: typedef FastMap Map; + typedef std::vector OrderingIndex; Map order_; - Index nVars_; + OrderingIndex orderingIndex_; public: @@ -51,25 +52,33 @@ public: /// @{ /// Default constructor for empty ordering - Ordering() : nVars_(0) {} + Ordering() {} + + /// Copy constructor + Ordering(const Ordering& other); /// Construct from list, assigns order indices sequentially to list items. - Ordering(const std::list & L) ; + Ordering(const std::list & L); + + /// Assignment operator + Ordering& operator=(const Ordering& rhs); /// @} /// @name Standard Interface /// @{ - /** One greater than the maximum ordering index, i.e. including missing indices in the count. See also size(). */ - Index nVars() const { return nVars_; } - /** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */ - Index size() const { return order_.size(); } + Index size() const { return orderingIndex_.size(); } const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const + Key key(Index index) const { + if(index >= orderingIndex_.size()) + throw std::out_of_range("Attempting to access out-of-range index in Ordering"); + else + return orderingIndex_[index]->first; } /** Assigns the ordering index of the requested \c key into \c index if the symbol * is present in the ordering, otherwise does not modify \c index. The @@ -85,8 +94,7 @@ public: index = i->second; return true; } else - return false; - } + return false; } /// Access the index for the requested key, throws std::out_of_range if the /// key is not present in the ordering (note that this differs from the @@ -122,23 +130,25 @@ public: */ const_iterator find(Key key) const { return order_.find(key); } - /** - * Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(), - * i.e., returns a pair of iterator and success (false if already present) - */ - std::pair tryInsert(const value_type& key_order) { - std::pair it_ok(order_.insert(key_order)); - if(it_ok.second == true && key_order.second+1 > nVars_) - nVars_ = key_order.second+1; - return it_ok; - } - - /** Try insert, but will fail if the key is already present */ + /** Insert a key-index pair, but will fail if the key is already present */ iterator insert(const value_type& key_order) { - std::pair it_ok(tryInsert(key_order)); - if(!it_ok.second) throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); - else return it_ok.first; - } + std::pair it_ok(order_.insert(key_order)); + if(it_ok.second) { + if(key_order.second >= orderingIndex_.size()) + orderingIndex_.resize(key_order.second + 1); + orderingIndex_[key_order.second] = it_ok.first; + return it_ok.first; + } else + throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); } + + /// Test if the key exists in the ordering. + bool exists(Key key) const { return order_.count(key) > 0; } + + /** Insert a key-index pair, but will fail if the key is already present */ + iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); } + + /// Adds a new key to the ordering with an index of one greater than the current highest index. + Index push_back(Key key) { return insert(std::make_pair(key, orderingIndex_.size()))->second; } /// @} /// @name Advanced Interface @@ -154,18 +164,6 @@ public: */ iterator end() { return order_.end(); } - /// Test if the key exists in the ordering. - bool exists(Key key) const { return order_.count(key) > 0; } - - ///TODO: comment - std::pair tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); } - - ///TODO: comment - iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); } - - /// Adds a new key to the ordering with an index of one greater than the current highest index. - Index push_back(Key key) { return insert(std::make_pair(key, nVars_))->second; } - /** Remove the last (last-ordered, not highest-sorting key) symbol/index pair * from the ordering (this version is \f$ O(n) \f$, use it when you do not * know the last-ordered key). @@ -177,16 +175,6 @@ public: */ value_type pop_back(); - /** Remove the last-ordered symbol from the ordering (this version is - * \f$ O(\log n) \f$, use it if you already know the last-ordered key). - * - * Throws std::invalid_argument if the requested key is not actually the - * last-ordered. - * - * @return The index of the symbol that was removed. - */ - Index pop_back(Key key); - /** * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are * very useful for unit tests. This functionality is courtesy of @@ -201,17 +189,13 @@ public: * internally, permuting an initial key-sorted ordering into a fill-reducing * ordering. */ - void permuteWithInverse(const Permutation& inversePermutation); + void permuteInPlace(const Permutation& permutation); + + void permuteInPlace(const Permutation& selector, const Permutation& permutation); /// Synonym for operator[](Key) Index& at(Key key) { return operator[](key); } - /** - * Create an inverse mapping from Index->Key, useful for decoding linear systems - * @return inverse mapping structure - */ - InvertedMap invert() const; - /// @} /// @name Testable /// @{ @@ -228,16 +212,26 @@ private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { + template + void save(ARCHIVE & ar, const unsigned int version) const + { ar & BOOST_SERIALIZATION_NVP(order_); - ar & BOOST_SERIALIZATION_NVP(nVars_); - } + size_t size_ = orderingIndex_.size(); // Save only the size but not the iterators + ar & BOOST_SERIALIZATION_NVP(size_); + } + template + void load(ARCHIVE & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(order_); + size_t size_; + ar & BOOST_SERIALIZATION_NVP(size_); + orderingIndex_.resize(size_); + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; // Assign the iterators + } + BOOST_SERIALIZATION_SPLIT_MEMBER() }; // \class Ordering -// typedef for use with matlab -typedef Ordering::InvertedMap InvertedOrdering; - /** * @class Unordered * @brief a set of unordered indices @@ -263,14 +257,15 @@ public: // Create an index formatter that looks up the Key in an inverse ordering, then // formats the key using the provided key formatter, used in saveGraph. -struct OrderingIndexFormatter { - Ordering::InvertedMap inverseOrdering; - const KeyFormatter& keyFormatter; +class OrderingIndexFormatter { +private: + Ordering ordering_; + KeyFormatter keyFormatter_; +public: OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : - inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {} + ordering_(ordering), keyFormatter_(keyFormatter) {} std::string operator()(Index index) { - return keyFormatter(inverseOrdering.at(index)); - } + return keyFormatter_(ordering_.key(index)); } }; } // \namespace gtsam diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 4f2845a66..ac135f113 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -81,7 +81,7 @@ namespace gtsam { Values result; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value + const Vector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract result.values_.insert(key, retractedValue); // Add retracted result directly to result values diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp index 42a176761..a62278d1e 100644 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ b/gtsam/nonlinear/tests/testOrdering.cpp @@ -23,26 +23,31 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( testOrdering, simple_modifications ) { +TEST( Ordering, simple_modifications ) { Ordering ordering; // create an ordering Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); ordering += x1, x2, x3, x4; + EXPECT_LONGS_EQUAL(0, ordering[x1]); + EXPECT_LONGS_EQUAL(1, ordering[x2]); + EXPECT_LONGS_EQUAL(2, ordering[x3]); + EXPECT_LONGS_EQUAL(3, ordering[x4]); + EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0)); + EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1)); + EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2)); + EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3)); + // pop the last two elements Ordering::value_type x4p = ordering.pop_back(); EXPECT_LONGS_EQUAL(3, ordering.size()); EXPECT(assert_equal(x4, x4p.first)); - Index x3p = ordering.pop_back(x3); + Index x3p = ordering.pop_back().second; EXPECT_LONGS_EQUAL(2, ordering.size()); EXPECT_LONGS_EQUAL(2, (int)x3p); - // try to pop an element that doesn't exist and isn't last - CHECK_EXCEPTION(ordering.pop_back(x4), std::invalid_argument); - CHECK_EXCEPTION(ordering.pop_back(x1), std::invalid_argument); - // reassemble back make the ordering 1, 2, 4, 3 EXPECT_LONGS_EQUAL(2, ordering.push_back(x4)); EXPECT_LONGS_EQUAL(3, ordering.push_back(x3)); @@ -50,6 +55,9 @@ TEST( testOrdering, simple_modifications ) { EXPECT_LONGS_EQUAL(2, ordering[x4]); EXPECT_LONGS_EQUAL(3, ordering[x3]); + EXPECT_LONGS_EQUAL(Key(x4), ordering.key(2)); + EXPECT_LONGS_EQUAL(Key(x3), ordering.key(3)); + // verify Ordering expectedFinal; expectedFinal += x1, x2, x4, x3; @@ -57,7 +65,36 @@ TEST( testOrdering, simple_modifications ) { } /* ************************************************************************* */ -TEST( testOrdering, invert ) { +TEST(Ordering, permute) { + Ordering ordering; + ordering += 2, 4, 6, 8; + + Ordering expected; + expected += 2, 8, 6, 4; + + Permutation permutation(4); + permutation[0] = 0; + permutation[1] = 3; + permutation[2] = 2; + permutation[3] = 1; + + Ordering actual = ordering; + actual.permuteInPlace(permutation); + + EXPECT(assert_equal(expected, actual)); + + EXPECT_LONGS_EQUAL(0, actual[2]); + EXPECT_LONGS_EQUAL(1, actual[8]); + EXPECT_LONGS_EQUAL(2, actual[6]); + EXPECT_LONGS_EQUAL(3, actual[4]); + EXPECT_LONGS_EQUAL(2, actual.key(0)); + EXPECT_LONGS_EQUAL(8, actual.key(1)); + EXPECT_LONGS_EQUAL(6, actual.key(2)); + EXPECT_LONGS_EQUAL(4, actual.key(3)); +} + +/* ************************************************************************* */ +TEST( Ordering, invert ) { // creates a map with the opposite mapping: Index->Key Ordering ordering; @@ -65,14 +102,10 @@ TEST( testOrdering, invert ) { Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); ordering += x1, x2, x3, x4; - Ordering::InvertedMap actual = ordering.invert(); - Ordering::InvertedMap expected; - expected.insert(make_pair(0, x1)); - expected.insert(make_pair(1, x2)); - expected.insert(make_pair(2, x3)); - expected.insert(make_pair(3, x4)); - - EXPECT(assert_container_equality(expected, actual)); + EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0)); + EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1)); + EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2)); + EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3)); } /* ************************************************************************* */ diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index e87835b67..715ec7058 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -32,6 +32,7 @@ #pragma GCC diagnostic pop #include // for 'list_of()' #include +#include using namespace std; using namespace gtsam; @@ -45,7 +46,8 @@ double computeError(const GaussianBayesNet& gbn, const LieVector& values) { // Convert Vector to VectorValues VectorValues vv = *allocateVectorValues(gbn); - vv.vector() = values; + internal::writeVectorValuesSlices(values, vv, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size())); // Convert to factor graph GaussianFactorGraph gfg(gbn); @@ -57,7 +59,8 @@ double computeErrorBt(const BayesTree& gbt, const LieVector // Convert Vector to VectorValues VectorValues vv = *allocateVectorValues(gbt); - vv.vector() = values; + internal::writeVectorValuesSlices(values, vv, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size())); // Convert to factor graph GaussianFactorGraph gfg(gbt); @@ -89,20 +92,22 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { // Compute the Hessian numerically Matrix hessian = numericalHessian( boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).vector())); + LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector())); // Compute the gradient numerically VectorValues gradientValues = *allocateVectorValues(gbn); Vector gradient = numericalGradient( boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(VectorValues::Zero(gradientValues).vector())); - gradientValues.vector() = gradient; + LieVector(VectorValues::Zero(gradientValues).asVector())); + internal::writeVectorValuesSlices(gradient, gradientValues, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); VectorValues denseMatrixGradient = *allocateVectorValues(gbn); - denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10); + internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); // Compute the steepest descent point @@ -269,20 +274,22 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { // Compute the Hessian numerically Matrix hessian = numericalHessian( boost::function(boost::bind(&computeErrorBt, bt, _1)), - LieVector(VectorValues::Zero(*allocateVectorValues(bt)).vector())); + LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector())); // Compute the gradient numerically VectorValues gradientValues = *allocateVectorValues(bt); Vector gradient = numericalGradient( boost::function(boost::bind(&computeErrorBt, bt, _1)), - LieVector(VectorValues::Zero(gradientValues).vector())); - gradientValues.vector() = gradient; + LieVector(VectorValues::Zero(gradientValues).asVector())); + internal::writeVectorValuesSlices(gradient, gradientValues, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); LONGS_EQUAL(11, augmentedHessian.cols()); VectorValues denseMatrixGradient = *allocateVectorValues(bt); - denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10); + internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient, + boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size())); EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); // Compute the steepest descent point @@ -338,12 +345,12 @@ TEST(DoglegOptimizer, ComputeBlend) { VectorValues xn = optimize(gbn); // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point - EXPECT(xu.vector().norm() < xn.vector().norm()); + EXPECT(xu.asVector().norm() < xn.asVector().norm()); // Compute blend double Delta = 1.5; VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn); - DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10); + DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10); } /* ************************************************************************* */ @@ -371,12 +378,12 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { double Delta1 = 0.5; // Less than steepest descent VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn)); - DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5); + DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5); double Delta2 = 1.5; // Between steepest descent and Newton's method VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn)); - DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5); + DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5); EXPECT(assert_equal(expected2, actual2)); double Delta3 = 5.0; // Larger than Newton's method point diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 94476d87a..b8bea8c6e 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -252,7 +252,7 @@ TEST( GaussianFactorGraph, getOrdering) Ordering original; original += L(1),X(1),X(2); FactorGraph symbolic(createGaussianFactorGraph(original)); Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); - Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); + Ordering actual = original; actual.permuteInPlace(perm); Ordering expected; expected += L(1),X(2),X(1); EXPECT(assert_equal(expected,actual)); } diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index bb709e077..befb66ef0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -207,6 +207,7 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) { EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); } + /* ************************************************************************* */ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { diff --git a/tests/timeiSAM2Chain.cpp b/tests/timeiSAM2Chain.cpp new file mode 100644 index 000000000..9ed18a1ef --- /dev/null +++ b/tests/timeiSAM2Chain.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file timeiSAM2Chain.cpp + * @brief Times each iteration of a long chain in iSAM2, to measure asymptotic performance + * @author Richard Roberts + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +typedef Pose2 Pose; + +typedef NoiseModelFactor1 NM1; +typedef NoiseModelFactor2 NM2; +noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim()); + +int main(int argc, char *argv[]) { + + const size_t steps = 50000; + + cout << "Playing forward " << steps << " time steps..." << endl; + + ISAM2 isam2; + + // Times + vector times(steps); + + for(size_t step=0; step < steps; ++step) { + + Values newVariables; + NonlinearFactorGraph newFactors; + + // Collect measurements and new variables for the current step + gttic_(Create_measurements); + if(step == 0) { + // Add prior + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newVariables.insert(0, Pose()); + } else { + Vector eta = Vector::Random(Pose::Dim()) * 0.1; + Pose2 between = Pose().retract(eta); + // Add between + newFactors.add(BetweenFactor(step-1, step, between, model)); + newVariables.insert(step, isam2.calculateEstimate(step-1) * between); + } + + gttoc_(Create_measurements); + + // Update iSAM2 + gttic_(Update_ISAM2); + isam2.update(newFactors, newVariables); + gttoc_(Update_ISAM2); + + tictoc_finishedIteration_(); + + tictoc_getNode(node, Update_ISAM2); + times[step] = node->time(); + + if(step % 1000 == 0) { + cout << "Step " << step << endl; + tictoc_print_(); + } + } + + tictoc_print_(); + + // Write times + ofstream timesFile("times.txt"); + BOOST_FOREACH(double t, times) { + timesFile << t << "\n"; } + + return 0; +}