12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added the ability to remove factors from the ConcurrentBatchFilter

release/4.3a0
Stephen Williams 2013-08-10 17:16:38 +00:00
parent ebc6a66c3a
commit 34a3a6ea49
2 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -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<FastList<Key> >& keysToMove) {
ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
const boost::optional<FastList<Key> >& keysToMove, const boost::optional< std::vector<size_t> >& 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<FastList<Key> >& 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;
}
/* ************************************************************************* */

9
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -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<size_t> 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<FastList<Key> >& keysToMove = boost::none);
const boost::optional<FastList<Key> >& keysToMove = boost::none, const boost::optional< std::vector<size_t> >& removeFactorIndices = boost::none);
/**
* Perform any required operations before the synchronization process starts.
@ -203,8 +205,9 @@ private:
void moveSeparator(const FastList<Key>& 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,