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

Added optional iSAM2 parameter that forces extra keys into the re-eliminated set. This can be used to force certain keys to be re-ordered.

release/4.3a0
Stephen Williams 2013-04-08 18:05:48 +00:00
parent 4c6a460fa0
commit 9c4e19d141
2 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/nonlinear/ISAM2.cpp
View File

@ -552,7 +552,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
/* ************************************************************************* */ /* ************************************************************************* */
ISAM2Result ISAM2::update( ISAM2Result ISAM2::update(
const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices, const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastMap<Key,int> >& constrainedKeys, const boost::optional<FastList<Key> >& noRelinKeys, bool force_relinearize) { const boost::optional<FastMap<Key,int> >& constrainedKeys, const boost::optional<FastList<Key> >& noRelinKeys,
const boost::optional<FastList<Key> >& extraReelimKeys, bool force_relinearize) {
const bool debug = ISDEBUG("ISAM2 update"); const bool debug = ISDEBUG("ISAM2 update");
const bool verbose = ISDEBUG("ISAM2 update verbose"); const bool verbose = ISDEBUG("ISAM2 update verbose");
@ -648,6 +649,12 @@ ISAM2Result ISAM2::update(
FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
} }
// Also mark any provided extra re-eliminate keys
if(extraReelimKeys) {
BOOST_FOREACH(Key key, *extraReelimKeys) {
markedKeys.insert(ordering_.at(key));
}
}
// Observed keys for detailed results // Observed keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {

5
gtsam/nonlinear/ISAM2.h
View File

@ -544,12 +544,17 @@ public:
* (Params::relinearizeSkip). * (Params::relinearizeSkip).
* @param constrainedKeys is an optional map of keys to group labels, such that a variable can * @param constrainedKeys is an optional map of keys to group labels, such that a variable can
* be constrained to a particular grouping in the BayesTree * be constrained to a particular grouping in the BayesTree
* @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization
* point, regardless of the size of the linear delta
* @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless
* of the size of the linear delta. This allows the provided keys to be reordered.
* @return An ISAM2Result struct containing information about the update * @return An ISAM2Result struct containing information about the update
*/ */
GTSAM_EXPORT ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), GTSAM_EXPORT ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(), const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none, const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none,
const boost::optional<FastList<Key> >& noRelinKeys = boost::none, const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
bool force_relinearize = false); bool force_relinearize = false);
/** Marginalize out variables listed in leafKeys. These keys must be leaves /** Marginalize out variables listed in leafKeys. These keys must be leaves