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

minor fixes

release/4.3a0
Varun Agrawal 2022-12-30 10:47:06 +05:30
parent cf46c36862
commit 28f349c57d
3 changed files with 6 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -108,7 +108,6 @@ void HybridBayesNet::updateDiscreteConditionals(
for (size_t i = 0; i < this->size(); i++) {
HybridConditional::shared_ptr conditional = this->at(i);
if (conditional->isDiscrete()) {
// std::cout << demangle(typeid(conditional).name()) << std::endl;
auto discrete = conditional->asDiscrete();
KeyVector frontals(discrete->frontals().begin(),
discrete->frontals().end());
@ -218,8 +217,7 @@ HybridValues HybridBayesNet::optimize() const {
DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize();
// Given the MPE, compute the optimal continuous values.
GaussianBayesNet gbn = choose(mpe);
return HybridValues(gbn.optimize(), mpe);
return HybridValues(optimize(mpe), mpe);
}
/* ************************************************************************* */

23
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -539,27 +539,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

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

@ -269,26 +269,8 @@ 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 example of robot moving in 1D with
// given measurements and equal mode priors.
Switching switching(K, between_sigma, measurement_sigma, measurements,
"1/1 1/1");
auto graph = switching.linearizedFactorGraph;
@ -297,18 +279,6 @@ TEST(HybridEstimation, Probability) {
HybridBayesNet::shared_ptr bayesNet = graph.eliminateSequential(ordering);
auto discreteConditional = bayesNet->atDiscrete(bayesNet->size() - 3);
// 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
@ -332,24 +302,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,
@ -373,25 +325,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);
}
discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
Ordering discrete(graph.discreteKeys());
auto discreteBayesTree =
discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete);
@ -483,10 +416,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));
}