From 4c2581f40e02436305855fa86150d1986d70d5a6 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 17 Mar 2012 23:57:44 +0000 Subject: [PATCH] In progress --- .cproject | 2 +- .project | 2 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 11 ++++++++--- gtsam/nonlinear/ISAM2-impl.cpp | 7 +++++++ gtsam/nonlinear/ISAM2-impl.h | 3 ++- gtsam/nonlinear/ISAM2-inl.h | 8 ++++++++ gtsam/nonlinear/ISAM2.cpp | 19 ++++++++++--------- gtsam/nonlinear/ISAM2.h | 8 ++++++++ gtsam/nonlinear/Values.h | 4 ++++ 9 files changed, 49 insertions(+), 15 deletions(-) diff --git a/.cproject b/.cproject index 6a32e5c08..d1d201f0c 100644 --- a/.cproject +++ b/.cproject @@ -72,7 +72,7 @@ - + diff --git a/.project b/.project index e52e979df..79f74bfbc 100644 --- a/.project +++ b/.project @@ -31,7 +31,7 @@ org.eclipse.cdt.make.core.buildLocation - ${ProjDirPath}/build + ${workspace_loc:/gtsam/build-timing} org.eclipse.cdt.make.core.cleanBuildTarget diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 5026a155f..999147156 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -135,9 +135,14 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( const F& f, const VALUES& x0, const Ordering& ordering, const double f_error, const bool verbose) { // Compute steepest descent and Newton's method points - tic(0, "Steepest Descent"); - VectorValues dx_u = optimizeGradientSearch(Rd); - toc(0, "Steepest Descent"); + tic(0, "optimizeGradientSearch"); + tic(0, "allocateVectorValues"); + VectorValues dx_u = *allocateVectorValues(Rd); + toc(0, "allocateVectorValues"); + tic(1, "optimizeGradientSearchInPlace"); + optimizeGradientSearchInPlace(Rd, dx_u); + toc(1, "optimizeGradientSearchInPlace"); + toc(0, "optimizeGradientSearch"); tic(1, "optimize"); VectorValues dx_n = optimize(Rd); toc(1, "optimize"); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index b6d018796..807111753 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -23,6 +23,7 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Permuted& deltaNewton, Permuted& deltaGradSearch, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -37,6 +38,12 @@ void ISAM2::Impl::AddVariables( delta.container().append(dims); delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); delta.permutation().resize(originalnVars + newTheta.size()); + deltaNewton.container().append(dims); + deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaNewton.permutation().resize(originalnVars + newTheta.size()); + deltaGradSearch.container().append(dims); + deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaGradSearch.permutation().resize(originalnVars + newTheta.size()); { Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f49a385bc..be3f010e3 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -48,7 +48,8 @@ struct ISAM2::Impl { * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, + Permuted& deltaNewton, Permuted& deltaGradSearch, vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 89f727f8b..04395069c 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -29,6 +29,14 @@ namespace gtsam { using namespace std; +/* ************************************************************************* */ +template +VALUE ISAM2::calculateEstimate(Key key) const { + const Index index = getOrdering()[key]; + const SubVector delta = getDelta()[index]; + return theta_.at(key).retract(delta); +} + /* ************************************************************************* */ namespace internal { template diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 58cc4b23c..f302e174e 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -593,14 +593,6 @@ Values ISAM2::calculateEstimate() const { return ret; } -/* ************************************************************************* */ -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const SubVector delta = getDelta()[index]; - return theta_.at(key).retract(delta); -} - /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); @@ -617,11 +609,20 @@ const Permuted& ISAM2::getDelta() const { /* ************************************************************************* */ VectorValues optimize(const ISAM2& isam) { + tic(0, "allocateVectorValues"); VectorValues delta = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), delta); + toc(0, "allocateVectorValues"); + optimizeInPlace(isam, delta); return delta; } +/* ************************************************************************* */ +void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { + tic(1, "optimizeInPlace"); + internal::optimizeInPlace(isam.root(), delta); + toc(1, "optimizeInPlace"); +} + /* ************************************************************************* */ VectorValues optimizeGradientSearch(const ISAM2& isam) { tic(0, "Allocate VectorValues"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2d5b16d1b..535f21c75 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -292,6 +292,11 @@ protected: */ mutable Permuted delta_; + VectorValues deltaNewtonUnpermuted_; + mutable Permuted deltaNewtonUnpermuted_; + VectorValues deltaGradSearchUnpermuted_; + mutable Permuted deltaGradSearchUnpermuted_; + /** Indicates whether the current delta is up-to-date, only used * internally - delta will always be updated if necessary when it is * requested with getDelta() or calculateEstimate(). @@ -459,6 +464,9 @@ private: /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ VectorValues optimize(const ISAM2& isam); +/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ +void optimizeInPlace(const ISAM2& isam, VectorValues& delta); + /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain /// all variables that are contained in the top of the Bayes tree that has been diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bdd1739da..5f6dad022 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -117,6 +117,8 @@ namespace gtsam { typedef boost::transform_iterator< boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + typedef KeyValuePair value_type; + private: template struct _KeyValuePair { @@ -143,6 +145,7 @@ namespace gtsam { /** A key-value pair, with the value a specific derived Value type. */ typedef _KeyValuePair KeyValuePair; typedef _ConstKeyValuePair ConstKeyValuePair; + typedef KeyValuePair value_type; typedef boost::transform_iterator< @@ -208,6 +211,7 @@ namespace gtsam { public: /** A const key-value pair, with the value a specific derived Value type. */ typedef _ConstKeyValuePair KeyValuePair; + typedef KeyValuePair value_type; typedef typename Filtered::const_const_iterator iterator; typedef typename Filtered::const_const_iterator const_iterator;