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

update InconsistentEliminationRequested to show leftover keys

release/4.3a0
Varun Agrawal 2023-07-15 16:53:17 -04:00
parent 4de2d46012
commit 64cd436362
3 changed files with 95 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -74,8 +74,9 @@ namespace gtsam {
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
const auto [bayesNet, factorGraph] = etree.eliminate(function);
// If any factors are remaining, the ordering was incomplete
if(!factorGraph->empty())
throw InconsistentEliminationRequested();
if(!factorGraph->empty()) {
throw InconsistentEliminationRequested(factorGraph->keys());
}
// Return the Bayes net
return bayesNet;
}
@ -136,8 +137,9 @@ namespace gtsam {
JunctionTreeType junctionTree(etree);
const auto [bayesTree, factorGraph] = junctionTree.eliminate(function);
// If any factors are remaining, the ordering was incomplete
if(!factorGraph->empty())
throw InconsistentEliminationRequested();
if(!factorGraph->empty()) {
throw InconsistentEliminationRequested(factorGraph->keys());
}
// Return the Bayes tree
return bayesTree;
}

63
gtsam/inference/inferenceExceptions.h
View File

@ -12,30 +12,59 @@
/**
* @file inferenceExceptions.h
* @brief Exceptions that may be thrown by inference algorithms
* @author Richard Roberts
* @author Richard Roberts, Varun Agrawal
* @date Apr 25, 2013
*/
#pragma once
#include <gtsam/global_includes.h>
#include <exception>
#include <sstream>
namespace gtsam {
/** An inference algorithm was called with inconsistent arguments. The factor graph, ordering, or
* variable index were inconsistent with each other, or a full elimination routine was called
* with an ordering that does not include all of the variables. */
class InconsistentEliminationRequested : public std::exception {
public:
InconsistentEliminationRequested() noexcept {}
~InconsistentEliminationRequested() noexcept override {}
const char* what() const noexcept override {
return
"An inference algorithm was called with inconsistent arguments. The\n"
"factor graph, ordering, or variable index were inconsistent with each\n"
"other, or a full elimination routine was called with an ordering that\n"
"does not include all of the variables.";
}
};
/** An inference algorithm was called with inconsistent arguments. The factor
* graph, ordering, or variable index were inconsistent with each other, or a
* full elimination routine was called with an ordering that does not include
* all of the variables. */
class InconsistentEliminationRequested : public std::exception {
KeySet keys_;
const KeyFormatter& keyFormatter = DefaultKeyFormatter;
}
public:
InconsistentEliminationRequested() noexcept {}
InconsistentEliminationRequested(
const KeySet& keys,
const KeyFormatter& key_formatter = DefaultKeyFormatter)
: keys_(keys), keyFormatter(key_formatter) {}
~InconsistentEliminationRequested() noexcept override {}
const char* what() const noexcept override {
// Format keys for printing
std::stringstream sstr;
for (auto key : keys_) {
sstr << keyFormatter(key) << ", ";
}
std::string keys = sstr.str();
// remove final comma and space.
keys.pop_back();
keys.pop_back();
static std::string msg =
"An inference algorithm was called with inconsistent "
"arguments. "
"The\n"
"factor graph, ordering, or variable index were "
"inconsistent with "
"each\n"
"other, or a full elimination routine was called with "
"an ordering "
"that\n"
"does not include all of the variables.\n";
msg += ("Leftover keys after elimination: " + keys);
return msg.c_str();
}
};
} // namespace gtsam

52
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -18,16 +18,16 @@
* @author Richard Roberts
**/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/base/debug.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
using namespace std;
using namespace gtsam;
@ -435,6 +435,40 @@ TEST(GaussianFactorGraph, ProbPrime) {
EXPECT_DOUBLES_EQUAL(expected, gfg.probPrime(values), 1e-12);
}
TEST(GaussianFactorGraph, InconsistentEliminationMessage) {
// Create empty graph
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
using gtsam::symbol_shorthand::X;
fg.emplace_shared<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2),
unit2);
fg.emplace_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
Vector2(2.0, -1.0), unit2);
fg.emplace_shared<JacobianFactor>(1, -5 * I_2x2, 2, 5 * I_2x2,
Vector2(-1.0, 1.5), unit2);
fg.emplace_shared<JacobianFactor>(2, -5 * I_2x2, X(3), 5 * I_2x2,
Vector2(-1.0, 1.5), unit2);
Ordering ordering{0, 1};
try {
fg.eliminateSequential(ordering);
} catch (const exception& exc) {
std::string expected_exception_message = "An inference algorithm was called with inconsistent "
"arguments. "
"The\n"
"factor graph, ordering, or variable index were "
"inconsistent with "
"each\n"
"other, or a full elimination routine was called with "
"an ordering "
"that\n"
"does not include all of the variables.\n"
"Leftover keys after elimination: 2, x3";
EXPECT(expected_exception_message == exc.what());
}
}
/* ************************************************************************* */
int main() {
TestResult tr;