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

Made detail handling more explicit in update

release/4.3a0
Frank Dellaert 2019-06-03 13:53:37 -04:00
parent 62233395b2
commit 3ab9a1e3cc
5 changed files with 39 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -218,7 +218,7 @@ struct GTSAM_EXPORT UpdateImpl {
void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
gttic(updateKeys);
// Observed keys for detailed results
if (params_.enableDetailedResults) {
if (result->detail && params_.enableDetailedResults) {
for (Key key : markedKeys) {
result->detail->variableStatus[key].isObserved = true;
}
@ -359,8 +359,8 @@ struct GTSAM_EXPORT UpdateImpl {
// Mark keys in \Delta above threshold \beta:
KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
const VectorValues& delta,
const KeySet& fixedVariables, KeySet* markedKeys,
ISAM2Result* result) const {
const KeySet& fixedVariables,
KeySet* markedKeys) const {
gttic(gatherRelinearizeKeys);
// J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
KeySet relinKeys =
@ -381,37 +381,41 @@ struct GTSAM_EXPORT UpdateImpl {
}
}
// 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
markedKeys->insert(relinKeys.begin(), relinKeys.end());
return relinKeys;
}
// Record relinerization threshold keys in detailed results
void recordRelinearizeDetail(const KeySet& relinKeys,
ISAM2Result::DetailedResults* detail) const {
if (detail && params_.enableDetailedResults) {
for (Key key : relinKeys) {
detail->variableStatus[key].isAboveRelinThreshold = true;
detail->variableStatus[key].isRelinearized = true;
}
}
}
// Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors.
void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys,
KeySet* markedKeys, ISAM2Result* result) const {
KeySet* markedKeys,
ISAM2Result::DetailedResults* detail) const {
gttic(fluidFindAll);
for (const auto& root : roots)
// add other cliques that have the marked ones in the separator
root->findAll(relinKeys, markedKeys);
// Relinearization-involved keys for detailed results
if (params_.enableDetailedResults) {
if (detail && 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;
if (!detail->variableStatus[key].isAboveRelinThreshold) {
detail->variableStatus[key].isRelinearizeInvolved = true;
detail->variableStatus[key].isRelinearized = true;
}
}
}

19
gtsam/nonlinear/ISAM2.cpp
View File

@ -337,7 +337,7 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
}
// Root clique variables for detailed results
if (params_.enableDetailedResults) {
if (result->detail && params_.enableDetailedResults) {
for (const auto& root : roots_)
for (Key var : *root->conditional())
result->detail->variableStatus[var].inRootClique = true;
@ -346,9 +346,8 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams,
return affectedKeysSet;
}
/* ************************************************************************* */
void ISAM2::addVariables(
const Values& newTheta,
ISAM2Result::DetailedResults::StatusMap* variableStatus) {
void ISAM2::addVariables(const Values& newTheta,
ISAM2Result::DetailedResults* detail) {
gttic(addNewVariables);
theta_.insert(newTheta);
@ -359,9 +358,9 @@ void ISAM2::addVariables(
RgProd_.insert(newTheta.zeroVectors());
// New keys for detailed results
if (variableStatus && params_.enableDetailedResults) {
if (detail && params_.enableDetailedResults) {
for (Key key : newTheta.keys()) {
(*variableStatus)[key].isNew = true;
detail->variableStatus[key].isNew = true;
}
}
}
@ -430,7 +429,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
// 2. Initialize any new variables \Theta_{new} and add
// \Theta:=\Theta\cup\Theta_{new}.
addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0);
addVariables(newTheta, result.detail.get_ptr());
gttic(evaluate_error_before);
if (params_.evaluateNonlinearError)
@ -448,11 +447,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
if (relinearizeThisStep) {
// 4. Mark keys in \Delta above threshold \beta:
relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_,
&result.markedKeys, &result);
&result.markedKeys);
update.recordRelinearizeDetail(relinKeys, result.detail.get_ptr());
if (!relinKeys.empty()) {
// 5. Mark all cliques that involve marked variables \Theta_{J} and all
// their ancestors.
update.fluidFindAll(roots_, relinKeys, &result.markedKeys, &result);
update.fluidFindAll(roots_, relinKeys, &result.markedKeys,
result.detail.get_ptr());
// 6. Update linearization point for marked variables:
// \Theta_{J}:=\Theta_{J}+\Delta_{J}.
UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_);

5
gtsam/nonlinear/ISAM2.h
View File

@ -307,9 +307,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
* @param newTheta Initial values for new variables
* @param variableStatus optional detailed result structure
*/
void addVariables(
const Values& newTheta,
ISAM2Result::DetailedResults::StatusMap* variableStatus = 0);
void addVariables(const Values& newTheta,
ISAM2Result::DetailedResults* detail = 0);
/**
* Remove variables from the ISAM2 system.

6
gtsam/nonlinear/ISAM2Params.h
View File

@ -234,8 +234,8 @@ struct GTSAM_EXPORT ISAM2Params {
Factorization _factorization = ISAM2Params::CHOLESKY,
bool _cacheLinearizedFactors = true,
const KeyFormatter& _keyFormatter =
DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
)
DefaultKeyFormatter, ///< see ISAM2::Params::keyFormatter,
bool _enableDetailedResults = false)
: optimizationParams(_optimizationParams),
relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip),
@ -244,7 +244,7 @@ struct GTSAM_EXPORT ISAM2Params {
factorization(_factorization),
cacheLinearizedFactors(_cacheLinearizedFactors),
keyFormatter(_keyFormatter),
enableDetailedResults(false),
enableDetailedResults(_enableDetailedResults),
enablePartialRelinearizationCheck(false),
findUnusedFactorSlots(false) {}

6
tests/testGaussianISAM2.cpp
View File

@ -47,9 +47,11 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0,
ISAM2 createSlamlikeISAM2(
boost::optional<Values&> init_values = boost::none,
boost::optional<NonlinearFactorGraph&> full_graph = boost::none,
const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true),
const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0,
0, false, true,
ISAM2Params::CHOLESKY, true,
DefaultKeyFormatter, true),
size_t maxPoses = 10) {
// These variables will be reused and accumulate factors and values
ISAM2 isam(params);
Values fullinit;