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

add assertions for HybridEstimation example

release/4.3a0
Varun Agrawal 2022-10-22 18:29:28 -04:00
parent 58a15c3385
commit 74d6f0548d
1 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -82,8 +82,6 @@ TEST(HybridNonlinearISAM, Incremental) {
HybridNonlinearFactorGraph graph; HybridNonlinearFactorGraph graph;
Values initial; Values initial;
// switching.nonlinearFactorGraph.print();
// switching.linearizationPoint.print();
// Add the X(1) prior // Add the X(1) prior
graph.push_back(switching.nonlinearFactorGraph.at(0)); graph.push_back(switching.nonlinearFactorGraph.at(0));
initial.insert(X(1), switching.linearizationPoint.at<double>(X(1))); initial.insert(X(1), switching.linearizationPoint.at<double>(X(1)));
@ -92,6 +90,7 @@ TEST(HybridNonlinearISAM, Incremental) {
HybridGaussianFactorGraph bayesNet; HybridGaussianFactorGraph bayesNet;
for (size_t k = 1; k < K; k++) { for (size_t k = 1; k < K; k++) {
std::cout << ">>>>>>>>>>>>>>>>>>> k=" << k << std::endl;
// Motion Model // Motion Model
graph.push_back(switching.nonlinearFactorGraph.at(k)); graph.push_back(switching.nonlinearFactorGraph.at(k));
// Measurement // Measurement
@ -99,20 +98,28 @@ TEST(HybridNonlinearISAM, Incremental) {
initial.insert(X(k + 1), switching.linearizationPoint.at<double>(X(k + 1))); initial.insert(X(k + 1), switching.linearizationPoint.at<double>(X(k + 1)));
// std::cout << "\n============= " << k << std::endl;
// graph.print();
bayesNet = smoother.hybridBayesNet(); bayesNet = smoother.hybridBayesNet();
linearized = *graph.linearize(initial); linearized = *graph.linearize(initial);
Ordering ordering = getOrdering(bayesNet, linearized); Ordering ordering = getOrdering(bayesNet, linearized);
ordering.print();
smoother.update(linearized, ordering, 3); smoother.update(linearized, ordering, 3);
// if (k == 2) exit(0);
// smoother.hybridBayesNet().print();
graph.resize(0); graph.resize(0);
// initial.clear();
} }
HybridValues delta = smoother.hybridBayesNet().optimize();
Values result = initial.retract(delta.continuous());
DiscreteValues expected_discrete;
for (size_t k = 0; k < K - 1; k++) {
expected_discrete[M(k + 1)] = discrete_seq[k];
}
EXPECT(assert_equal(expected_discrete, delta.discrete()));
Values expected_continuous;
for (size_t k = 0; k < K; k++) {
expected_continuous.insert(X(k + 1), measurements[k]);
}
EXPECT(assert_equal(expected_continuous, result));
} }
/* ************************************************************************* */ /* ************************************************************************* */