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