From 34a3a6ea49b3064973a3d0bbb08473b09c41b734 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sat, 10 Aug 2013 17:16:38 +0000 Subject: [PATCH] Added the ability to remove factors from the ConcurrentBatchFilter --- .../nonlinear/ConcurrentBatchFilter.cpp | 19 ++++++++++--------- .../nonlinear/ConcurrentBatchFilter.h | 9 ++++++--- 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 74551f8e4..f0adef0c8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -132,7 +132,8 @@ bool ConcurrentBatchFilter::equals(const ConcurrentFilter& rhs, double tol) cons } /* ************************************************************************* */ -ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const boost::optional >& keysToMove) { +ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, + const boost::optional >& keysToMove, const boost::optional< std::vector >& removeFactorIndices) { gttic(update); @@ -163,7 +164,10 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto delta_[i].setZero(); } // Add the new factors to the graph, updating the variable index - insertFactors(newFactors); + result.newFactorsIndices = insertFactors(newFactors); + // Remove any user-specified factors from the graph + if(removeFactorIndices) + removeFactors(*removeFactorIndices); gttoc(augment_system); if(debug) std::cout << "ConcurrentBatchFilter::update Reordering System ..." << std::endl; @@ -178,7 +182,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto // Optimize the factors using a modified version of L-M gttic(optimize); if(factors_.size() > 0) { - result = optimize(factors_, theta_, ordering_, delta_, separatorValues_, parameters_); + optimize(factors_, theta_, ordering_, delta_, separatorValues_, parameters_, result); } gttoc(optimize); @@ -398,8 +402,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT } /* ************************************************************************* */ -ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, - VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) { +void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, + VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters, + ConcurrentBatchFilter::Result& result) { // const bool debug = ISDEBUG("ConcurrentBatchFilter optimize"); const bool debug = false; @@ -407,7 +412,6 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac if(debug) std::cout << "ConcurrentBatchFilter::optimize Begin" << std::endl; // Create output result structure - Result result; result.nonlinearVariables = theta.size() - linearValues.size(); result.linearVariables = linearValues.size(); @@ -428,7 +432,6 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac // check if we're already close enough if(result.error <= errorTol) { if(debug) { std::cout << "Exiting, as error = " << result.error << " < " << errorTol << std::endl; } - return result; } if(debug) { @@ -535,8 +538,6 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac if(debug) { std::cout << "newError: " << result.error << std::endl; } if(debug) std::cout << "ConcurrentBatchFilter::optimize End" << std::endl; - - return result; } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index b0d8ab546..96752bd4e 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -40,6 +40,7 @@ public: size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point + std::vector newFactorsIndices; ///< The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed in double error; ///< The final factor graph error /// Constructor @@ -115,9 +116,10 @@ public: * You must include here all new variables occurring in newFactors that were not already * in the filter. * @param keysToMove An optional set of keys to move from the filter to the smoother + * @param removeFactorIndices An optional set of indices corresponding to the factors you want to remove from the graph */ virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional >& keysToMove = boost::none); + const boost::optional >& keysToMove = boost::none, const boost::optional< std::vector >& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. @@ -203,8 +205,9 @@ private: void moveSeparator(const FastList& keysToMove); /** Use a modified version of L-M to update the linearization point and delta */ - static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, - VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters); + static void optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, + VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters, + Result& result); /** Print just the nonlinear keys in a nonlinear factor */ static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,