From 02075b75755400f0f597ba1ee2cc8d4e61e5de41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 21:08:31 +0100 Subject: [PATCH] Moved to project --- .../tests/testBasisDecompositions.cpp | 231 ------------------ 1 file changed, 231 deletions(-) delete mode 100644 gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp deleted file mode 100644 index c3bf83f71..000000000 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ /dev/null @@ -1,231 +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 testBasisDecompositions.cpp - * @date November 23, 2014 - * @author Frank Dellaert - * @brief unit tests for Basis Decompositions w Expressions - */ - -#include - -namespace gtsam { - -/// Fourier -template -class Fourier { - -public: - - typedef Eigen::Matrix Coefficients; - typedef Eigen::Matrix Jacobian; - -private: - - double x_; - Jacobian H_; - -public: - - /// Constructor - Fourier(double x) : - x_(x) { - H_(0, 0) = 1; - for (size_t i = 1; i < N; i += 2) { - H_(0, i) = cos(i * x); - H_(0, i + 1) = sin(i * x); - } - } - - /// Given coefficients c, predict value for x - double operator()(const Coefficients& c, OptionalJacobian<1, N> H) { - if (H) - (*H) = H_; - return H_ * c; - } -}; - -} - -#include -#include -#include -#include - -namespace gtsam { - -/// For now, this is our sequence representation -typedef std::map Sequence; - -/** - * Class that does Fourier Decomposition via least squares - */ -class FourierDecomposition { -public: - - typedef Vector3 Coefficients; ///< Fourier coefficients - -private: - Coefficients c_; - -public: - - /// Create nonlinear FG from Sequence - static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph; - Expression c(0); - typedef std::pair Sample; - BOOST_FOREACH(Sample sample, sequence) { - Expression expression(Fourier<3>(sample.first), c); - ExpressionFactor factor(model, sample.second, expression); - graph.add(factor); - } - return graph; - } - - /// Create linear FG from Sequence - static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph = NonlinearGraph(sequence, model); - Values values; - values.insert(0, Coefficients::Zero()); // does not matter - GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); - return gfg; - } - - /// Constructor - FourierDecomposition(const Sequence& sequence, - const SharedNoiseModel& model) { - GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model); - VectorValues solution = gfg->optimize(); - c_ = solution.at(0); - } - - /// Return Fourier coefficients - Coefficients coefficients() { - return c_; - } -}; - -} - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - -//****************************************************************************** -TEST(BasisDecompositions, Fourier) { - Fourier<3> fx(0); - Eigen::Matrix expectedH, actualH; - Vector3 c(1.5661, 1.2717, 1.2717); - expectedH = numericalDerivative11( - boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c); - EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9); - EXPECT(assert_equal((Matrix)expectedH, actualH)); -} - -//****************************************************************************** -TEST(BasisDecompositions, ManualFourier) { - - // Create linear factor graph - GaussianFactorGraph g; - Key key(1); - Expression c(key); - Values values; - values.insert(key, Vector3::Zero()); // does not matter - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - - // Manual JacobianFactor - Matrix A(1, 3); - A << 1, cos(x), sin(x); - Vector b(1); - b << y; - JacobianFactor f1(key, A, b); - g.add(f1); - - // With ExpressionFactor - Expression expression(Fourier<3>(x), c); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9); - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - } - - // Solve - VectorValues actual = g.optimize(); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); -} - -//****************************************************************************** -TEST(BasisDecompositions, FourierDecomposition) { - - // Create example sequence - Sequence sequence; - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - sequence[x] = y; - } - - // Do Fourier Decomposition - FourierDecomposition actual(sequence, model); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4)); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** -