remove extraneous file
Varun Agrawal
parent
3f201f3f4b
commit
ec5149265a
|
|
@ -1,158 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testHybridPruning.cpp
|
||||
* @brief Unit tests for end-to-end Hybrid Estimation
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/discrete/DiscreteBayesNet.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearISAM.h>
|
||||
#include <gtsam/hybrid/HybridSmoother.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.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 "Switching.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
using symbol_shorthand::X;
|
||||
|
||||
/****************************************************************************/
|
||||
// Test approximate inference with an additional pruning step.
|
||||
TEST_DISABLED(HybridPruning, ISAM) {
|
||||
size_t K = 16;
|
||||
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 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");
|
||||
HybridNonlinearISAM isam;
|
||||
HybridNonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
|
||||
// Add the X(0) prior
|
||||
graph.push_back(switching.nonlinearFactorGraph.at(0));
|
||||
initial.insert(X(0), switching.linearizationPoint.at<double>(X(0)));
|
||||
|
||||
HybridGaussianFactorGraph linearized;
|
||||
HybridGaussianFactorGraph bayesNet;
|
||||
|
||||
for (size_t k = 1; k < K; k++) {
|
||||
// Motion Model
|
||||
graph.push_back(switching.nonlinearFactorGraph.at(k));
|
||||
// Measurement
|
||||
graph.push_back(switching.nonlinearFactorGraph.at(k + K - 1));
|
||||
|
||||
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
|
||||
|
||||
isam.update(graph, initial, 3);
|
||||
graph.resize(0);
|
||||
initial.clear();
|
||||
}
|
||||
|
||||
Values result = isam.estimate();
|
||||
DiscreteValues assignment = isam.assignment();
|
||||
|
||||
DiscreteValues expected_discrete;
|
||||
for (size_t k = 0; k < K - 1; k++) {
|
||||
expected_discrete[M(k)] = discrete_seq[k];
|
||||
}
|
||||
|
||||
std::cout << "\n\n\nNonlinear Version!!\n\n" << std::endl;
|
||||
GTSAM_PRINT(expected_discrete);
|
||||
GTSAM_PRINT(assignment);
|
||||
EXPECT(assert_equal(expected_discrete, assignment));
|
||||
|
||||
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(HybridPruning, GaussianISAM) {
|
||||
size_t K = 16;
|
||||
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 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");
|
||||
HybridGaussianISAM isam;
|
||||
HybridGaussianFactorGraph graph;
|
||||
Values initial;
|
||||
|
||||
// Add the X(0) prior
|
||||
graph.push_back(switching.linearizedFactorGraph.at(0));
|
||||
initial.insert(X(0), switching.linearizationPoint.at<double>(X(0)));
|
||||
|
||||
HybridGaussianFactorGraph linearized;
|
||||
HybridGaussianFactorGraph bayesNet;
|
||||
|
||||
for (size_t k = 1; k < K; k++) {
|
||||
// Motion Model
|
||||
graph.push_back(switching.linearizedFactorGraph.at(k));
|
||||
// Measurement
|
||||
graph.push_back(switching.linearizedFactorGraph.at(k + K - 1));
|
||||
|
||||
// initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
|
||||
|
||||
isam.update(graph, 3);
|
||||
graph.resize(0);
|
||||
// initial.clear();
|
||||
}
|
||||
|
||||
HybridValues values = isam.optimize();
|
||||
|
||||
DiscreteValues expected_discrete;
|
||||
for (size_t k = 0; k < K - 1; k++) {
|
||||
expected_discrete[M(k)] = discrete_seq[k];
|
||||
}
|
||||
|
||||
EXPECT(assert_equal(expected_discrete, values.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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue