You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

176 lines
4.6 KiB

  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. #include <iostream>
  11. #include <fstream>
  12. #include <iomanip>
  13. #include "main.h"
  14. #include <Eigen/LevenbergMarquardt>
  15. using namespace std;
  16. using namespace StormEigen;
  17. template <typename Scalar>
  18. struct sparseGaussianTest : SparseFunctor<Scalar, int>
  19. {
  20. typedef Matrix<Scalar,Dynamic,1> VectorType;
  21. typedef SparseFunctor<Scalar,int> Base;
  22. typedef typename Base::JacobianType JacobianType;
  23. sparseGaussianTest(int inputs, int values) : SparseFunctor<Scalar,int>(inputs,values)
  24. { }
  25. VectorType model(const VectorType& uv, VectorType& x)
  26. {
  27. VectorType y; //Change this to use expression template
  28. int m = Base::values();
  29. int n = Base::inputs();
  30. eigen_assert(uv.size()%2 == 0);
  31. eigen_assert(uv.size() == n);
  32. eigen_assert(x.size() == m);
  33. y.setZero(m);
  34. int half = n/2;
  35. VectorBlock<const VectorType> u(uv, 0, half);
  36. VectorBlock<const VectorType> v(uv, half, half);
  37. Scalar coeff;
  38. for (int j = 0; j < m; j++)
  39. {
  40. for (int i = 0; i < half; i++)
  41. {
  42. coeff = (x(j)-i)/v(i);
  43. coeff *= coeff;
  44. if (coeff < 1. && coeff > 0.)
  45. y(j) += u(i)*std::pow((1-coeff), 2);
  46. }
  47. }
  48. return y;
  49. }
  50. void initPoints(VectorType& uv_ref, VectorType& x)
  51. {
  52. m_x = x;
  53. m_y = this->model(uv_ref,x);
  54. }
  55. int operator()(const VectorType& uv, VectorType& fvec)
  56. {
  57. int m = Base::values();
  58. int n = Base::inputs();
  59. eigen_assert(uv.size()%2 == 0);
  60. eigen_assert(uv.size() == n);
  61. int half = n/2;
  62. VectorBlock<const VectorType> u(uv, 0, half);
  63. VectorBlock<const VectorType> v(uv, half, half);
  64. fvec = m_y;
  65. Scalar coeff;
  66. for (int j = 0; j < m; j++)
  67. {
  68. for (int i = 0; i < half; i++)
  69. {
  70. coeff = (m_x(j)-i)/v(i);
  71. coeff *= coeff;
  72. if (coeff < 1. && coeff > 0.)
  73. fvec(j) -= u(i)*std::pow((1-coeff), 2);
  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. Scalar coeff;
  89. //Derivatives with respect to u
  90. for (int col = 0; col < half; col++)
  91. {
  92. for (int row = 0; row < m; row++)
  93. {
  94. coeff = (m_x(row)-col)/v(col);
  95. coeff = coeff*coeff;
  96. if(coeff < 1. && coeff > 0.)
  97. {
  98. fjac.coeffRef(row,col) = -(1-coeff)*(1-coeff);
  99. }
  100. }
  101. }
  102. //Derivatives with respect to v
  103. for (int col = 0; col < half; col++)
  104. {
  105. for (int row = 0; row < m; row++)
  106. {
  107. coeff = (m_x(row)-col)/v(col);
  108. coeff = coeff*coeff;
  109. if(coeff < 1. && coeff > 0.)
  110. {
  111. fjac.coeffRef(row,col+half) = -4 * (u(col)/v(col))*coeff*(1-coeff);
  112. }
  113. }
  114. }
  115. return 0;
  116. }
  117. VectorType m_x, m_y; //Data points
  118. };
  119. template<typename T>
  120. void test_sparseLM_T()
  121. {
  122. typedef Matrix<T,Dynamic,1> VectorType;
  123. int inputs = 10;
  124. int values = 2000;
  125. sparseGaussianTest<T> sparse_gaussian(inputs, values);
  126. VectorType uv(inputs),uv_ref(inputs);
  127. VectorType x(values);
  128. // Generate the reference solution
  129. uv_ref << -2, 1, 4 ,8, 6, 1.8, 1.2, 1.1, 1.9 , 3;
  130. //Generate the reference data points
  131. x.setRandom();
  132. x = 10*x;
  133. x.array() += 10;
  134. sparse_gaussian.initPoints(uv_ref, x);
  135. // Generate the initial parameters
  136. VectorBlock<VectorType> u(uv, 0, inputs/2);
  137. VectorBlock<VectorType> v(uv, inputs/2, inputs/2);
  138. v.setOnes();
  139. //Generate u or Solve for u from v
  140. u.setOnes();
  141. // Solve the optimization problem
  142. LevenbergMarquardt<sparseGaussianTest<T> > lm(sparse_gaussian);
  143. int info;
  144. // info = lm.minimize(uv);
  145. VERIFY_IS_EQUAL(info,1);
  146. // Do a step by step solution and save the residual
  147. int maxiter = 200;
  148. int iter = 0;
  149. MatrixXd Err(values, maxiter);
  150. MatrixXd Mod(values, maxiter);
  151. LevenbergMarquardtSpace::Status status;
  152. status = lm.minimizeInit(uv);
  153. if (status==LevenbergMarquardtSpace::ImproperInputParameters)
  154. return ;
  155. }
  156. void test_sparseLM()
  157. {
  158. CALL_SUBTEST_1(test_sparseLM_T<double>());
  159. // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
  160. }