From 5b9271cb97a5ed0668ac3be7f2840bb19ef79a52 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 23 Nov 2012 23:22:53 +0000 Subject: [PATCH] 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