denseLM.cpp
Go to the documentation of this file.
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 {
24  typedef typename Base::JacobianType JacobianType;
26 
27  DenseLM(int n, int m) : DenseFunctor<Scalar>(n,m)
28  { }
29 
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;
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  }
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;
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;
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 {
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;
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 {
132  info = lm.minimizeInit(uv);
134  return info;
135  do
136  {
137  info = lm.minimizeOneStep(uv);
139 
140  VERIFY_IS_EQUAL(info, 1);
141  //FIXME Check other parameters
142  return info;
143 }
144 
145 template<typename T>
147 {
149 
150  int inputs = 10;
151  int values = 1000;
152  DenseLM<T> dense_gaussian(inputs, values);
153  VectorType uv(inputs),uv_ref(inputs);
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 
186 {
187  CALL_SUBTEST_2(test_denseLM_T<double>());
188 
189  // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
190 }
DenseLM::VectorType
Matrix< Scalar, Dynamic, 1 > VectorType
Definition: denseLM.cpp:25
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::DenseFunctor< Scalar >::Scalar
Scalar Scalar
Definition: LevenbergMarquardt/LevenbergMarquardt.h:44
VERIFY_IS_EQUAL
#define VERIFY_IS_EQUAL(a, b)
Definition: main.h:386
DenseLM::operator()
int operator()(const VectorType &uv, VectorType &fvec)
Definition: denseLM.cpp:56
eigen_assert
#define eigen_assert(x)
Definition: Macros.h:1037
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
test_minimizeSteps
int test_minimizeSteps(FunctorType &functor, VectorType &uv)
Definition: denseLM.cpp:128
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
Eigen::LevenbergMarquardt
Performs non linear optimization over a non-linear function, using a variant of the Levenberg Marquar...
Definition: LevenbergMarquardt/LevenbergMarquardt.h:110
n
int n
Definition: BiCGSTAB_simple.cpp:1
DenseLM::model
VectorType model(const VectorType &uv, VectorType &x)
Definition: denseLM.cpp:30
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
Eigen::LevenbergMarquardtSpace::Status
Status
Definition: LevenbergMarquardt/LevenbergMarquardt.h:25
Eigen::LevenbergMarquardt::minimizeInit
LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:294
DenseLM::Base
DenseFunctor< Scalar > Base
Definition: denseLM.cpp:23
Eigen::PlainObjectBase::rows
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:143
Eigen::LevenbergMarquardtSpace::ImproperInputParameters
@ ImproperInputParameters
Definition: LevenbergMarquardt/LevenbergMarquardt.h:28
Eigen::LevenbergMarquardt::lmder1
LevenbergMarquardtSpace::Status lmder1(FVectorType &x, const Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
Definition: LevenbergMarquardt/LevenbergMarquardt.h:344
info
else if n * info
Definition: 3rdparty/Eigen/lapack/cholesky.cpp:18
test_lmder
int test_lmder(FunctorType &functor, VectorType &uv)
Definition: denseLM.cpp:115
Eigen::Matrix::coeffRef
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Definition: PlainObjectBase.h:175
DenseLM
Definition: denseLM.cpp:21
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
CALL_SUBTEST_2
#define CALL_SUBTEST_2(FUNC)
Definition: split_test_helper.h:10
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
y
Scalar * y
Definition: level1_cplx_impl.h:124
test_denseLM_T
void test_denseLM_T()
Definition: denseLM.cpp:146
Eigen::LevenbergMarquardt::minimizeOneStep
LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x)
Definition: LMonestep.h:21
EIGEN_DECLARE_TEST
EIGEN_DECLARE_TEST(denseLM)
Definition: denseLM.cpp:185
Eigen::LevenbergMarquardtSpace::Running
@ Running
Definition: LevenbergMarquardt/LevenbergMarquardt.h:27
DenseLM::m_y
VectorType m_y
Definition: denseLM.cpp:98
Eigen::PlainObjectBase::cols
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:145
main.h
std
Definition: BFloat16.h:88
DenseLM::JacobianType
Base::JacobianType JacobianType
Definition: denseLM.cpp:24
Eigen::VectorBlock
Expression of a fixed-size or dynamic-size sub-vector.
Definition: ForwardDeclarations.h:85
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
test_minimizeLM
int test_minimizeLM(FunctorType &functor, VectorType &uv)
Definition: denseLM.cpp:102
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
VectorType
Definition: FFTW.cpp:65
DenseLM::DenseLM
DenseLM(int n, int m)
Definition: denseLM.cpp:27
Eigen::LevenbergMarquardt::minimize
LevenbergMarquardtSpace::Status minimize(FVectorType &x)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:277
Eigen::half
Definition: Half.h:142
DenseLM::initPoints
void initPoints(VectorType &uv_ref, VectorType &x)
Definition: denseLM.cpp:50
DenseLM::df
int df(const VectorType &uv, JacobianType &fjac)
Definition: denseLM.cpp:78
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
Eigen::DenseFunctor
Definition: LevenbergMarquardt/LevenbergMarquardt.h:42


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:12