ground/gcs/src/libs/eigen/test/denseLM.cpp

   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>
  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 }