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

Added an argument to ISAM2::update to not change the linearization point of a given set of variables

release/4.3a0
Richard Roberts 2013-02-21 15:59:50 +00:00
parent 0daf989740
commit b4f5413989
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/ISAM2.cpp
View File

@ -548,7 +548,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, bool force_relinearize) { const boost::optional<FastMap<Key,int> >& constrainedKeys,
const boost::optional<FastList<Key> >& noRelinKeys, 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");
@ -665,7 +666,6 @@ ISAM2Result ISAM2::update(
FastSet<Index> relinKeys; FastSet<Index> relinKeys;
if (relinearizeThisStep) { if (relinearizeThisStep) {
gttic(gather_relinearize_keys); gttic(gather_relinearize_keys);
vector<bool> markedRelinMask(ordering_.size(), false);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
if(params_.enablePartialRelinearizationCheck) if(params_.enablePartialRelinearizationCheck)
relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold); relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold);
@ -673,6 +673,13 @@ ISAM2Result ISAM2::update(
relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold); relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold);
if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging
// Remove from relinKeys any keys whose linearization points are fixed
if(noRelinKeys) {
BOOST_FOREACH(Key key, *noRelinKeys) {
relinKeys.erase(ordering_[key]);
}
}
// Above relin threshold keys for detailed results // Above relin threshold keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, relinKeys) { BOOST_FOREACH(Index index, relinKeys) {
@ -680,6 +687,7 @@ ISAM2Result ISAM2::update(
result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
// Add the variables being relinearized to the marked keys // Add the variables being relinearized to the marked keys
vector<bool> markedRelinMask(ordering_.size(), false);
BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
markedKeys.insert(relinKeys.begin(), relinKeys.end()); markedKeys.insert(relinKeys.begin(), relinKeys.end());
gttoc(gather_relinearize_keys); gttoc(gather_relinearize_keys);

1
gtsam/nonlinear/ISAM2.h
View File

@ -544,6 +544,7 @@ public:
ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), 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,
bool force_relinearize = false); bool force_relinearize = false);
/** Access the current linearization point */ /** Access the current linearization point */