source: pacpussensors/trunk/Vislab/lib3dv-1.2.0/lib3dv/eigen/test/denseLM.cpp@ 148

Last change on this file since 148 was 136, checked in by ldecherf, 8 years ago

Doc

File size: 4.9 KB
Line 
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}
Note: See TracBrowser for help on using the repository browser.