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 #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; 26 sparseGaussianTest(int inputs, int values) : SparseFunctor<Scalar,int>(inputs,values) 27 { } 28 29 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 } 54 void initPoints(VectorType& uv_ref, VectorType& x) 55 { 56 m_x = x; 57 m_y = this->model(uv_ref,x); 58 } 59 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 83 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> 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 } 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
