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
11#include <iostream>
12#include <fstream>
13#include <iomanip>
14
15#include "main.h"
16#include <Eigen/LevenbergMarquardt>
17using namespace std;
18using namespace Eigen;
19
20template<typename Scalar>
21struct DenseLM : DenseFunctor<Scalar>
22{
23  typedef DenseFunctor<Scalar> Base;
24  typedef typename Base::JacobianType JacobianType;
25  typedef Matrix<Scalar,Dynamic,1> VectorType;
26
27  DenseLM(int n, int m) : DenseFunctor<Scalar>(n,m)
28  { }
29
30  VectorType model(const VectorType& uv, VectorType& x)
31  {
32    VectorType y; // Should change to use expression template
33    int m = Base::values();
34    int n = Base::inputs();
35    eigen_assert(uv.size()%2 == 0);
36    eigen_assert(uv.size() == n);
37    eigen_assert(x.size() == m);
38    y.setZero(m);
39    int half = n/2;
40    VectorBlock<const VectorType> u(uv, 0, half);
41    VectorBlock<const VectorType> v(uv, half, half);
42    for (int j = 0; j < m; j++)
43    {
44      for (int i = 0; i < half; i++)
45        y(j) += u(i)*std::exp(-(x(j)-i)*(x(j)-i)/(v(i)*v(i)));
46    }
47    return y;
48
49  }
50  void initPoints(VectorType& uv_ref, VectorType& x)
51  {
52    m_x = x;
53    m_y = this->model(uv_ref, x);
54  }
55
56  int operator()(const VectorType& uv, VectorType& fvec)
57  {
58
59    int m = Base::values();
60    int n = Base::inputs();
61    eigen_assert(uv.size()%2 == 0);
62    eigen_assert(uv.size() == n);
63    eigen_assert(fvec.size() == m);
64    int half = n/2;
65    VectorBlock<const VectorType> u(uv, 0, half);
66    VectorBlock<const VectorType> v(uv, half, half);
67    for (int j = 0; j < m; j++)
68    {
69      fvec(j) = m_y(j);
70      for (int i = 0; i < half; i++)
71      {
72        fvec(j) -= u(i) *std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i)));
73      }
74    }
75
76    return 0;
77  }
78  int df(const VectorType& uv, JacobianType& fjac)
79  {
80    int m = Base::values();
81    int n = Base::inputs();
82    eigen_assert(n == uv.size());
83    eigen_assert(fjac.rows() == m);
84    eigen_assert(fjac.cols() == n);
85    int half = n/2;
86    VectorBlock<const VectorType> u(uv, 0, half);
87    VectorBlock<const VectorType> v(uv, half, half);
88    for (int j = 0; j < m; j++)
89    {
90      for (int i = 0; i < half; i++)
91      {
92        fjac.coeffRef(j,i) = -std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i)));
93        fjac.coeffRef(j,i+half) = -2.*u(i)*(m_x(j)-i)*(m_x(j)-i)/(std::pow(v(i),3)) * std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i)));
94      }
95    }
96    return 0;
97  }
98  VectorType m_x, m_y; //Data Points
99};
100
101template<typename FunctorType, typename VectorType>
102int test_minimizeLM(FunctorType& functor, VectorType& uv)
103{
104  LevenbergMarquardt<FunctorType> lm(functor);
105  LevenbergMarquardtSpace::Status info;
106
107  info = lm.minimize(uv);
108
109  VERIFY_IS_EQUAL(info, 1);
110  //FIXME Check other parameters
111  return info;
112}
113
114template<typename FunctorType, typename VectorType>
115int test_lmder(FunctorType& functor, VectorType& uv)
116{
117  typedef typename VectorType::Scalar Scalar;
118  LevenbergMarquardtSpace::Status info;
119  LevenbergMarquardt<FunctorType> lm(functor);
120  info = lm.lmder1(uv);
121
122  VERIFY_IS_EQUAL(info, 1);
123  //FIXME Check other parameters
124  return info;
125}
126
127template<typename FunctorType, typename VectorType>
128int test_minimizeSteps(FunctorType& functor, VectorType& uv)
129{
130  LevenbergMarquardtSpace::Status info;
131  LevenbergMarquardt<FunctorType> lm(functor);
132  info = lm.minimizeInit(uv);
133  if (info==LevenbergMarquardtSpace::ImproperInputParameters)
134      return info;
135  do
136  {
137    info = lm.minimizeOneStep(uv);
138  } while (info==LevenbergMarquardtSpace::Running);
139
140  VERIFY_IS_EQUAL(info, 1);
141  //FIXME Check other parameters
142  return info;
143}
144
145template<typename T>
146void test_denseLM_T()
147{
148  typedef Matrix<T,Dynamic,1> VectorType;
149
150  int inputs = 10;
151  int values = 1000;
152  DenseLM<T> dense_gaussian(inputs, values);
153  VectorType uv(inputs),uv_ref(inputs);
154  VectorType x(values);
155
156  // Generate the reference solution
157  uv_ref << -2, 1, 4 ,8, 6, 1.8, 1.2, 1.1, 1.9 , 3;
158
159  //Generate the reference data points
160  x.setRandom();
161  x = 10*x;
162  x.array() += 10;
163  dense_gaussian.initPoints(uv_ref, x);
164
165  // Generate the initial parameters
166  VectorBlock<VectorType> u(uv, 0, inputs/2);
167  VectorBlock<VectorType> v(uv, inputs/2, inputs/2);
168
169  // Solve the optimization problem
170
171  //Solve in one go
172  u.setOnes(); v.setOnes();
173  test_minimizeLM(dense_gaussian, uv);
174
175  //Solve until the machine precision
176  u.setOnes(); v.setOnes();
177  test_lmder(dense_gaussian, uv);
178
179  // Solve step by step
180  v.setOnes(); u.setOnes();
181  test_minimizeSteps(dense_gaussian, uv);
182
183}
184
185void test_denseLM()
186{
187  CALL_SUBTEST_2(test_denseLM_T<double>());
188
189  // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
190}
191