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

update HybridNonlinearFactorGraph::restrict to ignore empty discrete factors

release/4.3a0
Varun Agrawal 2025-04-11 15:19:24 -04:00
parent d2d1e2c7b0
commit 4a7cf4cc40
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -237,12 +237,16 @@ HybridNonlinearFactorGraph HybridNonlinearFactorGraph::restrict(
if (auto hf = dynamic_pointer_cast<HybridFactor>(f)) {
result.push_back(hf->restrict(discreteValues));
} else if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
result.push_back(df->restrict(discreteValues));
// In the case where all the values have been selected, we ignore the
// factor since it doesn't add any information
if (df->discreteKeys().size() > 0) {
result.push_back(df->restrict(discreteValues));
}
} else {
result.push_back(f); // Everything else is just added as is
}
}
return result;
}