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 (at) inria.fr> 5 // Copyright (C) 2012 Gael Guennebaud <gael.guennebaud (at) 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 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 101 template<typename FunctorType, typename VectorType> 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> 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> 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> 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 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
