Moved to project
dellaert
parent
4fd2a288a2
commit
02075b7575
|
|
@ -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 <gtsam/base/Matrix.h>
|
|
||||||
|
|
||||||
namespace gtsam {
|
|
||||||
|
|
||||||
/// Fourier
|
|
||||||
template<int N>
|
|
||||||
class Fourier {
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
typedef Eigen::Matrix<double, N, 1> Coefficients;
|
|
||||||
typedef Eigen::Matrix<double, 1, N> 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 <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
||||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
||||||
#include <gtsam/linear/VectorValues.h>
|
|
||||||
|
|
||||||
namespace gtsam {
|
|
||||||
|
|
||||||
/// For now, this is our sequence representation
|
|
||||||
typedef std::map<double, double> 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<Coefficients> c(0);
|
|
||||||
typedef std::pair<double, double> Sample;
|
|
||||||
BOOST_FOREACH(Sample sample, sequence) {
|
|
||||||
Expression<double> expression(Fourier<3>(sample.first), c);
|
|
||||||
ExpressionFactor<double> 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<Coefficients>(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 <gtsam_unstable/nonlinear/expressionTesting.h>
|
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
using namespace gtsam;
|
|
||||||
|
|
||||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
|
|
||||||
|
|
||||||
//******************************************************************************
|
|
||||||
TEST(BasisDecompositions, Fourier) {
|
|
||||||
Fourier<3> fx(0);
|
|
||||||
Eigen::Matrix<double, 1, 3> expectedH, actualH;
|
|
||||||
Vector3 c(1.5661, 1.2717, 1.2717);
|
|
||||||
expectedH = numericalDerivative11<double, Vector3>(
|
|
||||||
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<Vector3> c(key);
|
|
||||||
Values values;
|
|
||||||
values.insert<Vector3>(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<double> expression(Fourier<3>(x), c);
|
|
||||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9);
|
|
||||||
{
|
|
||||||
ExpressionFactor<double> f2(model, y, expression);
|
|
||||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
|
||||||
boost::shared_ptr<JacobianFactor> jf = //
|
|
||||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
|
||||||
CHECK(jf);
|
|
||||||
EXPECT( assert_equal(f1, *jf, 1e-9));
|
|
||||||
}
|
|
||||||
{
|
|
||||||
ExpressionFactor<double> f2(model, y, expression);
|
|
||||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
|
||||||
boost::shared_ptr<JacobianFactor> jf = //
|
|
||||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
|
||||||
CHECK(jf);
|
|
||||||
EXPECT( assert_equal(f1, *jf, 1e-9));
|
|
||||||
}
|
|
||||||
{
|
|
||||||
ExpressionFactor<double> f2(model, y, expression);
|
|
||||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
|
||||||
boost::shared_ptr<JacobianFactor> jf = //
|
|
||||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
|
||||||
CHECK(jf);
|
|
||||||
EXPECT( assert_equal(f1, *jf, 1e-9));
|
|
||||||
}
|
|
||||||
{
|
|
||||||
ExpressionFactor<double> f2(model, y, expression);
|
|
||||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
|
||||||
boost::shared_ptr<JacobianFactor> jf = //
|
|
||||||
boost::dynamic_pointer_cast<JacobianFactor>(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);
|
|
||||||
}
|
|
||||||
//******************************************************************************
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue