From 06f836c0a7bd1eb58dee7becec1aa64c5f613ef6 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 23 Nov 2012 23:22:46 +0000 Subject: [PATCH 02/16] Using vector instead of deque in VariableIndex, BayesTree::Nodes, and GaussianISAM::Dims. In practice it appears to be faster due to smart reallocation strategies (still need to investigate whether we should use reserve, resize, or neither). --- gtsam/inference/BayesTree.h | 2 +- gtsam/inference/VariableIndex.cpp | 2 +- gtsam/inference/VariableIndex.h | 2 +- gtsam/linear/GaussianISAM.h | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 3982a5987..1e6001978 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -92,7 +92,7 @@ namespace gtsam { }; /** Map from indices to Clique */ - typedef std::deque Nodes; + typedef std::vector Nodes; protected: diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 4e2da7492..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]]); diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index df0a8fec9..adfbd206f 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. 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() {} From 5b9271cb97a5ed0668ac3be7f2840bb19ef79a52 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 23 Nov 2012 23:22:53 +0000 Subject: [PATCH 03/16] VV work --- gtsam/linear/GaussianFactorGraph.cpp | 4 +- gtsam/linear/HessianFactor.cpp | 4 +- gtsam/linear/VectorValues.cpp | 112 ++++++------- gtsam/linear/VectorValues.h | 154 +++++++----------- gtsam/linear/tests/testVectorValues.cpp | 10 +- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 8 +- gtsam/nonlinear/ISAM2-impl.cpp | 6 +- gtsam/nonlinear/ISAM2.cpp | 4 +- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 2 +- .../NonlinearConjugateGradientOptimizer.cpp | 2 +- tests/testDoglegOptimizer.cpp | 28 ++-- 11 files changed, 138 insertions(+), 196 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 8f7d4234e..2dd03f3a7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -607,7 +607,7 @@ break; /* ************************************************************************* */ void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - r.vector() = Vector::Zero(r.dim()); + r.asVector() = Vector::Zero(r.dim()); Index i = 0; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); @@ -621,7 +621,7 @@ break; /* ************************************************************************* */ void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { - x.vector() = Vector::Zero(x.dim()); + x.asVector() = Vector::Zero(x.dim()); Index i = 0; BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9b97cf6bf..31acd351f 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -324,8 +324,8 @@ Matrix HessianFactor::information() const { double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) const double f = constantTerm(); - const double xtg = c.vector().dot(linearTerm()); - const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * c.vector(); + const double xtg = c.asVector().dot(linearTerm()); + const double xGx = c.asVector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * c.asVector(); return 0.5 * (f - 2.0 * xtg + xGx); } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 4dffd173c..4d952d063 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -24,30 +24,18 @@ 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 VectorValues::dims() const { - std::vector result(this->size()); + vector result(this->size()); for(Index j = 0; j < this->size(); ++j) result[j] = this->dim(j); return result; @@ -59,44 +47,30 @@ void VectorValues::insert(Index j, const Vector& value) { if(exists(j)) throw invalid_argument("VectorValues: requested variable index to insert already exists."); - // Get vector of dimensions - FastVector 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(size_t j=0; jhasSameStructure(c)); - VectorValues result(SameStructure(c)); - result.values_ = this->values_ + c.values_; - return result; -} - -/* ************************************************************************* */ -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 @@ -202,4 +154,36 @@ void VectorValues::swap(VectorValues& other) { this->maps_.swap(other.maps_); } +/* ************************************************************************* */ +double VectorValues::dot(const VectorValues& V) const { + +} + +/* ************************************************************************* */ +double VectorValues::norm() const { + +} + +/* ************************************************************************* */ +VectorValues VectorValues::operator+(const VectorValues& c) const { + assert(this->hasSameStructure(c)); + VectorValues result(SameStructure(c)); + result.values_ = this->values_ + c.values_; + return result; +} + +/* ************************************************************************* */ +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_; +} + } \ No newline at end of file diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 9cb278b10..113c563e1 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -92,15 +92,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 @@ -111,9 +110,6 @@ 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. * @return @@ -126,31 +122,31 @@ 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(); } + //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() && (*this)[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 @@ -160,17 +156,14 @@ namespace gtsam { */ 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: ", @@ -251,10 +244,10 @@ namespace gtsam { void setZero(); /** Reference the entire solution vector (const version). */ - const Vector& vector() const { chk(); return values_; } + //const Vector& asVector() const { return values_; } /** Reference the entire solution vector. */ - Vector& vector() { chk(); return values_; } + //Vector& asVector() { return values_; } /** Check whether this VectorValues has the same structure, meaning has the * same number of variables and that all variables are of the same dimension, @@ -264,16 +257,26 @@ namespace gtsam { */ bool hasSameStructure(const VectorValues& other) const; + /** + * Permute the entries of this VectorValues in place + */ + void permute(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; /** * + operator does element-wise addition. Both VectorValues must have the @@ -293,26 +296,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 void checkExists(Index j) const { - chk(); if(!exists(j)) throw std::out_of_range("VectorValues: requested variable index is not in this VectorValues."); } @@ -375,23 +363,9 @@ namespace gtsam { /** 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 @@ -399,48 +373,32 @@ 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()); + values_.resize(size() + dimensions.size()); + size_t i = 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; + values_.resize(dimensions.size()); + size_t i = 0; + BOOST_FOREACH(size_t dim, dimensions) { + values_[i] = Vector::Zero(dim); + ++ i; } -#endif + return ret; } namespace internal { @@ -448,8 +406,8 @@ namespace gtsam { // 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) { + template + Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) { // Find total dimensionality int dim = 0; for(ITERATOR j = first; j != last; ++j) @@ -469,8 +427,8 @@ namespace gtsam { // 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) { + template + void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) { // Copy vectors int varStart = 0; for(ITERATOR j = first; j != last; ++j) { diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index d6cdde4e8..d6ee0ef7e 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -89,7 +89,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())); } /* ************************************************************************* */ @@ -126,7 +126,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); @@ -169,7 +169,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); @@ -337,7 +337,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())); } /* ************************************************************************* */ @@ -364,7 +364,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())); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index b46fc6a3f..906c04a57 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -28,13 +28,13 @@ 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.asVector().squaredNorm(); + double x_n_norm_sq = dx_n.asVector().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); + x_d.asVector() = dx_u.asVector() * std::sqrt(DeltaSq / x_u_norm_sq); 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) { @@ -80,7 +80,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(); + blend.asVector() = (1. - tau) * x_u.asVector() + tau * x_n.asVector(); return blend; } diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 593666657..d52a38cb2 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -43,11 +43,11 @@ void ISAM2::Impl::AddVariables( const size_t originalDim = delta.dim(); const size_t originalnVars = delta.size(); delta.append(dims); - delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + delta.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); deltaNewton.append(dims); - deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaNewton.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); RgProd.append(dims); - RgProd.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + RgProd.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); { Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 058e0d736..078af066d 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -907,13 +907,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; + grad.asVector() *= step; gttoc(Compute_point); } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index f9dec6874..ba809b507 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.asVector().norm() << endl; if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); // update values diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index da14d8982..36928efdf 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; + step.asVector() *= alpha; return current.retract(step, ordering_); } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index e87835b67..8e17ca933 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -45,7 +45,7 @@ double computeError(const GaussianBayesNet& gbn, const LieVector& values) { // Convert Vector to VectorValues VectorValues vv = *allocateVectorValues(gbn); - vv.vector() = values; + vv.asVector() = values; // Convert to factor graph GaussianFactorGraph gfg(gbn); @@ -57,7 +57,7 @@ double computeErrorBt(const BayesTree& gbt, const LieVector // Convert Vector to VectorValues VectorValues vv = *allocateVectorValues(gbt); - vv.vector() = values; + vv.asVector() = values; // Convert to factor graph GaussianFactorGraph gfg(gbt); @@ -89,20 +89,20 @@ 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())); + gradientValues.asVector() = gradient; // 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); + denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10); EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); // Compute the steepest descent point @@ -269,20 +269,20 @@ 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())); + gradientValues.asVector() = gradient; // 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); + denseMatrixGradient.asVector() = -augmentedHessian.col(10).segment(0,10); EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5)); // Compute the steepest descent point @@ -338,12 +338,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 +371,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 From c7b9345aa186b5b00f482f5d02ae97df22e5c18d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 18 Dec 2012 14:20:49 +0000 Subject: [PATCH 04/16] Removed "full VectorValues" version of HessianFactor::error --- gtsam/linear/HessianFactor.cpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 5f48be50b..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.asVector(); - } 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); } From 7309aa0ffa021a7ce62d17110c8e2651b11f18a2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 18 Dec 2012 14:21:02 +0000 Subject: [PATCH 05/16] Unit tests pass with piecewise VectorValues implementation (for fast permutations) --- gtsam/linear/GaussianFactorGraph.cpp | 6 +- gtsam/linear/VectorValues.cpp | 153 ++++++++-------- gtsam/linear/VectorValues.h | 171 +++++++++--------- gtsam/linear/tests/testVectorValues.cpp | 17 +- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 10 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 4 +- gtsam/nonlinear/ISAM2-impl.cpp | 10 +- gtsam/nonlinear/ISAM2-inl.h | 4 +- gtsam/nonlinear/ISAM2.cpp | 14 +- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 2 +- gtsam/nonlinear/LinearContainerFactor.cpp | 5 +- .../NonlinearConjugateGradientOptimizer.cpp | 2 +- gtsam/nonlinear/Values.cpp | 2 +- tests/testDoglegOptimizer.cpp | 19 +- tests/testGaussianISAM2.cpp | 1 + 15 files changed, 215 insertions(+), 205 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 2dd03f3a7..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.asVector() = 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.asVector() = 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/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 1525b81b3..f4f350c23 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -20,6 +20,8 @@ #include #include +#include + using namespace std; namespace gtsam { @@ -29,13 +31,13 @@ VectorValues VectorValues::Zero(const VectorValues& x) { VectorValues result; result.values_.resize(x.size()); for(size_t j=0; j VectorValues::dims() const { - vector result(this->size()); + std::vector result(this->size()); for(Index j = 0; j < this->size(); ++j) result[j] = this->dim(j); return result; @@ -67,7 +69,7 @@ void VectorValues::print(const std::string& str, const IndexFormatter& formatter bool VectorValues::equals(const VectorValues& x, double tol) const { if(this->size() != x.size()) return false; - for(size_t j=0; jsize()), 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::permute(const Permutation& permutation) const { - // Create result and allocate space - VectorValues lhs; - lhs.values_.resize(this->dim()); - lhs.maps_.reserve(this->size()); +void VectorValues::permuteInPlace(const Permutation& permutation) { + // Allocate new values + Values newValues(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(); - } + // 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])); - return lhs; + // Swap the values into this VectorValues + values_.swap(newValues); } /* ************************************************************************* */ void VectorValues::swap(VectorValues& other) { this->values_.swap(other.values_); - this->maps_.swap(other.maps_); } /* ************************************************************************* */ -double VectorValues::dot(const VectorValues& V) const { - +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 { - assert(this->hasSameStructure(c)); - VectorValues result(SameStructure(c)); - result.values_ = this->values_ + c.values_; + 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 { - assert(this->hasSameStructure(c)); - VectorValues result(SameStructure(c)); - result.values_ = this->values_ - c.values_; + 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) { - assert(this->hasSameStructure(c)); - this->values_ += c.values_; -} - -/* ************************************************************************* */ -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; - } - return result; + 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"); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index c5a460684..980c74dc0 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -111,7 +111,7 @@ namespace gtsam { VectorValues() {} /** 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,14 +127,11 @@ namespace gtsam { /** 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() && (*this)[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). */ Vector& at(Index j) { checkExists(j); return values_[j]; } @@ -149,8 +146,8 @@ namespace gtsam { 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. */ @@ -167,7 +164,7 @@ namespace gtsam { /** 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; @@ -179,10 +176,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. */ @@ -214,17 +211,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 @@ -243,14 +239,11 @@ namespace gtsam { /** Set all entries to zero, does not modify the size. */ void setZero(); - /** Reference the entire solution vector (const version). */ - //const Vector& asVector() const { return values_; } - - /** Reference the entire solution vector. */ - //Vector& asVector() { 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, @@ -263,7 +256,7 @@ namespace gtsam { /** * Permute the entries of this VectorValues in place */ - void permute(const Permutation& permutation); + void permuteInPlace(const Permutation& permutation); /** * Swap the data in this VectorValues with another. @@ -276,11 +269,14 @@ namespace gtsam { /** Dot product with another VectorValues, interpreting both as vectors of * their concatenated values. */ - double dot(const VectorValues& V) const; + double dot(const VectorValues& v) const; /** Vector L2 norm */ double norm() const; + /** Squared vector L2 norm */ + double squaredNorm() const; + /** * + operator does element-wise addition. Both VectorValues must have the * same structure (checked when NDEBUG is not defined). @@ -316,50 +312,54 @@ 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: @@ -383,8 +383,8 @@ namespace gtsam { /* ************************************************************************* */ template void VectorValues::append(const CONTAINER& dimensions) { - values_.resize(size() + dimensions.size()); size_t i = size(); + values_.resize(size() + dimensions.size()); BOOST_FOREACH(size_t dim, dimensions) { values_[i] = Vector(dim); ++ i; @@ -395,51 +395,56 @@ namespace gtsam { template VectorValues VectorValues::Zero(const CONTAINER& dimensions) { VectorValues ret; - values_.resize(dimensions.size()); + ret.values_.resize(dimensions.size()); size_t i = 0; BOOST_FOREACH(size_t dim, dimensions) { - values_[i] = Vector::Zero(dim); + ret.values_[i] = Vector::Zero(dim); ++ i; } 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 VectorValues& 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, 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(); + /* ************************************************************************* */ + // 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 f80958504..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)); @@ -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)); @@ -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)); @@ -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)); @@ -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)); @@ -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 906c04a57..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.asVector().squaredNorm(); - double x_n_norm_sq = dx_n.asVector().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.asVector() = dx_u.asVector() * 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.asVector() = (1. - tau) * x_u.asVector() + tau * x_n.asVector(); + 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 85abaa150..40f32937d 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.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); deltaNewton.append(dims); - deltaNewton.asVector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); RgProd.append(dims); - RgProd.asVector().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) { 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 be68bd8f5..387de0b90 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -348,9 +348,9 @@ 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); @@ -484,9 +484,9 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark variableIndex_.permuteInPlace(partialSolveResult.fullReordering); 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.fullReordering); + deltaNewton_.permuteInPlace(partialSolveResult.fullReordering); + RgProd_.permuteInPlace(partialSolveResult.fullReordering); gttoc(permute_delta); gttic(permute_ordering); ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); @@ -911,7 +911,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { gttic(Compute_point); // Compute steepest descent point - grad.asVector() *= step; + scal(step, grad); gttoc(Compute_point); } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index b4e892996..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.asVector().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..2fe39f5f1 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -198,6 +198,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,8 +209,8 @@ 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; } diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 36928efdf..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.asVector() *= alpha; + scal(alpha, step); return current.retract(step, ordering_); } 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/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 8e17ca933..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.asVector() = 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.asVector() = 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); @@ -96,13 +99,15 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) { Vector gradient = numericalGradient( boost::function(boost::bind(&computeError, gbn, _1)), LieVector(VectorValues::Zero(gradientValues).asVector())); - gradientValues.asVector() = gradient; + 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.asVector() = -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 @@ -276,13 +281,15 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) { Vector gradient = numericalGradient( boost::function(boost::bind(&computeErrorBt, bt, _1)), LieVector(VectorValues::Zero(gradientValues).asVector())); - gradientValues.asVector() = gradient; + 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.asVector() = -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 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) { From 4e7393cc08daddc595f61d1fe74640ea7424fa02 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 18 Dec 2012 14:21:12 +0000 Subject: [PATCH 06/16] Added timing test for a long chain in iSAM2 --- tests/timeiSAM2Chain.cpp | 100 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 tests/timeiSAM2Chain.cpp diff --git a/tests/timeiSAM2Chain.cpp b/tests/timeiSAM2Chain.cpp new file mode 100644 index 000000000..cad75e17a --- /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 = 10000; + + 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; +} From b58fb7137709c00e2a09689699da10de7285b755 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 18 Dec 2012 14:21:28 +0000 Subject: [PATCH 07/16] Some usage (still more to do) of sparse and partial permutations in iSAM2 partial reordering to improve asymptotic performance. --- gtsam/inference/BayesNet-inl.h | 20 ++++++------ gtsam/inference/BayesNet.h | 2 +- gtsam/inference/BayesTreeCliqueBase-inl.h | 40 +++++++++++++++++------ gtsam/inference/BayesTreeCliqueBase.h | 9 ++++- gtsam/inference/IndexConditional.cpp | 31 ++++++++++++++---- gtsam/inference/IndexConditional.h | 8 ++++- gtsam/inference/Permutation.cpp | 21 ++++++++++-- gtsam/inference/Permutation.h | 4 ++- gtsam/inference/VariableIndex.cpp | 14 ++++++++ gtsam/inference/VariableIndex.h | 3 ++ gtsam/inference/tests/testPermutation.cpp | 23 +++++++++++++ gtsam/nonlinear/ISAM2-impl.cpp | 8 ++--- gtsam/nonlinear/ISAM2-impl.h | 5 +-- gtsam/nonlinear/ISAM2.cpp | 16 +++++---- gtsam/nonlinear/ISAM2.h | 12 +++++-- gtsam/nonlinear/Ordering.cpp | 7 ++++ gtsam/nonlinear/Ordering.h | 7 ++++ 17 files changed, 179 insertions(+), 51 deletions(-) diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 0fe78fbed..5375b209f 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -118,16 +118,16 @@ 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 + //bool BayesNet::permuteSeparatorWithInverse( + // const Permutation& inversePermutation) { + // bool separatorChanged = false; + // BOOST_FOREACH(sharedConditional conditional, conditionals_) { + // if (conditional->permuteSeparatorWithInverse(inversePermutation)) + // separatorChanged = true; + // } + // return separatorChanged; + //} /* ************************************************************************* */ template diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 1077c1cad..f8aaa7d17 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -225,7 +225,7 @@ public: * Returns true if any reordered variables appeared in the separator and * false if not. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation); iterator begin() {return conditionals_.begin();} /// +// bool BayesTreeCliqueBase::permuteSeparatorWithInverse( +// const Permutation& inversePermutation) { +// bool changed = conditional_->permuteSeparatorWithInverse( +// inversePermutation); +//#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); +// } +// } +//#endif +// if (changed) { +// BOOST_FOREACH(const derived_ptr& child, children_) { +// (void) child->permuteSeparatorWithInverse(inversePermutation); +// } +// } +// assertInvariants(); +// return changed; +// } + /* ************************************************************************* */ 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..a00980983 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -183,7 +183,14 @@ namespace gtsam { * traversing the whole tree. Returns whether any separator variables in * this subtree were reordered. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** Permute variables when they only appear in the separators. In this + * case the running intersection property will be used to prevent always + * traversing the whole tree. Returns whether any separator variables in + * this subtree were reordered. + */ + 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..1a5e6d516 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -44,16 +44,33 @@ namespace gtsam { } /* ************************************************************************* */ - bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - #ifndef NDEBUG - BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } - #endif + //bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + //#ifndef NDEBUG + // BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } + //#endif + // bool parentChanged = false; + // BOOST_FOREACH(KeyType& parent, parents()) { + // KeyType newParent = inversePermutation[parent]; + // if(parent != newParent) { + // parentChanged = true; + // parent = newParent; + // } + // } + // assertInvariants(); + // return parentChanged; + //} + + /* ************************************************************************* */ + 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..bd6d50c22 100644 --- a/gtsam/inference/Permutation.cpp +++ b/gtsam/inference/Permutation.cpp @@ -161,6 +161,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 +193,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 +205,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 +217,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..4fbd06199 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -192,12 +192,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 7e51c3d37..11ce68ec6 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -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 d465b57eb..149f8eaaf 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -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/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 40f32937d..2e840fe01 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -353,11 +353,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.cpp b/gtsam/nonlinear/ISAM2.cpp index 387de0b90..d19654ac9 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -481,22 +481,24 @@ 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_.permuteInPlace(partialSolveResult.fullReordering); - deltaNewton_.permuteInPlace(partialSolveResult.fullReordering); - RgProd_.permuteInPlace(partialSolveResult.fullReordering); + const Permutation fullReordering = *Permutation::Identity(delta_.size()). + partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); + delta_.permuteInPlace(fullReordering); + deltaNewton_.permuteInPlace(fullReordering); + RgProd_.permuteInPlace(fullReordering); gttoc(permute_delta); gttic(permute_ordering); - ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + ordering_.reduceWithInverse(partialSolveResult.reorderingInverse); 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 +516,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); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 7cc2c0518..51a876031 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; } diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 1f7c5375b..576e7fbb6 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -40,6 +40,13 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) { } } +/* ************************************************************************* */ +void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) { + BOOST_FOREACH(Ordering::value_type& key_order, *this) { + key_order.second = inverseReduction[key_order.second]; + } +} + /* ************************************************************************* */ void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str; diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index ddffe787e..60a21e5a1 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -203,6 +203,13 @@ public: */ void permuteWithInverse(const Permutation& inversePermutation); + /** + * Reorder the variables with a permutation. This is typically used + * internally, permuting an initial key-sorted ordering into a fill-reducing + * ordering. + */ + void reduceWithInverse(const internal::Reduction& inverseReduction); + /// Synonym for operator[](Key) Index& at(Key key) { return operator[](key); } From 58107479375719c2ba3b27a43e87f2d6a89c0963 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 18 Dec 2012 14:21:38 +0000 Subject: [PATCH 08/16] Removed 'permuteSeparatorWithInverse' from MATLAB wrapper --- gtsam.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index c4950e4e4..c475f1b55 100644 --- a/gtsam.h +++ b/gtsam.h @@ -765,7 +765,6 @@ class IndexConditional { gtsam::IndexFactor* toFactor() const; //Advanced interface - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); void permuteWithInverse(const gtsam::Permutation& inversePermutation); }; @@ -788,7 +787,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 @@ -832,7 +830,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; @@ -859,7 +856,6 @@ virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { //Advanced Interface void pop_front(); void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; #include @@ -1806,7 +1802,6 @@ virtual class ISAM2Clique { void print(string s); void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; class ISAM2Result { From c42bccbb3e36f038d2dced059291bd5745d60fc2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 18 Dec 2012 14:21:49 +0000 Subject: [PATCH 09/16] Increased chain length in timeiSAM2Chain --- tests/timeiSAM2Chain.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/timeiSAM2Chain.cpp b/tests/timeiSAM2Chain.cpp index cad75e17a..9ed18a1ef 100644 --- a/tests/timeiSAM2Chain.cpp +++ b/tests/timeiSAM2Chain.cpp @@ -43,7 +43,7 @@ noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim()); int main(int argc, char *argv[]) { - const size_t steps = 10000; + const size_t steps = 50000; cout << "Playing forward " << steps << " time steps..." << endl; From 4cb66dcdcc2aa2b16855eaa5b766356c994ce2eb Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 18 Dec 2012 14:21:58 +0000 Subject: [PATCH 10/16] For speed, added partial permutation version of VectorValues::permuteInPlace and removed Permutation::partialPermutation (which generated a full-length permutation with only a few entries rearranged) --- gtsam/inference/Permutation.cpp | 12 ------------ gtsam/inference/Permutation.h | 8 -------- gtsam/linear/VectorValues.cpp | 14 ++++++++++++++ gtsam/linear/VectorValues.h | 5 +++++ gtsam/nonlinear/ISAM2.cpp | 8 +++----- 5 files changed, 22 insertions(+), 25 deletions(-) diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp index bd6d50c22..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())); diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 4fbd06199..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 diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index f4f350c23..3c6c7133f 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -145,6 +145,20 @@ void VectorValues::permuteInPlace(const Permutation& permutation) { values_.swap(newValues); } +/* ************************************************************************* */ +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_); diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 980c74dc0..1997a4bd5 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -253,6 +253,11 @@ namespace gtsam { */ bool hasSameStructure(const VectorValues& other) const; + /** + * Permute the variables in the VariableIndex according to the given partial permutation + */ + void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation); + /** * Permute the entries of this VectorValues in place */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d19654ac9..7d579f206 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -484,11 +484,9 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_global_variable_index); gttic(permute_delta); - const Permutation fullReordering = *Permutation::Identity(delta_.size()). - partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - delta_.permuteInPlace(fullReordering); - deltaNewton_.permuteInPlace(fullReordering); - RgProd_.permuteInPlace(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_.reduceWithInverse(partialSolveResult.reorderingInverse); From 1d8d733182da91eb9ca38d48553430dd21c3628d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 19 Dec 2012 00:59:03 +0000 Subject: [PATCH 11/16] Updated MATLAB wrapper for VectorValues and Permutation changes in previous several commites (isam2-chain-optimizations branch) --- gtsam.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam.h b/gtsam.h index c475f1b55..12f7942fc 100644 --- a/gtsam.h +++ b/gtsam.h @@ -717,7 +717,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 { @@ -1038,12 +1037,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 From 3cf45fccba14b434e1280b3db37af2a9da4c92a4 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 19 Dec 2012 19:58:17 +0000 Subject: [PATCH 12/16] Removed unneeded qualifier from VectorValues::permuteInPlace() --- gtsam/linear/VectorValues.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 1997a4bd5..f1883f6b3 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -256,7 +256,7 @@ namespace gtsam { /** * Permute the variables in the VariableIndex according to the given partial permutation */ - void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation); + void permuteInPlace(const Permutation& selector, const Permutation& permutation); /** * Permute the entries of this VectorValues in place From 6488eae412ae692a951aaec14c591feca8d622cb Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 8 Jan 2013 23:31:02 +0000 Subject: [PATCH 13/16] Whitespace --- gtsam/inference/SymbolicFactorGraph.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index ccb421d52..7f71ab54a 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -117,15 +117,15 @@ namespace gtsam { FastSet keys; BOOST_FOREACH(const IndexFactor::shared_ptr& factor, factors) - BOOST_FOREACH(Index var, *factor) - keys.insert(var); + BOOST_FOREACH(Index var, *factor) + keys.insert(var); if (keys.size() < nrFrontals) throw invalid_argument( - "EliminateSymbolic requested to eliminate more variables than exist in graph."); + "EliminateSymbolic requested to eliminate more variables than exist in graph."); vector newKeys(keys.begin(), keys.end()); return make_pair(boost::make_shared(newKeys, nrFrontals), - boost::make_shared(newKeys.begin() + nrFrontals, newKeys.end())); + boost::make_shared(newKeys.begin() + nrFrontals, newKeys.end())); } /* ************************************************************************* */ From 6f9601f5e08c516cde0beb425955e8434bd6ffee Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 8 Jan 2013 23:31:03 +0000 Subject: [PATCH 14/16] Disabled print statements in unit test --- gtsam/nonlinear/tests/testValues.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 96f562f55..227f20834 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -43,9 +43,9 @@ class TestValueData { public: static int ConstructorCount; static int DestructorCount; - TestValueData(const TestValueData& other) { cout << "Copy constructor" << endl; ++ ConstructorCount; } - TestValueData() { cout << "Default constructor" << endl; ++ ConstructorCount; } - ~TestValueData() { cout << "Destructor" << endl; ++ DestructorCount; } + TestValueData(const TestValueData& other) { /*cout << "Copy constructor" << endl;*/ ++ ConstructorCount; } + TestValueData() { /*cout << "Default constructor" << endl;*/ ++ ConstructorCount; } + ~TestValueData() { /*cout << "Destructor" << endl;*/ ++ DestructorCount; } }; int TestValueData::ConstructorCount = 0; int TestValueData::DestructorCount = 0; From 013705232cce425887a47da1bec4ae61205dd166 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 8 Jan 2013 23:31:06 +0000 Subject: [PATCH 15/16] Maintain reverse-lookup inside of Ordering, to allow fast partial permutations, and modified iSAM2 to use partial permutations on Ordering. Removed InvertedOrdering and updated other GTSAM code and unit tests to use the Ordering reverse-lookup function "key" instead of calculating and inverse ordering. --- gtsam/nonlinear/ISAM2-impl.cpp | 21 ++-- gtsam/nonlinear/ISAM2.cpp | 27 +++-- gtsam/nonlinear/ISAM2.h | 3 - gtsam/nonlinear/LinearContainerFactor.cpp | 48 +------- gtsam/nonlinear/LinearContainerFactor.h | 23 +--- gtsam/nonlinear/NonlinearFactorGraph.cpp | 4 +- gtsam/nonlinear/NonlinearISAM.cpp | 6 +- gtsam/nonlinear/Ordering.cpp | 95 ++++++++------- gtsam/nonlinear/Ordering.h | 136 ++++++++++------------ gtsam/nonlinear/tests/testOrdering.cpp | 63 +++++++--- tests/testGaussianFactorGraphB.cpp | 2 +- 11 files changed, 189 insertions(+), 239 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 2e840fe01..da0a997c2 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -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(); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 7d579f206..256a3485a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -353,7 +353,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark 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) { @@ -489,7 +489,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_delta); gttic(permute_ordering); - ordering_.reduceWithInverse(partialSolveResult.reorderingInverse); + ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_ordering); if(params_.cacheLinearizedFactors) { gttic(permute_cached_linear); @@ -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; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 51a876031..5243efd27 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -502,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/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 2fe39f5f1..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); @@ -215,9 +181,8 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize( } // reset ordering - Ordering::InvertedMap invLocalOrdering = localOrdering.invert(); BOOST_FOREACH(Index& idx, linFactor->keys()) - idx = ordering[invLocalOrdering[idx] ]; + idx = ordering[localOrdering.key(idx)]; return linFactor; } @@ -259,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/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 576e7fbb6..4ab3a2b75 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -26,24 +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; } /* ************************************************************************* */ -void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) { - BOOST_FOREACH(Ordering::value_type& key_order, *this) { - key_order.second = inverseReduction[key_order.second]; +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]; } } @@ -51,16 +80,15 @@ void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) { 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 { @@ -79,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 60a21e5a1..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,24 +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); - /** - * Reorder the variables with a permutation. This is typically used - * internally, permuting an initial key-sorted ordering into a fill-reducing - * ordering. - */ - void reduceWithInverse(const internal::Reduction& inverseReduction); + 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 /// @{ @@ -235,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 @@ -270,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/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/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)); } From f2a371e6c11cf0cd3a6f3d2de9c4ee95283431d2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 5 Feb 2013 02:12:56 +0000 Subject: [PATCH 16/16] Cleaned up commented code from iSAM2 chain optimizations --- gtsam/inference/BayesNet-inl.h | 12 ------------ gtsam/inference/BayesNet.h | 7 ------- gtsam/inference/BayesTreeCliqueBase-inl.h | 23 ----------------------- gtsam/inference/BayesTreeCliqueBase.h | 7 ------- gtsam/inference/IndexConditional.cpp | 17 ----------------- 5 files changed, 66 deletions(-) diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 5375b209f..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 f8aaa7d17..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();} /// -// bool BayesTreeCliqueBase::permuteSeparatorWithInverse( -// const Permutation& inversePermutation) { -// bool changed = conditional_->permuteSeparatorWithInverse( -// inversePermutation); -//#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); -// } -// } -//#endif -// if (changed) { -// BOOST_FOREACH(const derived_ptr& child, children_) { -// (void) child->permuteSeparatorWithInverse(inversePermutation); -// } -// } -// assertInvariants(); -// return changed; -// } - /* ************************************************************************* */ template bool BayesTreeCliqueBase::reduceSeparatorWithInverse( diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index a00980983..3a9a1ef78 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -178,13 +178,6 @@ namespace gtsam { /** Permute the variables in the whole subtree rooted at this clique */ void permuteWithInverse(const Permutation& inversePermutation); - /** Permute variables when they only appear in the separators. In this - * case the running intersection property will be used to prevent always - * traversing the whole tree. Returns whether any separator variables in - * this subtree were reordered. - */ - //bool permuteSeparatorWithInverse(const Permutation& inversePermutation); - /** Permute variables when they only appear in the separators. In this * case the running intersection property will be used to prevent always * traversing the whole tree. Returns whether any separator variables in diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index 1a5e6d516..4167e56f2 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -43,23 +43,6 @@ namespace gtsam { #endif } - /* ************************************************************************* */ - //bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - //#ifndef NDEBUG - // BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } - //#endif - // bool parentChanged = false; - // BOOST_FOREACH(KeyType& parent, parents()) { - // KeyType newParent = inversePermutation[parent]; - // if(parent != newParent) { - // parentChanged = true; - // parent = newParent; - // } - // } - // assertInvariants(); - // return parentChanged; - //} - /* ************************************************************************* */ bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { #ifndef NDEBUG