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

address review comments

release/4.3a0
Varun Agrawal 2022-11-15 00:50:03 -05:00
parent 5e2cdfdd3b
commit 239412956c
6 changed files with 107 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -557,9 +557,9 @@ HybridGaussianFactorGraph::eliminateHybridSequential(
const boost::optional<Ordering> continuous,
const boost::optional<Ordering> discrete, const Eliminate &function,
OptionalVariableIndex variableIndex) const {
Ordering continuous_ordering =
const Ordering continuous_ordering =
continuous ? *continuous : Ordering(this->continuousKeys());
Ordering discrete_ordering =
const Ordering discrete_ordering =
discrete ? *discrete : Ordering(this->discreteKeys());
// Eliminate continuous
@ -570,7 +570,8 @@ HybridGaussianFactorGraph::eliminateHybridSequential(
function, variableIndex);
// Get the last continuous conditional which will have all the discrete keys
auto last_conditional = bayesNet->at(bayesNet->size() - 1);
HybridConditional::shared_ptr last_conditional =
bayesNet->at(bayesNet->size() - 1);
DiscreteKeys discrete_keys = last_conditional->discreteKeys();
// If not discrete variables, return the eliminated bayes net.
@ -578,9 +579,11 @@ HybridGaussianFactorGraph::eliminateHybridSequential(
return bayesNet;
}
AlgebraicDecisionTree<Key> probPrimeTree =
// DecisionTree for P'(X|M, Z) for all mode sequences M
const AlgebraicDecisionTree<Key> probPrimeTree =
this->continuousProbPrimes(discrete_keys, bayesNet);
// Add the model selection factor P(M|Z)
discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
// Perform discrete elimination
@ -622,9 +625,9 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal(
const boost::optional<Ordering> continuous,
const boost::optional<Ordering> discrete, const Eliminate &function,
OptionalVariableIndex variableIndex) const {
Ordering continuous_ordering =
const Ordering continuous_ordering =
continuous ? *continuous : Ordering(this->continuousKeys());
Ordering discrete_ordering =
const Ordering discrete_ordering =
discrete ? *discrete : Ordering(this->discreteKeys());
// Eliminate continuous
@ -635,9 +638,9 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal(
function, variableIndex);
// Get the last continuous conditional which will have all the discrete
Key last_continuous_key =
continuous_ordering.at(continuous_ordering.size() - 1);
auto last_conditional = (*bayesTree)[last_continuous_key]->conditional();
const Key last_continuous_key = continuous_ordering.back();
HybridConditional::shared_ptr last_conditional =
(*bayesTree)[last_continuous_key]->conditional();
DiscreteKeys discrete_keys = last_conditional->discreteKeys();
// If not discrete variables, return the eliminated bayes net.
@ -645,16 +648,24 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal(
return bayesTree;
}
AlgebraicDecisionTree<Key> probPrimeTree =
// DecisionTree for P'(X|M, Z) for all mode sequences M
const AlgebraicDecisionTree<Key> probPrimeTree =
this->continuousProbPrimes(discrete_keys, bayesTree);
// Add the model selection factor P(M|Z)
discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
auto updatedBayesTree =
// Eliminate discrete variables to get the discrete bayes tree.
// This bayes tree will be updated with the
// continuous variables as the child nodes.
HybridBayesTree::shared_ptr updatedBayesTree =
discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete_ordering,
function);
auto discrete_clique = (*updatedBayesTree)[discrete_ordering.at(0)];
// Get the clique with all the discrete keys.
// There should only be 1 clique.
const HybridBayesTree::sharedClique discrete_clique =
(*updatedBayesTree)[discrete_ordering.at(0)];
std::set<HybridBayesTreeClique::shared_ptr> clique_set;
for (auto node : bayesTree->nodes()) {

6
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -217,8 +217,10 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
const DiscreteValues& discreteValues) const;
/**
* @brief Compute the VectorValues solution for the continuous variables for
* each mode.
* @brief Helper method to compute the VectorValues solution for the
* continuous variables for each discrete mode.
* Used as a helper to compute q(\mu | M, Z) which is used by
* both P(X | M, Z) and P(M | Z).
*
* @tparam BAYES Template on the type of Bayes graph, either a bayes net or a
* bayes tree.

1
gtsam/hybrid/tests/testHybridBayesTree.cpp
View File

@ -141,7 +141,6 @@ TEST(HybridBayesTree, Optimize) {
DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}};
vector<double> probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656,
0.037152205, 0.12248971, 0.07349729, 0.08};
AlgebraicDecisionTree<Key> potentials(discrete_keys, probs);
dfg.emplace_shared<DecisionTreeFactor>(discrete_keys, probs);
DiscreteValues expectedMPE = dfg.optimize();

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

@ -79,6 +79,8 @@ TEST(HybridEstimation, Incremental) {
// Ground truth discrete seq
std::vector<size_t> discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0,
1, 1, 1, 0, 0, 1, 1, 0, 0, 0};
// Switching example of robot moving in 1D with given measurements and equal
// mode priors.
Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1");
HybridSmoother smoother;
HybridNonlinearFactorGraph graph;
@ -136,7 +138,7 @@ TEST(HybridEstimation, Incremental) {
* @param between_sigma Noise model sigma for the between factor.
* @return GaussianFactorGraph::shared_ptr
*/
GaussianFactorGraph::shared_ptr specificProblem(
GaussianFactorGraph::shared_ptr specificModesFactorGraph(
size_t K, const std::vector<double>& measurements,
const std::vector<size_t>& discrete_seq, double measurement_sigma = 0.1,
double between_sigma = 1.0) {
@ -184,7 +186,7 @@ std::vector<size_t> getDiscreteSequence(size_t x) {
}
/**
* @brief Helper method to get the probPrimeTree
* @brief Helper method to get the tree of unnormalized probabilities
* as per the new elimination scheme.
*
* @param graph The HybridGaussianFactorGraph to eliminate.
@ -242,18 +244,15 @@ AlgebraicDecisionTree<Key> probPrimeTree(
TEST(HybridEstimation, Probability) {
constexpr size_t K = 4;
std::vector<double> measurements = {0, 1, 2, 2};
// This is the correct sequence
// std::vector<size_t> discrete_seq = {1, 1, 0};
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++) {
std::vector<size_t> discrete_seq = getDiscreteSequence<K>(i);
discrete_seq_map[i] = getDiscreteSequence<K>(i);
GaussianFactorGraph::shared_ptr linear_graph = specificProblem(
K, measurements, discrete_seq, measurement_sigma, between_sigma);
GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph(
K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma);
auto bayes_net = linear_graph->eliminateSequential();
@ -263,7 +262,10 @@ TEST(HybridEstimation, Probability) {
expected_prob_primes.push_back(linear_graph->probPrime(values));
}
Switching switching(K, between_sigma, measurement_sigma, measurements);
// 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;
Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph());
@ -298,26 +300,30 @@ TEST(HybridEstimation, Probability) {
// Test if the probPrimeTree matches the probability of
// the individual factor graphs
for (size_t i = 0; i < pow(2, K - 1); i++) {
std::vector<size_t> discrete_seq = getDiscreteSequence<K>(i);
Assignment<Key> discrete_assignment;
for (size_t v = 0; v < discrete_seq.size(); v++) {
discrete_assignment[M(v)] = discrete_seq[v];
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);
}
// remainingGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
// Ordering discrete(graph.discreteKeys());
// // remainingGraph->print("remainingGraph");
// // discrete.print();
// auto discreteBayesNet = remainingGraph->eliminateSequential(discrete);
// bayesNet->add(*discreteBayesNet);
// // bayesNet->print();
Ordering discrete(graph.discreteKeys());
auto discreteBayesNet =
discreteGraph->BaseEliminateable::eliminateSequential(discrete);
bayesNet->add(*discreteBayesNet);
// HybridValues hybrid_values = bayesNet->optimize();
// hybrid_values.discrete().print();
HybridValues hybrid_values = bayesNet->optimize();
// This is the correct sequence as designed
DiscreteValues discrete_seq;
discrete_seq[M(0)] = 1;
discrete_seq[M(1)] = 1;
discrete_seq[M(2)] = 0;
EXPECT(assert_equal(discrete_seq, hybrid_values.discrete()));
}
/****************************************************************************/
@ -330,31 +336,34 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
constexpr size_t K = 4;
std::vector<double> measurements = {0, 1, 2, 2};
// This is the correct sequence
// std::vector<size_t> discrete_seq = {1, 1, 0};
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++) {
std::vector<size_t> discrete_seq = getDiscreteSequence<K>(i);
discrete_seq_map[i] = getDiscreteSequence<K>(i);
GaussianFactorGraph::shared_ptr linear_graph = specificProblem(
K, measurements, discrete_seq, measurement_sigma, between_sigma);
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();
std::cout << i << " " << linear_graph->error(values) << std::endl;
expected_errors.push_back(linear_graph->error(values));
expected_prob_primes.push_back(linear_graph->probPrime(values));
}
Switching switching(K, between_sigma, measurement_sigma, measurements);
// 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;
Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph());
// Get the tree of unnormalized probabilities for each mode sequence.
AlgebraicDecisionTree<Key> expected_probPrimeTree = probPrimeTree(graph);
// Eliminate continuous
@ -379,10 +388,9 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
// Test if the probPrimeTree matches the probability of
// the individual factor graphs
for (size_t i = 0; i < pow(2, K - 1); i++) {
std::vector<size_t> discrete_seq = getDiscreteSequence<K>(i);
Assignment<Key> discrete_assignment;
for (size_t v = 0; v < discrete_seq.size(); v++) {
discrete_assignment[M(v)] = discrete_seq[v];
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);
@ -390,13 +398,44 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree));
// Ordering discrete(graph.discreteKeys());
// auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete);
// // DiscreteBayesTree should have only 1 clique
// bayesTree->addClique((*discreteBayesTree)[discrete.at(0)]);
Ordering discrete(graph.discreteKeys());
auto discreteBayesTree =
discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete);
// // HybridValues hybrid_values = bayesNet->optimize();
// // hybrid_values.discrete().print();
EXPECT_LONGS_EQUAL(1, discreteBayesTree->size());
// DiscreteBayesTree should have only 1 clique
auto discrete_clique = (*discreteBayesTree)[discrete.at(0)];
std::set<HybridBayesTreeClique::shared_ptr> clique_set;
for (auto node : bayesTree->nodes()) {
clique_set.insert(node.second);
}
// Set the root of the bayes tree as the discrete clique
for (auto clique : clique_set) {
if (clique->conditional()->parents() ==
discrete_clique->conditional()->frontals()) {
discreteBayesTree->addClique(clique, discrete_clique);
} else {
// Remove the clique from the children of the parents since it will get
// added again in addClique.
auto clique_it = std::find(clique->parent()->children.begin(),
clique->parent()->children.end(), clique);
clique->parent()->children.erase(clique_it);
discreteBayesTree->addClique(clique, clique->parent());
}
}
HybridValues hybrid_values = discreteBayesTree->optimize();
// This is the correct sequence as designed
DiscreteValues discrete_seq;
discrete_seq[M(0)] = 1;
discrete_seq[M(1)] = 1;
discrete_seq[M(2)] = 0;
EXPECT(assert_equal(discrete_seq, hybrid_values.discrete()));
}
/* ************************************************************************* */

2
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -176,7 +176,7 @@ TEST(HybridGaussianElimination, IncrementalInference) {
auto discreteConditional = isam[M(1)]->conditional()->asDiscreteConditional();
// Test if the probability values are as expected with regression tests.
// Test the probability values with regression tests.
DiscreteValues assignment;
EXPECT(assert_equal(0.166667, m00_prob, 1e-5));
assignment[M(0)] = 0;

2
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -195,7 +195,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
auto discreteConditional =
bayesTree[M(1)]->conditional()->asDiscreteConditional();
// Test if the probability values are as expected with regression tests.
// Test the probability values with regression tests.
DiscreteValues assignment;
EXPECT(assert_equal(0.166667, m00_prob, 1e-5));
assignment[M(0)] = 0;