[136] | 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 | }
|
---|