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>
17 using namespace std;
18 using namespace Eigen;
19
20 template<typename Scalar>
21 struct DenseLM : DenseFunctor<Scalar>
22 {
23 typedef DenseFunctor<Scalar> Base;
24 typedef typename Base::JacobianType JacobianType;
25 typedef Matrix<Scalar,Dynamic,1> VectorType;
26
DenseLMDenseLM27 DenseLM(int n, int m) : DenseFunctor<Scalar>(n,m)
28 { }
29
modelDenseLM30 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 }
initPointsDenseLM50 void initPoints(VectorType& uv_ref, VectorType& x)
51 {
52 m_x = x;
53 m_y = this->model(uv_ref, x);
54 }
55
operator ()DenseLM56 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 }
dfDenseLM78 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
101 template<typename FunctorType, typename VectorType>
test_minimizeLM(FunctorType & functor,VectorType & uv)102 int 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
114 template<typename FunctorType, typename VectorType>
test_lmder(FunctorType & functor,VectorType & uv)115 int 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
127 template<typename FunctorType, typename VectorType>
test_minimizeSteps(FunctorType & functor,VectorType & uv)128 int 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
145 template<typename T>
test_denseLM_T()146 void 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
test_denseLM()185 void test_denseLM()
186 {
187 CALL_SUBTEST_2(test_denseLM_T<double>());
188
189 // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
190 }
191