17faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This file is part of Eigen, a lightweight C++ template library 27faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// for linear algebra. 37faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 47faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> 57faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr> 67faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 77faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This Source Code Form is subject to the terms of the Mozilla 87faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Public License v. 2.0. If a copy of the MPL was not distributed 97faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <iostream> 117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <fstream> 127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <iomanip> 137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include "main.h" 157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <Eigen/LevenbergMarquardt> 167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezusing namespace std; 187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezusing namespace Eigen; 197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate <typename Scalar> 217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezstruct sparseGaussianTest : SparseFunctor<Scalar, int> 227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,Dynamic,1> VectorType; 247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef SparseFunctor<Scalar,int> Base; 257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename Base::JacobianType JacobianType; 267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sparseGaussianTest(int inputs, int values) : SparseFunctor<Scalar,int>(inputs,values) 277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { } 287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType model(const VectorType& uv, VectorType& x) 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType y; //Change this to use expression template 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int m = Base::values(); 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int n = Base::inputs(); 347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size()%2 == 0); 357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size() == n); 367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(x.size() == m); 377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez y.setZero(m); 387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int half = n/2; 397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> u(uv, 0, half); 407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> v(uv, half, half); 417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar coeff; 427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int j = 0; j < m; j++) 437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int i = 0; i < half; i++) 457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff = (x(j)-i)/v(i); 477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff *= coeff; 487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (coeff < 1. && coeff > 0.) 497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez y(j) += u(i)*std::pow((1-coeff), 2); 507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return y; 537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void initPoints(VectorType& uv_ref, VectorType& x) 557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_x = x; 577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_y = this->model(uv_ref,x); 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int operator()(const VectorType& uv, VectorType& fvec) 607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int m = Base::values(); 627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int n = Base::inputs(); 637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size()%2 == 0); 647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size() == n); 657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int half = n/2; 667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> u(uv, 0, half); 677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> v(uv, half, half); 687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fvec = m_y; 697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar coeff; 707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int j = 0; j < m; j++) 717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int i = 0; i < half; i++) 737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff = (m_x(j)-i)/v(i); 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff *= coeff; 767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (coeff < 1. && coeff > 0.) 777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fvec(j) -= u(i)*std::pow((1-coeff), 2); 787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return 0; 817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int df(const VectorType& uv, JacobianType& fjac) 847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int m = Base::values(); 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int n = Base::inputs(); 877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n == uv.size()); 887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(fjac.rows() == m); 897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(fjac.cols() == n); 907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int half = n/2; 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> u(uv, 0, half); 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> v(uv, half, half); 937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar coeff; 947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //Derivatives with respect to u 967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int col = 0; col < half; col++) 977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int row = 0; row < m; row++) 997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff = (m_x(row)-col)/v(col); 1017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff = coeff*coeff; 1027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if(coeff < 1. && coeff > 0.) 1037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1047faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fjac.coeffRef(row,col) = -(1-coeff)*(1-coeff); 1057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1087faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //Derivatives with respect to v 1097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int col = 0; col < half; col++) 1107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int row = 0; row < m; row++) 1127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff = (m_x(row)-col)/v(col); 1147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez coeff = coeff*coeff; 1157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if(coeff < 1. && coeff > 0.) 1167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fjac.coeffRef(row,col+half) = -4 * (u(col)/v(col))*coeff*(1-coeff); 1187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return 0; 1227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType m_x, m_y; //Data points 1257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez}; 1267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename T> 1297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid test_sparseLM_T() 1307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<T,Dynamic,1> VectorType; 1327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int inputs = 10; 1347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int values = 2000; 1357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sparseGaussianTest<T> sparse_gaussian(inputs, values); 1367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType uv(inputs),uv_ref(inputs); 1377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType x(values); 1387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Generate the reference solution 1397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez uv_ref << -2, 1, 4 ,8, 6, 1.8, 1.2, 1.1, 1.9 , 3; 1407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //Generate the reference data points 1417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x.setRandom(); 1427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x = 10*x; 1437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x.array() += 10; 1447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez sparse_gaussian.initPoints(uv_ref, x); 1457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Generate the initial parameters 1487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<VectorType> u(uv, 0, inputs/2); 1497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<VectorType> v(uv, inputs/2, inputs/2); 1507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez v.setOnes(); 1517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //Generate u or Solve for u from v 1527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez u.setOnes(); 1537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Solve the optimization problem 1557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardt<sparseGaussianTest<T> > lm(sparse_gaussian); 1567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int info; 1577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// info = lm.minimize(uv); 1587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_EQUAL(info,1); 1607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Do a step by step solution and save the residual 1617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int maxiter = 200; 1627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int iter = 0; 1637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez MatrixXd Err(values, maxiter); 1647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez MatrixXd Mod(values, maxiter); 1657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status status; 1667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez status = lm.minimizeInit(uv); 1677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (status==LevenbergMarquardtSpace::ImproperInputParameters) 1687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return ; 1697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 1717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid test_sparseLM() 1727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST_1(test_sparseLM_T<double>()); 1747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>()); 1767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 177