diff --git a/.project b/.project index 79f74bfbc..e52e979df 100644 --- a/.project +++ b/.project @@ -31,7 +31,7 @@ org.eclipse.cdt.make.core.buildLocation - ${workspace_loc:/gtsam/build-timing} + ${ProjDirPath}/build org.eclipse.cdt.make.core.cleanBuildTarget diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 0d6c8fc73..2fca36b6a 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -194,7 +194,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES static const bool debug = false; if(debug) conditional.print("Solving conditional in place"); - Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); + Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); xS = conditional.get_d() - conditional.get_S() * xS; Vector soln = conditional.permutation().transpose() * conditional.get_R().triangularView().solve(xS); @@ -202,7 +202,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on "); gtsam::print(soln, "full back-substitution solution: "); } - writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals()); + internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals()); } /* ************************************************************************* */ @@ -217,7 +217,7 @@ void GaussianConditional::solveInPlace(Permuted& x) const { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { - Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); + Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); // TODO: verify permutation frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R())); GaussianConditional::const_iterator it; @@ -225,14 +225,14 @@ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { const Index i = *it; transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]); } - writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); + internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); } /* ************************************************************************* */ void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { - Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); + Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); frontalVec = emul(frontalVec, get_sigmas()); - writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); + internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); } } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 6c3ec797e..4edca2521 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -156,3 +156,19 @@ void VectorValues::operator+=(const VectorValues& c) { assert(this->hasSameStructure(c)); this->values_ += c.values_; } + +/* ************************************************************************* */ +VectorValues& VectorValues::operator=(const Permuted& rhs) { + if(this->size() != rhs.size()) + throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); + for(size_t j=0; jsize(); ++j) { + if(exists(j)) { + SubVector& l(this->at(j)); + const SubVector& r(rhs[j]); + if(l.rows() != r.rows()) + throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); + l = r; + } + } + return *this; +} diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 479710885..869fde4bb 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -273,6 +274,11 @@ namespace gtsam { */ void operator+=(const VectorValues& c); + /** Assignment operator from Permuted, requires the dimensions + * of the assignee to already be properly pre-allocated. + */ + VectorValues& operator=(const Permuted& rhs); + /// @} private: diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 13029493e..458bbfda4 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -22,8 +22,8 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Permuted& deltaNewton, Permuted& deltaGradSearch, + const Values& newTheta, Values& theta, Permuted& delta, + Permuted& deltaNewton, Permuted& deltaGradSearch, vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -48,6 +48,8 @@ void ISAM2::Impl::AddVariables( Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { delta.permutation()[nextVar] = nextVar; + deltaNewton.permutation()[nextVar] = nextVar; + deltaGradSearch.permutation()[nextVar] = nextVar; ordering.insert(key_value.key, nextVar); if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; ++ nextVar; @@ -301,21 +303,47 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: /* ************************************************************************* */ namespace internal { -size_t updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, - const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd, vector& updated) { +void updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, + const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd, size_t& varsUpdated) { + // Check if any frontal or separator keys were recalculated, if so, we need + // update deltas and recurse to children, but if not, we do not need to + // recurse further because of the running separator property. + bool anyReplaced = false; + BOOST_FOREACH(Index j, *clique->conditional()) { + if(replacedKeys[j]) { + anyReplaced = true; + break; + } + } + if(anyReplaced) { + // Update the current variable + // Get VectorValues slice corresponding to current variables + Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals()); + Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents()); - // Update the current variable - // Get VectorValues slice corresponding to current variables - Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals()); - Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents()); + // Compute R*g and S*g for this clique + Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS; - // Compute R*g and S*g for this clique - Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS; + // Write into RgProd vector + internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals()); - // Write into RgProd vector - internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->begin(), (*clique)->end()); + // Now solve the part of the Newton's method point for this clique (back-substitution) + (*clique)->solveInPlace(deltaNewton); + + // If debugging, set recalculated keys to false so we can check them later +#ifndef NDEBUG + BOOST_FOREACH(Index j, (*clique)->frontals()) { + replacedKeys[j] = false; } +#endif + + varsUpdated += (*clique)->nrFrontals(); + + // Recurse to children + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + updateDoglegDeltas(child, replacedKeys, grad, deltaNewton, RgProd, varsUpdated); } + } } } @@ -323,15 +351,23 @@ size_t updateDoglegDeltas(const boost::shared_ptr& clique, std::vec size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector& replacedKeys, Permuted& deltaNewton, Permuted& RgProd) { - // Keep a set of flags for whether each variable has been updated. - vector updated(replacedKeys.size()); - // Get gradient VectorValues grad = *allocateVectorValues(isam); gradientAtZero(isam, grad); // Update variables - return internal::updateDoglegDeltas(root, replacedKeys, grad, deltaNewton, RgProd, updated); + size_t varsUpdated = 0; + internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated); + + // Make sure all were updated +#ifndef NDEBUG + for(size_t j=0; j& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); static size_t UpdateDoglegDeltas(const ISAM2& isam, std::vector& replacedKeys, - const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd); + Permuted& deltaNewton, Permuted& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6b66c1ce3..246e43211 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -38,7 +38,8 @@ static const double batchThreshold = 0.65; /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { + delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), + deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -46,7 +47,8 @@ ISAM2::ISAM2(const ISAM2Params& params): /* ************************************************************************* */ ISAM2::ISAM2(): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { + delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), + deltaDoglegUptodate_(true), deltaUptodate_(true) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -344,6 +346,8 @@ boost::shared_ptr > ISAM2::recalculate( toc(3,"permute global variable index"); tic(4,"permute delta"); delta_.permute(partialSolveResult.fullReordering); + deltaNewton_.permute(partialSolveResult.fullReordering); + RgProd_.permute(partialSolveResult.fullReordering); toc(4,"permute delta"); tic(5,"permute ordering"); ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); @@ -441,7 +445,7 @@ ISAM2Result ISAM2::update( tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_); + Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_); toc(2,"add new variables"); tic(3,"evaluate error before"); @@ -543,6 +547,7 @@ ISAM2Result ISAM2::update( toc(10,"evaluate error after"); result.cliques = this->nodes().size(); + deltaDoglegUptodate_ = false; deltaUptodate_ = false; return result; @@ -575,9 +580,6 @@ void ISAM2::updateDelta(bool forceFullSolve) const { doglegDelta_ = doglegResult.Delta; delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution - - // Clear replaced mask - deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); } deltaUptodate_ = true; @@ -618,9 +620,17 @@ VectorValues optimize(const ISAM2& isam) { /* ************************************************************************* */ void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { - tic(1, "optimizeInPlace"); - internal::optimizeInPlace(isam.root(), delta); - toc(1, "optimizeInPlace"); + // We may need to update the solution calcaulations + if(!isam.deltaDoglegUptodate_) { + tic(1, "UpdateDoglegDeltas"); + ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); + isam.deltaDoglegUptodate_ = true; + toc(1, "UpdateDoglegDeltas"); + } + + tic(2, "copy delta"); + delta = isam.deltaNewton_; + toc(2, "copy delta"); } /* ************************************************************************* */ @@ -635,27 +645,30 @@ VectorValues optimizeGradientSearch(const ISAM2& isam) { } /* ************************************************************************* */ -void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { +void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { + // We may need to update the solution calcaulations + if(!isam.deltaDoglegUptodate_) { + tic(1, "UpdateDoglegDeltas"); + ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); + isam.deltaDoglegUptodate_ = true; + toc(1, "UpdateDoglegDeltas"); + } + tic(1, "Compute Gradient"); // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(Rd, grad); + gradientAtZero(isam, grad); double gradientSqNorm = grad.dot(grad); toc(1, "Compute Gradient"); - tic(2, "Compute R*g"); - // Compute R * g - FactorGraph Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - toc(2, "Compute R*g"); - - tic(3, "Compute minimizing step size"); + tic(2, "Compute minimizing step size"); // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - toc(3, "Compute minimizing step size"); + double RgNormSq = isam.RgProd_.container().vector().squaredNorm(); + double step = -gradientSqNorm / RgNormSq; + toc(2, "Compute minimizing step size"); tic(4, "Compute point"); // Compute steepest descent point - scal(step, grad); + grad.vector() *= step; toc(4, "Compute point"); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8d5c49268..6a504009c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -296,6 +296,7 @@ protected: mutable Permuted deltaNewton_; VectorValues RgProdUnpermuted_; mutable Permuted RgProd_; + mutable bool deltaDoglegUptodate_; /** Indicates whether the current delta is up-to-date, only used * internally - delta will always be updated if necessary when it is @@ -359,6 +360,11 @@ public: newISAM2->variableIndex_ = variableIndex_; newISAM2->deltaUnpermuted_ = deltaUnpermuted_; newISAM2->delta_ = delta_; + newISAM2->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_; + newISAM2->deltaNewton_ = deltaNewton_; + newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_; + newISAM2->RgProd_ = RgProd_; + newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_; newISAM2->deltaUptodate_ = deltaUptodate_; newISAM2->deltaReplacedMask_ = deltaReplacedMask_; newISAM2->nonlinearFactors_ = nonlinearFactors_; @@ -459,6 +465,9 @@ private: // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; + friend void optimizeInPlace(const ISAM2&, VectorValues&); + friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&); + }; // ISAM2 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 831d17478..a990d2ded 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -29,7 +29,7 @@ using boost::shared_ptr; const double tol = 1e-4; /* ************************************************************************* */ -TEST(ISAM2, AddVariables) { +TEST_UNSAFE(ISAM2, AddVariables) { // Create initial state Values theta; @@ -48,6 +48,26 @@ TEST(ISAM2, AddVariables) { Permuted delta(permutation, deltaUnpermuted); + VectorValues deltaNewtonUnpermuted; + deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5)); + + Permutation permutationNewton(2); + permutationNewton[0] = 1; + permutationNewton[1] = 0; + + Permuted deltaNewton(permutationNewton, deltaNewtonUnpermuted); + + VectorValues deltaRgUnpermuted; + deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3)); + deltaRgUnpermuted.insert(1, Vector_(2, .4, .5)); + + Permutation permutationRg(2); + permutationRg[0] = 1; + permutationRg[1] = 0; + + Permuted deltaRg(permutationRg, deltaRgUnpermuted); + vector replacedKeys(2, false); Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); @@ -78,6 +98,30 @@ TEST(ISAM2, AddVariables) { Permuted deltaExpected(permutationExpected, deltaUnpermutedExpected); + VectorValues deltaNewtonUnpermutedExpected; + deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5)); + deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); + + Permutation permutationNewtonExpected(3); + permutationNewtonExpected[0] = 1; + permutationNewtonExpected[1] = 0; + permutationNewtonExpected[2] = 2; + + Permuted deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected); + + VectorValues deltaRgUnpermutedExpected; + deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5)); + deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); + + Permutation permutationRgExpected(3); + permutationRgExpected[0] = 1; + permutationRgExpected[1] = 0; + permutationRgExpected[2] = 2; + + Permuted deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected); + vector replacedKeysExpected(3, false); Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); @@ -86,11 +130,15 @@ TEST(ISAM2, AddVariables) { 3, ISAM2::sharedClique()); // Expand initial state - ISAM2::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); + ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation())); + EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted)); + EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation())); + EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted)); + EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation())); EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); }