HybridSmoother tests
Varun Agrawal
parent
fff828f599
commit
47f47fedc1
|
|
@ -115,103 +115,6 @@ TEST(HybridEstimation, Full) {
|
|||
EXPECT(assert_equal(expected_continuous, result));
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
// Test approximate inference with an additional pruning step.
|
||||
TEST(HybridEstimation, IncrementalSmoother) {
|
||||
using namespace estimation_fixture;
|
||||
|
||||
size_t K = 15;
|
||||
|
||||
// Switching example of robot moving in 1D
|
||||
// with given measurements and equal mode priors.
|
||||
HybridNonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
Switching switching = InitializeEstimationProblem(K, 1.0, 0.1, measurements,
|
||||
"1/1 1/1", graph, initial);
|
||||
HybridSmoother smoother;
|
||||
|
||||
HybridGaussianFactorGraph linearized;
|
||||
|
||||
constexpr size_t maxNrLeaves = 3;
|
||||
for (size_t k = 1; k < K; k++) {
|
||||
if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain
|
||||
graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model
|
||||
graph.push_back(switching.unaryFactors.at(k)); // Measurement
|
||||
|
||||
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
|
||||
|
||||
linearized = *graph.linearize(initial);
|
||||
Ordering ordering = smoother.getOrdering(linearized);
|
||||
|
||||
smoother.update(linearized, maxNrLeaves, ordering);
|
||||
graph.resize(0);
|
||||
}
|
||||
|
||||
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)] = 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), measurements[k]);
|
||||
}
|
||||
EXPECT(assert_equal(expected_continuous, result));
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
// Test if pruned factor is set to correct error and no errors are thrown.
|
||||
TEST(HybridEstimation, ValidPruningError) {
|
||||
using namespace estimation_fixture;
|
||||
|
||||
size_t K = 8;
|
||||
|
||||
HybridNonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
Switching switching = InitializeEstimationProblem(K, 1e-2, 1e-3, measurements,
|
||||
"1/1 1/1", graph, initial);
|
||||
HybridSmoother smoother;
|
||||
|
||||
HybridGaussianFactorGraph linearized;
|
||||
|
||||
constexpr size_t maxNrLeaves = 3;
|
||||
for (size_t k = 1; k < K; k++) {
|
||||
if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain
|
||||
graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model
|
||||
graph.push_back(switching.unaryFactors.at(k)); // Measurement
|
||||
|
||||
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
|
||||
|
||||
linearized = *graph.linearize(initial);
|
||||
Ordering ordering = smoother.getOrdering(linearized);
|
||||
|
||||
smoother.update(linearized, maxNrLeaves, ordering);
|
||||
|
||||
graph.resize(0);
|
||||
}
|
||||
|
||||
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)] = 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), measurements[k]);
|
||||
}
|
||||
EXPECT(assert_equal(expected_continuous, result));
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
// Test approximate inference with an additional pruning step.
|
||||
TEST(HybridEstimation, ISAM) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,177 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testHybridSmoother.cpp
|
||||
* @brief Unit tests for HybridSmoother
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearISAM.h>
|
||||
#include <gtsam/hybrid/HybridSmoother.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
// Include for test suite
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "Switching.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::Z;
|
||||
|
||||
namespace estimation_fixture {
|
||||
std::vector<double> measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6,
|
||||
7, 8, 9, 9, 9, 10, 11, 11, 11, 11};
|
||||
// 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 InitializeEstimationProblem(
|
||||
const size_t K, const double between_sigma, const double measurement_sigma,
|
||||
const std::vector<double>& measurements,
|
||||
const std::string& transitionProbabilityTable,
|
||||
HybridNonlinearFactorGraph* graph, Values* initial) {
|
||||
Switching switching(K, between_sigma, measurement_sigma, measurements,
|
||||
transitionProbabilityTable);
|
||||
|
||||
// Add prior on M(0)
|
||||
graph->push_back(switching.modeChain.at(0));
|
||||
|
||||
// Add the X(0) prior
|
||||
graph->push_back(switching.unaryFactors.at(0));
|
||||
initial->insert(X(0), switching.linearizationPoint.at<double>(X(0)));
|
||||
|
||||
return switching;
|
||||
}
|
||||
|
||||
} // namespace estimation_fixture
|
||||
|
||||
/****************************************************************************/
|
||||
// Test approximate inference with an additional pruning step.
|
||||
TEST(HybridSmoother, IncrementalSmoother) {
|
||||
using namespace estimation_fixture;
|
||||
|
||||
size_t K = 5;
|
||||
|
||||
// Switching example of robot moving in 1D
|
||||
// with given measurements and equal mode priors.
|
||||
HybridNonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
Switching switching = InitializeEstimationProblem(
|
||||
K, 1.0, 0.1, measurements, "1/1 1/1", &graph, &initial);
|
||||
|
||||
HybridSmoother smoother;
|
||||
constexpr size_t maxNrLeaves = 5;
|
||||
|
||||
// Loop over timesteps from 1...K-1
|
||||
for (size_t k = 1; k < K; k++) {
|
||||
if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain
|
||||
graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model
|
||||
graph.push_back(switching.unaryFactors.at(k)); // Measurement
|
||||
|
||||
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
|
||||
|
||||
HybridGaussianFactorGraph linearized = *graph.linearize(initial);
|
||||
Ordering ordering = smoother.getOrdering(linearized);
|
||||
|
||||
smoother.update(linearized, maxNrLeaves, ordering);
|
||||
|
||||
// Clear all the factors from the graph
|
||||
graph.resize(0);
|
||||
}
|
||||
|
||||
EXPECT_LONGS_EQUAL(11,
|
||||
smoother.hybridBayesNet().at(0)->asDiscrete()->nrValues());
|
||||
|
||||
// Get the continuous delta update as well as
|
||||
// the optimal discrete assignment.
|
||||
HybridValues delta = smoother.hybridBayesNet().optimize();
|
||||
|
||||
// Check discrete assignment
|
||||
DiscreteValues expected_discrete;
|
||||
for (size_t k = 0; k < K - 1; k++) {
|
||||
expected_discrete[M(k)] = discrete_seq[k];
|
||||
}
|
||||
EXPECT(assert_equal(expected_discrete, delta.discrete()));
|
||||
|
||||
// Update nonlinear solution and verify
|
||||
Values result = initial.retract(delta.continuous());
|
||||
Values expected_continuous;
|
||||
for (size_t k = 0; k < K; k++) {
|
||||
expected_continuous.insert(X(k), measurements[k]);
|
||||
}
|
||||
EXPECT(assert_equal(expected_continuous, result));
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
// Test if pruned Bayes net is set to correct error and no errors are thrown.
|
||||
TEST(HybridSmoother, ValidPruningError) {
|
||||
using namespace estimation_fixture;
|
||||
|
||||
size_t K = 8;
|
||||
|
||||
// Switching example of robot moving in 1D
|
||||
// with given measurements and equal mode priors.
|
||||
HybridNonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
Switching switching = InitializeEstimationProblem(
|
||||
K, 0.1, 0.1, measurements, "1/1 1/1", &graph, &initial);
|
||||
HybridSmoother smoother;
|
||||
|
||||
constexpr size_t maxNrLeaves = 3;
|
||||
for (size_t k = 1; k < K; k++) {
|
||||
if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain
|
||||
graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model
|
||||
graph.push_back(switching.unaryFactors.at(k)); // Measurement
|
||||
|
||||
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
|
||||
|
||||
HybridGaussianFactorGraph linearized = *graph.linearize(initial);
|
||||
Ordering ordering = smoother.getOrdering(linearized);
|
||||
|
||||
smoother.update(linearized, maxNrLeaves, ordering);
|
||||
|
||||
// Clear all the factors from the graph
|
||||
graph.resize(0);
|
||||
}
|
||||
|
||||
EXPECT_LONGS_EQUAL(14,
|
||||
smoother.hybridBayesNet().at(0)->asDiscrete()->nrValues());
|
||||
|
||||
// Get the continuous delta update as well as
|
||||
// the optimal discrete assignment.
|
||||
HybridValues delta = smoother.hybridBayesNet().optimize();
|
||||
|
||||
auto errorTree = smoother.hybridBayesNet().errorTree(delta.continuous());
|
||||
EXPECT_DOUBLES_EQUAL(0.0, errorTree(delta.discrete()), 1e-8);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue