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

Split up relinearize again for clarity

release/4.3a0
Frank Dellaert 2019-06-02 00:32:06 -04:00
parent 696d8076d1
commit 8b01b81027
3 changed files with 84 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
gtsam/nonlinear/ISAM2-impl.h
View File

@ -135,7 +135,7 @@ struct GTSAM_EXPORT UpdateImpl {
}
}
// 1. Add any new factors \Factors:=\Factors\cup\Factors'.
// Add any new factors \Factors:=\Factors\cup\Factors'.
void pushBackFactors(const NonlinearFactorGraph& newFactors,
NonlinearFactorGraph* nonlinearFactors,
GaussianFactorGraph* linearFactors,
@ -181,7 +181,7 @@ struct GTSAM_EXPORT UpdateImpl {
std::inserter(result->unusedKeys, result->unusedKeys.end()));
}
// 3. Mark linear update
// Mark linear update
void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
const NonlinearFactorGraph& nonlinearFactors,
ISAM2Result* result) const {
@ -197,6 +197,7 @@ struct GTSAM_EXPORT UpdateImpl {
result->markedKeys.insert(key);
}
}
// Also, keys that were not observed in existing factors, but whose affected
// keys have been extended now (e.g. smart factors)
if (updateParams_.newAffectedKeys) {
@ -343,6 +344,67 @@ struct GTSAM_EXPORT UpdateImpl {
return relinKeys;
}
// Mark keys in \Delta above threshold \beta:
KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
const VectorValues& delta,
const KeySet& fixedVariables,
ISAM2Result* result) const {
gttic(gatherRelinearizeKeys);
// J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
KeySet relinKeys =
params_.enablePartialRelinearizationCheck
? CheckRelinearizationPartial(roots, delta,
params_.relinearizeThreshold)
: CheckRelinearizationFull(delta, params_.relinearizeThreshold);
if (updateParams_.forceFullSolve)
relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging
// Remove from relinKeys any keys whose linearization points are fixed
for (Key key : fixedVariables) {
relinKeys.erase(key);
}
if (updateParams_.noRelinKeys) {
for (Key key : *updateParams_.noRelinKeys) {
relinKeys.erase(key);
}
}
// Record above relinerization threshold keys in detailed results
if (params_.enableDetailedResults) {
for (Key key : relinKeys) {
result->detail->variableStatus[key].isAboveRelinThreshold = true;
result->detail->variableStatus[key].isRelinearized = true;
}
}
// Add the variables being relinearized to the marked keys
result->markedKeys.insert(relinKeys.begin(), relinKeys.end());
return relinKeys;
}
// Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors.
void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys,
ISAM2Result* result) const {
gttic(fluidFindAll);
for (const auto& root : roots)
// add other cliques that have the marked ones in the separator
root->findAll(relinKeys, &result->markedKeys);
// Relinearization-involved keys for detailed results
if (params_.enableDetailedResults) {
KeySet involvedRelinKeys;
for (const auto& root : roots)
root->findAll(relinKeys, &involvedRelinKeys);
for (Key key : involvedRelinKeys) {
if (!result->detail->variableStatus[key].isAboveRelinThreshold) {
result->detail->variableStatus[key].isRelinearizeInvolved = true;
result->detail->variableStatus[key].isRelinearized = true;
}
}
}
}
/**
* Apply expmap to the given values, but only for indices appearing in
* \c mask. Values are expmapped in-place.
@ -350,6 +412,7 @@ struct GTSAM_EXPORT UpdateImpl {
*/
static void ExpmapMasked(const VectorValues& delta, const KeySet& mask,
Values* theta) {
gttic(ExpmapMasked);
assert(theta->size() == delta.size());
Values::iterator key_value;
VectorValues::const_iterator key_delta;
@ -372,80 +435,7 @@ struct GTSAM_EXPORT UpdateImpl {
}
}
KeySet relinearize(const ISAM2::Roots& roots, const VectorValues& delta,
const KeySet& fixedVariables, Values* theta,
ISAM2Result* result) const {
KeySet relinKeys;
gttic(gather_relinearize_keys);
// 4. Mark keys in \Delta above threshold \beta:
// J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
if (params_.enablePartialRelinearizationCheck)
relinKeys = CheckRelinearizationPartial(roots, delta,
params_.relinearizeThreshold);
else
relinKeys = CheckRelinearizationFull(delta, params_.relinearizeThreshold);
if (updateParams_.forceFullSolve)
relinKeys =
CheckRelinearizationFull(delta, 0.0); // This is used for debugging
// Remove from relinKeys any keys whose linearization points are fixed
for (Key key : fixedVariables) {
relinKeys.erase(key);
}
if (updateParams_.noRelinKeys) {
for (Key key : *updateParams_.noRelinKeys) {
relinKeys.erase(key);
}
}
// Above relin threshold keys for detailed results
if (params_.enableDetailedResults) {
for (Key key : relinKeys) {
result->detail->variableStatus[key].isAboveRelinThreshold = true;
result->detail->variableStatus[key].isRelinearized = true;
}
}
// Add the variables being relinearized to the marked keys
KeySet markedRelinMask;
for (const Key key : relinKeys) markedRelinMask.insert(key);
result->markedKeys.insert(relinKeys.begin(), relinKeys.end());
gttoc(gather_relinearize_keys);
gttic(fluid_find_all);
// 5. Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors.
if (!relinKeys.empty()) {
for (const auto& root : roots)
// add other cliques that have the marked ones in the separator
root->findAll(markedRelinMask, &result->markedKeys);
// Relin involved keys for detailed results
if (params_.enableDetailedResults) {
KeySet involvedRelinKeys;
for (const auto& root : roots)
root->findAll(markedRelinMask, &involvedRelinKeys);
for (Key key : involvedRelinKeys) {
if (!result->detail->variableStatus[key].isAboveRelinThreshold) {
result->detail->variableStatus[key].isRelinearizeInvolved = true;
result->detail->variableStatus[key].isRelinearized = true;
}
}
}
}
gttoc(fluid_find_all);
gttic(expmap);
// 6. Update linearization point for marked variables:
// \Theta_{J}:=\Theta_{J}+\Delta_{J}.
if (!relinKeys.empty()) ExpmapMasked(delta, markedRelinMask, theta);
gttoc(expmap);
result->variablesRelinearized = result->markedKeys.size();
return relinKeys;
}
// 7. Linearize new factors
// Linearize new factors
void linearizeNewFactors(const NonlinearFactorGraph& newFactors,
const Values& theta, size_t numNonlinearFactors,
const FactorIndices& newFactorsIndices,
@ -625,8 +615,8 @@ struct GTSAM_EXPORT UpdateImpl {
// Removed unused keys:
VariableIndex affectedFactorsVarIndex = variableIndex;
affectedFactorsVarIndex.removeUnusedVariables(
result->unusedKeys.begin(), result->unusedKeys.end());
affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(),
result->unusedKeys.end());
for (const Key key : result->unusedKeys) {
affectedKeysSet.erase(key);

21
gtsam/nonlinear/ISAM2.cpp
View File

@ -119,9 +119,6 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
this->update_count_++;
UpdateImpl::LogStartingUpdate(newFactors, *this);
ISAM2Result result;
if (params_.enableDetailedResults)
result.detail = ISAM2Result::DetailedResults();
const bool relinearizeThisStep =
updateParams.force_relinearize ||
(params_.enableRelinearization &&
@ -132,6 +129,9 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
updateDelta(updateParams.forceFullSolve);
}
ISAM2Result result;
if (params_.enableDetailedResults)
result.detail = ISAM2Result::DetailedResults();
UpdateImpl update(params_, updateParams);
// 1. Add any new factors \Factors:=\Factors\cup\Factors'.
@ -150,14 +150,25 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result);
// Check relinearization if we're at the nth step, or we are using a looser
// loop relin threshold
// loop relinerization threshold.
KeySet relinKeys;
if (relinearizeThisStep) {
// 4. Mark keys in \Delta above threshold \beta:
relinKeys =
update.relinearize(roots_, delta_, fixedVariables_, &theta_, &result);
update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, &result);
if (!relinKeys.empty()) {
// 5. Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors.
update.fluidFindAll(roots_, relinKeys, &result);
// 6. Update linearization point for marked variables:
// \Theta_{J}:=\Theta_{J}+\Delta_{J}.
UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_);
}
result.variablesRelinearized = result.markedKeys.size();
} else {
result.variablesRelinearized = 0;
}
// TODO(frank): should be result.variablesRelinearized = relinKeys.size(); ?
// 7. Linearize new factors
update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(),

1
tests/testGaussianISAM2.cpp
View File

@ -5,7 +5,6 @@
*/
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>