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

showing difference in computed probabilities

release/4.3a0
Varun Agrawal 2022-10-25 14:02:36 -04:00
parent dcdcf30f52
commit 1789bb74fe
2 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/hybrid/tests/Switching.h
View File

@ -130,6 +130,7 @@ struct Switching {
* @param K The total number of timesteps.
* @param between_sigma The stddev between poses.
* @param prior_sigma The stddev on priors (also used for measurements).
* @param measurements Vector of measurements for each timestep.
*/
Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1,
std::vector<double> measurements = {})

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

@ -201,10 +201,14 @@ TEST(HybridEstimation, Probability) {
VectorValues values = bayes_net->optimize();
std::cout << i << " : " << linear_graph->probPrime(values) << std::endl;
}
// std::cout << linear_graph->error(values) << std::endl;
// // values.at();
// // linearizationPoint.retract(values).print();
Switching switching(K, between_sigma, measurement_sigma, measurements);
auto graph = switching.linearizedFactorGraph;
Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph());
HybridBayesNet::shared_ptr bayesNet = graph.eliminateSequential(ordering);
const DecisionTreeFactor::shared_ptr decisionTree =
bayesNet->discreteConditionals();
decisionTree->print();
}
/* ************************************************************************* */