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

Added the ability to remove factors from ConcurrentIncrementalFilter

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

11
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -42,7 +42,8 @@ bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const boost::optional<FastList<Key> >& keysToMove) { ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
const boost::optional<FastList<Key> >& keysToMove, const boost::optional< std::vector<size_t> >& removeFactorIndices) {
gttic(update); gttic(update);
@ -54,8 +55,11 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
// Create the return result meta-data // Create the return result meta-data
Result result; Result result;
// We do not need to remove any factors at this time // Remove any user-provided factors from iSAM2
gtsam::FastVector<size_t> removedFactors; gtsam::FastVector<size_t> removedFactors;
if(removeFactorIndices){
removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
}
// Generate ordering constraints that force the 'keys to move' to the end // Generate ordering constraints that force the 'keys to move' to the end
boost::optional<gtsam::FastMap<gtsam::Key,int> > orderingConstraints = boost::none; boost::optional<gtsam::FastMap<gtsam::Key,int> > orderingConstraints = boost::none;
@ -147,7 +151,8 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
result.iterations = 1; result.iterations = 1;
result.linearVariables = isam2_.getFixedVariables().size(); result.linearVariables = isam2_.getFixedVariables().size();
result.nonlinearVariables = isam2_.getLinearizationPoint().size() - result.linearVariables; result.nonlinearVariables = isam2_.getLinearizationPoint().size() - result.linearVariables;
result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate()); result.newFactorsIndices = isam2Result.newFactorsIndices;
// result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate());
if(debug) std::cout << "ConcurrentIncrementalFilter::update End" << std::endl; if(debug) std::cout << "ConcurrentIncrementalFilter::update End" << std::endl;

5
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
View File

@ -38,6 +38,7 @@ public:
size_t iterations; ///< The number of optimizer iterations performed size_t iterations; ///< The number of optimizer iterations performed
size_t nonlinearVariables; ///< The number of variables that can be relinearized 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 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 double error; ///< The final factor graph error
/// Constructor /// Constructor
@ -110,9 +111,11 @@ public:
* You must include here all new variables occurring in newFactors that were not already * You must include here all new variables occurring in newFactors that were not already
* in the filter. * in the filter.
* @param keysToMove An optional set of keys to move from the filter to the smoother * @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
*/ */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), 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. * Perform any required operations before the synchronization process starts.