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
17 using namespace std;
18 using namespace Eigen;
19
20 template <typename Scalar>
21 struct sparseGaussianTest : SparseFunctor<Scalar, int>
22 {
23 typedef Matrix<Scalar,Dynamic,1> VectorType;
24 typedef SparseFunctor<Scalar,int> Base;
25 typedef typename Base::JacobianType JacobianType;
sparseGaussianTestsparseGaussianTest26 sparseGaussianTest(int inputs, int values) : SparseFunctor<Scalar,int>(inputs,values)
27 { }
28
modelsparseGaussianTest29 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 }
initPointssparseGaussianTest54 void initPoints(VectorType& uv_ref, VectorType& x)
55 {
56 m_x = x;
57 m_y = this->model(uv_ref,x);
58 }
operator ()sparseGaussianTest59 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
dfsparseGaussianTest83 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
128 template<typename T>
test_sparseLM_T()129 void 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 }
test_sparseLM()171 void test_sparseLM()
172 {
173 CALL_SUBTEST_1(test_sparseLM_T<double>());
174
175 // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
176 }
177