diff --git a/.cproject b/.cproject index d1d201f0c..b8e21aead 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + @@ -311,6 +311,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -337,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -345,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -393,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -401,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -409,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -425,20 +428,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -465,6 +459,7 @@ make + testGraph.run true false @@ -536,6 +531,7 @@ make + testInference.run true false @@ -543,6 +539,7 @@ make + testGaussianFactor.run true false @@ -550,6 +547,7 @@ make + testJunctionTree.run true false @@ -557,6 +555,7 @@ make + testSymbolicBayesNet.run true false @@ -564,6 +563,7 @@ make + testSymbolicFactorGraph.run true false @@ -633,22 +633,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -665,6 +649,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -689,26 +689,34 @@ true true - + make - -j2 - all + -j2 VERBOSE=1 + check.nonlinear + true + false + true + + + make + -j5 + timing.nonlinear true true true - + make - -j2 - check + -j5 + nonlinear.testValues.run true true true - + make - -j2 - clean + -j5 + nonlinear.testOrdering.run true true true @@ -761,34 +769,26 @@ true true - + make - -j2 VERBOSE=1 - check.nonlinear - true - false - true - - - make - -j5 - timing.nonlinear + -j2 + all true true true - + make - -j5 - nonlinear.testValues.run + -j2 + check true true true - + make - -j5 - nonlinear.testOrdering.run + -j2 + clean true true true @@ -1139,7 +1139,6 @@ make - testErrors.run true false @@ -1619,6 +1618,7 @@ make + testSimulated2DOriented.run true false @@ -1658,6 +1658,7 @@ make + testSimulated2D.run true false @@ -1665,6 +1666,7 @@ make + testSimulated3D.run true false @@ -1936,6 +1938,7 @@ make + tests/testGaussianISAM2 true false @@ -1957,93 +1960,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - check - true - true - true - - - make - -j5 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j5 - gtsam-shared - true - true - true - - - make - -j5 - gtsam-static - true - true - true - - - make - -j5 - timing - true - true - true - - - make - -j5 - examples - true - true - true - - - make - -j5 - VERBOSE=1 all - true - true - true - - - make - -j5 - VERBOSE=1 check - true - true - true - make -j2 @@ -2140,23 +2056,7 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - + make -j2 install @@ -2164,21 +2064,85 @@ true true - + make -j2 + clean + true + true + true + + + make + -j5 + check + true + true + true + + + make + -j5 all true true true - + cmake .. true false true + + make + -j5 + gtsam-shared + true + true + true + + + make + -j5 + gtsam-static + true + true + true + + + make + -j5 + timing + true + true + true + + + make + -j5 + examples + true + true + true + + + make + -j5 + VERBOSE=1 all + true + true + true + + + make + -j5 + VERBOSE=1 check + true + true + true + make -j5 @@ -2219,6 +2183,45 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + diff --git a/.project b/.project index e52e979df..f59d79cb7 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j8 + -j7 org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 458bbfda4..6df7e8450 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -330,13 +330,7 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, std::vecto internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals()); // 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 + //(*clique)->solveInPlace(deltaNewton); varsUpdated += (*clique)->nrFrontals(); @@ -348,7 +342,7 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, std::vecto } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector& replacedKeys, +size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, Permuted& deltaNewton, Permuted& RgProd) { // Get gradient @@ -358,15 +352,24 @@ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector& rep // Update variables size_t varsUpdated = 0; internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated); + optimizeWildfire(isam.root(), wildfireThreshold, replacedKeys, deltaNewton); - // Make sure all were updated -#ifndef NDEBUG - for(size_t j=0; j(isam.root(), expected); + for(size_t j = 0; j Rd_jfg(isam); + Errors Rg = Rd_jfg * grad; + double RgMagExpected = dot(Rg, Rg); + double RgMagActual = RgProd.container().vector().squaredNorm(); + cout << fabs(RgMagExpected - RgMagActual) << endl; + assert(fabs(RgMagExpected - RgMagActual) < (1e-8 * RgMagActual + 1e-4)); #endif + replacedKeys.assign(replacedKeys.size(), false); + return varsUpdated; } diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 6a956af98..351a371df 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -126,7 +126,7 @@ struct ISAM2::Impl { static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); - static size_t UpdateDoglegDeltas(const ISAM2& isam, std::vector& replacedKeys, + static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, Permuted& deltaNewton, Permuted& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 246e43211..b9a95b51a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -242,6 +242,8 @@ boost::shared_ptr > ISAM2::recalculate( toc(2,"permute global variable index"); tic(3,"permute delta"); delta_.permute(*colamd); + deltaNewton_.permute(*colamd); + RgProd_.permute(*colamd); toc(3,"permute delta"); tic(4,"permute ordering"); ordering_.permuteWithInverse(*colamdInverse); @@ -623,7 +625,14 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { // We may need to update the solution calcaulations if(!isam.deltaDoglegUptodate_) { tic(1, "UpdateDoglegDeltas"); - ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); + double wildfireThreshold = 0.0; + if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else + assert(false); + ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); isam.deltaDoglegUptodate_ = true; toc(1, "UpdateDoglegDeltas"); } @@ -649,7 +658,14 @@ 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_); + double wildfireThreshold = 0.0; + if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams)) + wildfireThreshold = boost::get(isam.params().optimizationParams).wildfireThreshold; + else + assert(false); + ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_); isam.deltaDoglegUptodate_ = true; toc(1, "UpdateDoglegDeltas"); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 6a504009c..ca688ecc0 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -54,15 +54,18 @@ struct ISAM2GaussNewtonParams { */ struct ISAM2DoglegParams { double initialDelta; ///< The initial trust region radius for Dogleg + double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001) DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode bool verbose; ///< Whether Dogleg prints iteration and convergence information /** Specify parameters as constructor arguments */ ISAM2DoglegParams( - double _initialDelta = 1.0, ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::initialDelta - DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::adaptationMode - bool _verbose = false ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::verbose - ) : initialDelta(_initialDelta), adaptationMode(_adaptationMode), verbose(_verbose) {} + double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta + double _wildfireThreshold = 0.0, ///< see ISAM2DoglegParams::wildfireThreshold + DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode + bool _verbose = false ///< see ISAM2DoglegParams::verbose + ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold), + adaptationMode(_adaptationMode), verbose(_verbose) {} }; /**