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

Make error calculation concise

release/4.3a0
Frank Dellaert 2019-06-03 14:12:31 -04:00
parent 3ab9a1e3cc
commit 6f7e92afdc
2 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -185,6 +185,13 @@ struct GTSAM_EXPORT UpdateImpl {
std::inserter(*unusedKeys, unusedKeys->end())); std::inserter(*unusedKeys, unusedKeys->end()));
} }
// Calculate nonlinear error
void error(const NonlinearFactorGraph& nonlinearFactors,
const Values& estimate, boost::optional<double>* error) const {
gttic(error);
error->reset(nonlinearFactors.error(estimate));
}
// Mark linear update // Mark linear update
void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
const NonlinearFactorGraph& nonlinearFactors, const NonlinearFactorGraph& nonlinearFactors,

12
gtsam/nonlinear/ISAM2.cpp
View File

@ -367,6 +367,8 @@ void ISAM2::addVariables(const Values& newTheta,
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::removeVariables(const KeySet& unusedKeys) { void ISAM2::removeVariables(const KeySet& unusedKeys) {
gttic(removeVariables);
variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
for (Key key : unusedKeys) { for (Key key : unusedKeys) {
delta_.erase(key); delta_.erase(key);
@ -431,10 +433,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
// \Theta:=\Theta\cup\Theta_{new}. // \Theta:=\Theta\cup\Theta_{new}.
addVariables(newTheta, result.detail.get_ptr()); addVariables(newTheta, result.detail.get_ptr());
gttic(evaluate_error_before);
if (params_.evaluateNonlinearError) if (params_.evaluateNonlinearError)
result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore);
gttoc(evaluate_error_before);
// 3. Mark linear update // 3. Mark linear update
update.gatherInvolvedKeys(newFactors, nonlinearFactors_, update.gatherInvolvedKeys(newFactors, nonlinearFactors_,
@ -490,16 +490,12 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
// Update data structures to remove unused keys // Update data structures to remove unused keys
if (!result.unusedKeys.empty()) { if (!result.unusedKeys.empty()) {
gttic(remove_variables);
removeVariables(result.unusedKeys); removeVariables(result.unusedKeys);
gttoc(remove_variables);
} }
result.cliques = this->nodes().size(); result.cliques = this->nodes().size();
gttic(evaluate_error_after);
if (params_.evaluateNonlinearError) if (params_.evaluateNonlinearError)
result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); update.error(nonlinearFactors_, calculateEstimate(), &result.errorAfter);
gttoc(evaluate_error_after);
return result; return result;
} }