HybridSmoother tests
Varun Agrawal
parent
fff828f599
commit
47f47fedc1
|
|
@ -115,103 +115,6 @@ TEST(HybridEstimation, Full) {
|
||||||
EXPECT(assert_equal(expected_continuous, result));
|
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 approximate inference with an additional pruning step.
|
||||||
TEST(HybridEstimation, ISAM) {
|
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