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

Merge branch 'develop' into hybrid/model-selection

release/4.3a0
Varun Agrawal 2022-12-30 13:49:32 +05:30
parent ef83628efe 6466b03b5c
commit 3ab91b27d5
3 changed files with 4 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -249,7 +249,7 @@ HybridValues HybridBayesNet::optimize() const {
DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize();
// Given the MPE, compute the optimal continuous values.
return HybridValues(optimize(mpe)), mpe);
return HybridValues(optimize(mpe), mpe);
}
/* ************************************************************************* */

23
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -540,27 +540,4 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::probPrime(
return prob_tree;
}
/* ************************************************************************ */
std::pair<Ordering, Ordering>
HybridGaussianFactorGraph::separateContinuousDiscreteOrdering(
const Ordering &ordering) const {
KeySet all_continuous_keys = this->continuousKeys();
KeySet all_discrete_keys = this->discreteKeys();
Ordering continuous_ordering, discrete_ordering;
for (auto &&key : ordering) {
if (std::find(all_continuous_keys.begin(), all_continuous_keys.end(),
key) != all_continuous_keys.end()) {
continuous_ordering.push_back(key);
} else if (std::find(all_discrete_keys.begin(), all_discrete_keys.end(),
key) != all_discrete_keys.end()) {
discrete_ordering.push_back(key);
} else {
throw std::runtime_error("Key in ordering not present in factors.");
}
}
return std::make_pair(continuous_ordering, discrete_ordering);
}
} // namespace gtsam

72
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -291,24 +291,6 @@ TEST(HybridEstimation, Probability) {
std::vector<double> measurements = {0, 1, 2, 2};
double between_sigma = 1.0, measurement_sigma = 0.1;
std::vector<double> expected_errors, expected_prob_primes;
std::map<size_t, std::vector<size_t>> discrete_seq_map;
for (size_t i = 0; i < pow(2, K - 1); i++) {
discrete_seq_map[i] = getDiscreteSequence<K>(i);
GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph(
K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma);
auto bayes_net = linear_graph->eliminateSequential();
VectorValues values = bayes_net->optimize();
double error = linear_graph->error(values);
expected_errors.push_back(error);
double prob_prime = linear_graph->probPrime(values);
expected_prob_primes.push_back(prob_prime);
}
// Switching example of robot moving in 1D with
// given measurements and equal mode priors.
Switching switching(K, between_sigma, measurement_sigma, measurements,
@ -332,18 +314,6 @@ TEST(HybridEstimation, Probability) {
}
auto discreteConditional = discreteBayesNet->atDiscrete(0);
// Test if the probPrimeTree matches the probability of
// the individual factor graphs
for (size_t i = 0; i < pow(2, K - 1); i++) {
DiscreteValues discrete_assignment;
for (size_t v = 0; v < discrete_seq_map[i].size(); v++) {
discrete_assignment[M(v)] = discrete_seq_map[i][v];
}
double discrete_transition_prob = 0.25;
EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i) * discrete_transition_prob,
(*discreteConditional)(discrete_assignment), 1e-8);
}
HybridValues hybrid_values = bayesNet->optimize();
// This is the correct sequence as designed
@ -367,24 +337,6 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
double between_sigma = 1.0, measurement_sigma = 0.1;
// For each discrete mode sequence, create the individual factor graphs and
// optimize each.
std::vector<double> expected_errors, expected_prob_primes;
std::map<size_t, std::vector<size_t>> discrete_seq_map;
for (size_t i = 0; i < pow(2, K - 1); i++) {
discrete_seq_map[i] = getDiscreteSequence<K>(i);
GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph(
K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma);
auto bayes_tree = linear_graph->eliminateMultifrontal();
VectorValues values = bayes_tree->optimize();
expected_errors.push_back(linear_graph->error(values));
expected_prob_primes.push_back(linear_graph->probPrime(values));
}
// Switching example of robot moving in 1D with given measurements and equal
// mode priors.
Switching switching(K, between_sigma, measurement_sigma, measurements,
@ -408,23 +360,6 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
auto last_conditional = (*bayesTree)[last_continuous_key]->conditional();
DiscreteKeys discrete_keys = last_conditional->discreteKeys();
// Create a decision tree of all the different VectorValues
AlgebraicDecisionTree<Key> probPrimeTree =
graph.continuousProbPrimes(discrete_keys, bayesTree);
EXPECT(assert_equal(expected_probPrimeTree, probPrimeTree));
// Test if the probPrimeTree matches the probability of
// the individual factor graphs
for (size_t i = 0; i < pow(2, K - 1); i++) {
Assignment<Key> discrete_assignment;
for (size_t v = 0; v < discrete_seq_map[i].size(); v++) {
discrete_assignment[M(v)] = discrete_seq_map[i][v];
}
EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i),
probPrimeTree(discrete_assignment), 1e-8);
}
Ordering discrete(graph.discreteKeys());
auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete);
@ -515,10 +450,9 @@ TEST(HybridEstimation, eliminateSequentialRegression) {
HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering);
// GTSAM_PRINT(*bn);
// TODO(dellaert): dc should be discrete conditional on m0, but it is an unnormalized factor?
// DiscreteKey m(M(0), 2);
// DiscreteConditional expected(m % "0.51341712/1");
// auto dc = bn->back()->asDiscreteConditional();
// TODO(dellaert): dc should be discrete conditional on m0, but it is an
// unnormalized factor? DiscreteKey m(M(0), 2); DiscreteConditional expected(m
// % "0.51341712/1"); auto dc = bn->back()->asDiscreteConditional();
// EXPECT(assert_equal(expected, *dc, 1e-9));
}