1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
5// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
6//
7// This Source Code Form is subject to the terms of the Mozilla
8// Public License v. 2.0. If a copy of the MPL was not distributed
9// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10#include <iostream>
11#include <fstream>
12#include <iomanip>
13
14#include "main.h"
15#include <Eigen/LevenbergMarquardt>
16
17using namespace std;
18using namespace Eigen;
19
20template <typename Scalar>
21struct sparseGaussianTest : SparseFunctor<Scalar, int>
22{
23  typedef Matrix<Scalar,Dynamic,1> VectorType;
24  typedef SparseFunctor<Scalar,int> Base;
25  typedef typename Base::JacobianType JacobianType;
26  sparseGaussianTest(int inputs, int values) : SparseFunctor<Scalar,int>(inputs,values)
27  { }
28
29  VectorType model(const VectorType& uv, VectorType& x)
30  {
31    VectorType y; //Change this to use expression template
32    int m = Base::values();
33    int n = Base::inputs();
34    eigen_assert(uv.size()%2 == 0);
35    eigen_assert(uv.size() == n);
36    eigen_assert(x.size() == m);
37    y.setZero(m);
38    int half = n/2;
39    VectorBlock<const VectorType> u(uv, 0, half);
40    VectorBlock<const VectorType> v(uv, half, half);
41    Scalar coeff;
42    for (int j = 0; j < m; j++)
43    {
44      for (int i = 0; i < half; i++)
45      {
46        coeff = (x(j)-i)/v(i);
47        coeff *= coeff;
48        if (coeff < 1. && coeff > 0.)
49          y(j) += u(i)*std::pow((1-coeff), 2);
50      }
51    }
52    return y;
53  }
54  void initPoints(VectorType& uv_ref, VectorType& x)
55  {
56    m_x = x;
57    m_y = this->model(uv_ref,x);
58  }
59  int operator()(const VectorType& uv, VectorType& fvec)
60  {
61    int m = Base::values();
62    int n = Base::inputs();
63    eigen_assert(uv.size()%2 == 0);
64    eigen_assert(uv.size() == n);
65    int half = n/2;
66    VectorBlock<const VectorType> u(uv, 0, half);
67    VectorBlock<const VectorType> v(uv, half, half);
68    fvec = m_y;
69    Scalar coeff;
70    for (int j = 0; j < m; j++)
71    {
72      for (int i = 0; i < half; i++)
73      {
74        coeff = (m_x(j)-i)/v(i);
75        coeff *= coeff;
76        if (coeff < 1. && coeff > 0.)
77          fvec(j) -= u(i)*std::pow((1-coeff), 2);
78      }
79    }
80    return 0;
81  }
82
83  int df(const VectorType& uv, JacobianType& fjac)
84  {
85    int m = Base::values();
86    int n = Base::inputs();
87    eigen_assert(n == uv.size());
88    eigen_assert(fjac.rows() == m);
89    eigen_assert(fjac.cols() == n);
90    int half = n/2;
91    VectorBlock<const VectorType> u(uv, 0, half);
92    VectorBlock<const VectorType> v(uv, half, half);
93    Scalar coeff;
94
95    //Derivatives with respect to u
96    for (int col = 0; col < half; col++)
97    {
98      for (int row = 0; row < m; row++)
99      {
100        coeff = (m_x(row)-col)/v(col);
101          coeff = coeff*coeff;
102        if(coeff < 1. && coeff > 0.)
103        {
104          fjac.coeffRef(row,col) = -(1-coeff)*(1-coeff);
105        }
106      }
107    }
108    //Derivatives with respect to v
109    for (int col = 0; col < half; col++)
110    {
111      for (int row = 0; row < m; row++)
112      {
113        coeff = (m_x(row)-col)/v(col);
114        coeff = coeff*coeff;
115        if(coeff < 1. && coeff > 0.)
116        {
117          fjac.coeffRef(row,col+half) = -4 * (u(col)/v(col))*coeff*(1-coeff);
118        }
119      }
120    }
121    return 0;
122  }
123
124  VectorType m_x, m_y; //Data points
125};
126
127
128template<typename T>
129void test_sparseLM_T()
130{
131  typedef Matrix<T,Dynamic,1> VectorType;
132
133  int inputs = 10;
134  int values = 2000;
135  sparseGaussianTest<T> sparse_gaussian(inputs, values);
136  VectorType uv(inputs),uv_ref(inputs);
137  VectorType x(values);
138  // Generate the reference solution
139  uv_ref << -2, 1, 4 ,8, 6, 1.8, 1.2, 1.1, 1.9 , 3;
140  //Generate the reference data points
141  x.setRandom();
142  x = 10*x;
143  x.array() += 10;
144  sparse_gaussian.initPoints(uv_ref, x);
145
146
147  // Generate the initial parameters
148  VectorBlock<VectorType> u(uv, 0, inputs/2);
149  VectorBlock<VectorType> v(uv, inputs/2, inputs/2);
150  v.setOnes();
151  //Generate u or Solve for u from v
152  u.setOnes();
153
154  // Solve the optimization problem
155  LevenbergMarquardt<sparseGaussianTest<T> > lm(sparse_gaussian);
156  int info;
157//   info = lm.minimize(uv);
158
159  VERIFY_IS_EQUAL(info,1);
160    // Do a step by step solution and save the residual
161  int maxiter = 200;
162  int iter = 0;
163  MatrixXd Err(values, maxiter);
164  MatrixXd Mod(values, maxiter);
165  LevenbergMarquardtSpace::Status status;
166  status = lm.minimizeInit(uv);
167  if (status==LevenbergMarquardtSpace::ImproperInputParameters)
168      return ;
169
170}
171void test_sparseLM()
172{
173  CALL_SUBTEST_1(test_sparseLM_T<double>());
174
175  // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
176}
177