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 117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <iostream> 127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <fstream> 137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <iomanip> 147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include "main.h" 167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#include <Eigen/LevenbergMarquardt> 177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezusing namespace std; 187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezusing namespace Eigen; 197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename Scalar> 217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezstruct DenseLM : DenseFunctor<Scalar> 227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef DenseFunctor<Scalar> Base; 247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename Base::JacobianType JacobianType; 257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,Dynamic,1> VectorType; 267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez DenseLM(int n, int m) : DenseFunctor<Scalar>(n,m) 287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { } 297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType model(const VectorType& uv, VectorType& x) 317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType y; // Should change to use expression template 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int m = Base::values(); 347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int n = Base::inputs(); 357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size()%2 == 0); 367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size() == n); 377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(x.size() == m); 387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez y.setZero(m); 397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int half = n/2; 407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> u(uv, 0, half); 417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> v(uv, half, half); 427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int j = 0; j < m; j++) 437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int i = 0; i < half; i++) 457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez y(j) += u(i)*std::exp(-(x(j)-i)*(x(j)-i)/(v(i)*v(i))); 467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return y; 487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void initPoints(VectorType& uv_ref, VectorType& x) 517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_x = x; 537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_y = this->model(uv_ref, x); 547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int operator()(const VectorType& uv, VectorType& fvec) 577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int m = Base::values(); 607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int n = Base::inputs(); 617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size()%2 == 0); 627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(uv.size() == n); 637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(fvec.size() == m); 647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int half = n/2; 657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> u(uv, 0, half); 667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> v(uv, half, half); 677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int j = 0; j < m; j++) 687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fvec(j) = m_y(j); 707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int i = 0; i < half; i++) 717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fvec(j) -= u(i) *std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i))); 737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return 0; 777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int df(const VectorType& uv, JacobianType& fjac) 797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int m = Base::values(); 817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int n = Base::inputs(); 827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(n == uv.size()); 837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(fjac.rows() == m); 847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert(fjac.cols() == n); 857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int half = n/2; 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> u(uv, 0, half); 877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<const VectorType> v(uv, half, half); 887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int j = 0; j < m; j++) 897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (int i = 0; i < half; i++) 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez fjac.coeffRef(j,i) = -std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i))); 937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 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))); 947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return 0; 977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType m_x, m_y; //Data Points 997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez}; 1007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename FunctorType, typename VectorType> 1027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezint test_minimizeLM(FunctorType& functor, VectorType& uv) 1037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1047faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardt<FunctorType> lm(functor); 1057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status info; 1067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez info = lm.minimize(uv); 1087faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_EQUAL(info, 1); 1107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //FIXME Check other parameters 1117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return info; 1127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 1137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename FunctorType, typename VectorType> 1157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezint test_lmder(FunctorType& functor, VectorType& uv) 1167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename VectorType::Scalar Scalar; 1187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status info; 1197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardt<FunctorType> lm(functor); 1207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez info = lm.lmder1(uv); 1217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_EQUAL(info, 1); 1237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //FIXME Check other parameters 1247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return info; 1257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 1267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename FunctorType, typename VectorType> 1287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezint test_minimizeSteps(FunctorType& functor, VectorType& uv) 1297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status info; 1317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardt<FunctorType> lm(functor); 1327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez info = lm.minimizeInit(uv); 1337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (info==LevenbergMarquardtSpace::ImproperInputParameters) 1347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return info; 1357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez do 1367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez info = lm.minimizeOneStep(uv); 1387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } while (info==LevenbergMarquardtSpace::Running); 1397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_EQUAL(info, 1); 1417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //FIXME Check other parameters 1427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return info; 1437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 1447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename T> 1467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid test_denseLM_T() 1477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<T,Dynamic,1> VectorType; 1497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int inputs = 10; 1517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int values = 1000; 1527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez DenseLM<T> dense_gaussian(inputs, values); 1537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType uv(inputs),uv_ref(inputs); 1547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType x(values); 1557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Generate the reference solution 1577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez uv_ref << -2, 1, 4 ,8, 6, 1.8, 1.2, 1.1, 1.9 , 3; 1587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //Generate the reference data points 1607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x.setRandom(); 1617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x = 10*x; 1627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez x.array() += 10; 1637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez dense_gaussian.initPoints(uv_ref, x); 1647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Generate the initial parameters 1667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<VectorType> u(uv, 0, inputs/2); 1677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorBlock<VectorType> v(uv, inputs/2, inputs/2); 1687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Solve the optimization problem 1707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //Solve in one go 1727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez u.setOnes(); v.setOnes(); 1737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez test_minimizeLM(dense_gaussian, uv); 1747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //Solve until the machine precision 1767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez u.setOnes(); v.setOnes(); 1777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez test_lmder(dense_gaussian, uv); 1787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Solve step by step 1807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez v.setOnes(); u.setOnes(); 1817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez test_minimizeSteps(dense_gaussian, uv); 1827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 1847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid test_denseLM() 1867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CALL_SUBTEST_2(test_denseLM_T<double>()); 1887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>()); 1907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 191