Fourier prototype that unfortunately does not compile

release/4.3a0
dellaert 2014-11-23 23:16:04 +01:00
parent f40940dffe
commit e0f9f7a33e
1 changed files with 42 additions and 1 deletions

View File

@ -32,19 +32,60 @@ using namespace gtsam;
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
/// 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, boost::optional<Jacobian&> H) {
return H_ * c;
}
};
//******************************************************************************
TEST(BasisDecompositions, Fourier) {
// Create linear factor graph
GaussianFactorGraph g;
Key key(1);
Vector3_ c(key);
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;
g.add(key, A, b, model);
JacobianFactor f1(key, A, b, model);
// With ExpressionFactor
Expression<double> expression(Fourier<3>(x), c);
ExpressionFactor<double> f2(model, y, expression);
g.add(f1);
}
// Solve