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

first restrict before checking numer of keys

release/4.3a0
Varun Agrawal 2025-04-11 16:46:11 -04:00
parent 4a7cf4cc40
commit d492068704
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/* ************************************************************************* */
@ -237,10 +236,11 @@ 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)) {
// 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));
auto restricted_df = df->restrict(discreteValues);
// In the case where all the discrete values have been selected,
// we ignore the factor since it doesn't add any information
if (restricted_df->discreteKeys().size() > 0) {
result.push_back(restricted_df);
}
} else {
result.push_back(f); // Everything else is just added as is