LP-311 Remove basic/advanced stabilization tab auto-switch (autotune/txpid lock issues)
[librepilot.git] / ground / gcs / src / libs / eigen / test / jacobisvd.cpp
blob12c556e59a43dc9f45cf95ef0439232696c7274c
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
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/.
11 // discard stack allocation as that too bypasses malloc
12 #define EIGEN_STACK_ALLOCATION_LIMIT 0
13 #define EIGEN_RUNTIME_NO_MALLOC
14 #include "main.h"
15 #include <Eigen/SVD>
17 template<typename MatrixType, int QRPreconditioner>
18 void jacobisvd_check_full(const MatrixType& m, const JacobiSVD<MatrixType, QRPreconditioner>& svd)
20 typedef typename MatrixType::Index Index;
21 Index rows = m.rows();
22 Index cols = m.cols();
24 enum {
25 RowsAtCompileTime = MatrixType::RowsAtCompileTime,
26 ColsAtCompileTime = MatrixType::ColsAtCompileTime
29 typedef typename MatrixType::Scalar Scalar;
30 typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime> MatrixUType;
31 typedef Matrix<Scalar, ColsAtCompileTime, ColsAtCompileTime> MatrixVType;
33 MatrixType sigma = MatrixType::Zero(rows,cols);
34 sigma.diagonal() = svd.singularValues().template cast<Scalar>();
35 MatrixUType u = svd.matrixU();
36 MatrixVType v = svd.matrixV();
38 VERIFY_IS_APPROX(m, u * sigma * v.adjoint());
39 VERIFY_IS_UNITARY(u);
40 VERIFY_IS_UNITARY(v);
43 template<typename MatrixType, int QRPreconditioner>
44 void jacobisvd_compare_to_full(const MatrixType& m,
45 unsigned int computationOptions,
46 const JacobiSVD<MatrixType, QRPreconditioner>& referenceSvd)
48 typedef typename MatrixType::Index Index;
49 Index rows = m.rows();
50 Index cols = m.cols();
51 Index diagSize = (std::min)(rows, cols);
53 JacobiSVD<MatrixType, QRPreconditioner> svd(m, computationOptions);
55 VERIFY_IS_APPROX(svd.singularValues(), referenceSvd.singularValues());
56 if(computationOptions & ComputeFullU)
57 VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU());
58 if(computationOptions & ComputeThinU)
59 VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU().leftCols(diagSize));
60 if(computationOptions & ComputeFullV)
61 VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV());
62 if(computationOptions & ComputeThinV)
63 VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV().leftCols(diagSize));
66 template<typename MatrixType, int QRPreconditioner>
67 void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions)
69 typedef typename MatrixType::Scalar Scalar;
70 typedef typename MatrixType::RealScalar RealScalar;
71 typedef typename MatrixType::Index Index;
72 Index rows = m.rows();
73 Index cols = m.cols();
75 enum {
76 RowsAtCompileTime = MatrixType::RowsAtCompileTime,
77 ColsAtCompileTime = MatrixType::ColsAtCompileTime
80 typedef Matrix<Scalar, RowsAtCompileTime, Dynamic> RhsType;
81 typedef Matrix<Scalar, ColsAtCompileTime, Dynamic> SolutionType;
83 RhsType rhs = RhsType::Random(rows, internal::random<Index>(1, cols));
84 JacobiSVD<MatrixType, QRPreconditioner> svd(m, computationOptions);
86 if(internal::is_same<RealScalar,double>::value) svd.setThreshold(1e-8);
87 else if(internal::is_same<RealScalar,float>::value) svd.setThreshold(1e-4);
89 SolutionType x = svd.solve(rhs);
91 RealScalar residual = (m*x-rhs).norm();
92 // Check that there is no significantly better solution in the neighborhood of x
93 if(!test_isMuchSmallerThan(residual,rhs.norm()))
95 // If the residual is very small, then we have an exact solution, so we are already good.
96 for(int k=0;k<x.rows();++k)
98 SolutionType y(x);
99 y.row(k).array() += 2*NumTraits<RealScalar>::epsilon();
100 RealScalar residual_y = (m*y-rhs).norm();
101 VERIFY( test_isApprox(residual_y,residual) || residual < residual_y );
103 y.row(k) = x.row(k).array() - 2*NumTraits<RealScalar>::epsilon();
104 residual_y = (m*y-rhs).norm();
105 VERIFY( test_isApprox(residual_y,residual) || residual < residual_y );
109 // evaluate normal equation which works also for least-squares solutions
110 if(internal::is_same<RealScalar,double>::value)
112 // This test is not stable with single precision.
113 // This is probably because squaring m signicantly affects the precision.
114 VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs);
117 // check minimal norm solutions
119 // generate a full-rank m x n problem with m<n
120 enum {
121 RankAtCompileTime2 = ColsAtCompileTime==Dynamic ? Dynamic : (ColsAtCompileTime)/2+1,
122 RowsAtCompileTime3 = ColsAtCompileTime==Dynamic ? Dynamic : ColsAtCompileTime+1
124 typedef Matrix<Scalar, RankAtCompileTime2, ColsAtCompileTime> MatrixType2;
125 typedef Matrix<Scalar, RankAtCompileTime2, 1> RhsType2;
126 typedef Matrix<Scalar, ColsAtCompileTime, RankAtCompileTime2> MatrixType2T;
127 Index rank = RankAtCompileTime2==Dynamic ? internal::random<Index>(1,cols) : Index(RankAtCompileTime2);
128 MatrixType2 m2(rank,cols);
129 int guard = 0;
130 do {
131 m2.setRandom();
132 } while(m2.jacobiSvd().setThreshold(test_precision<Scalar>()).rank()!=rank && (++guard)<10);
133 VERIFY(guard<10);
134 RhsType2 rhs2 = RhsType2::Random(rank);
135 // use QR to find a reference minimal norm solution
136 HouseholderQR<MatrixType2T> qr(m2.adjoint());
137 Matrix<Scalar,Dynamic,1> tmp = qr.matrixQR().topLeftCorner(rank,rank).template triangularView<Upper>().adjoint().solve(rhs2);
138 tmp.conservativeResize(cols);
139 tmp.tail(cols-rank).setZero();
140 SolutionType x21 = qr.householderQ() * tmp;
141 // now check with SVD
142 JacobiSVD<MatrixType2, ColPivHouseholderQRPreconditioner> svd2(m2, computationOptions);
143 SolutionType x22 = svd2.solve(rhs2);
144 VERIFY_IS_APPROX(m2*x21, rhs2);
145 VERIFY_IS_APPROX(m2*x22, rhs2);
146 VERIFY_IS_APPROX(x21, x22);
148 // Now check with a rank deficient matrix
149 typedef Matrix<Scalar, RowsAtCompileTime3, ColsAtCompileTime> MatrixType3;
150 typedef Matrix<Scalar, RowsAtCompileTime3, 1> RhsType3;
151 Index rows3 = RowsAtCompileTime3==Dynamic ? internal::random<Index>(rank+1,2*cols) : Index(RowsAtCompileTime3);
152 Matrix<Scalar,RowsAtCompileTime3,Dynamic> C = Matrix<Scalar,RowsAtCompileTime3,Dynamic>::Random(rows3,rank);
153 MatrixType3 m3 = C * m2;
154 RhsType3 rhs3 = C * rhs2;
155 JacobiSVD<MatrixType3, ColPivHouseholderQRPreconditioner> svd3(m3, computationOptions);
156 SolutionType x3 = svd3.solve(rhs3);
157 if(svd3.rank()!=rank) {
158 std::cout << m3 << "\n\n";
159 std::cout << svd3.singularValues().transpose() << "\n";
160 std::cout << svd3.rank() << " == " << rank << "\n";
161 std::cout << x21.norm() << " == " << x3.norm() << "\n";
163 // VERIFY_IS_APPROX(m3*x3, rhs3);
164 VERIFY_IS_APPROX(m3*x21, rhs3);
165 VERIFY_IS_APPROX(m2*x3, rhs2);
167 VERIFY_IS_APPROX(x21, x3);
171 template<typename MatrixType, int QRPreconditioner>
172 void jacobisvd_test_all_computation_options(const MatrixType& m)
174 if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols())
175 return;
176 JacobiSVD<MatrixType, QRPreconditioner> fullSvd(m, ComputeFullU|ComputeFullV);
177 CALL_SUBTEST(( jacobisvd_check_full(m, fullSvd) ));
178 CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeFullV) ));
180 #if defined __INTEL_COMPILER
181 // remark #111: statement is unreachable
182 #pragma warning disable 111
183 #endif
184 if(QRPreconditioner == FullPivHouseholderQRPreconditioner)
185 return;
187 CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU, fullSvd) ));
188 CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullV, fullSvd) ));
189 CALL_SUBTEST(( jacobisvd_compare_to_full(m, 0, fullSvd) ));
191 if (MatrixType::ColsAtCompileTime == Dynamic) {
192 // thin U/V are only available with dynamic number of columns
193 CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd) ));
194 CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinV, fullSvd) ));
195 CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd) ));
196 CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU , fullSvd) ));
197 CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd) ));
198 CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeThinV) ));
199 CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeFullV) ));
200 CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeThinV) ));
202 // test reconstruction
203 typedef typename MatrixType::Index Index;
204 Index diagSize = (std::min)(m.rows(), m.cols());
205 JacobiSVD<MatrixType, QRPreconditioner> svd(m, ComputeThinU | ComputeThinV);
206 VERIFY_IS_APPROX(m, svd.matrixU().leftCols(diagSize) * svd.singularValues().asDiagonal() * svd.matrixV().leftCols(diagSize).adjoint());
210 template<typename MatrixType>
211 void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
213 MatrixType m = a;
214 if(pickrandom)
216 typedef typename MatrixType::Scalar Scalar;
217 typedef typename MatrixType::RealScalar RealScalar;
218 typedef typename MatrixType::Index Index;
219 Index diagSize = (std::min)(a.rows(), a.cols());
220 RealScalar s = std::numeric_limits<RealScalar>::max_exponent10/4;
221 s = internal::random<RealScalar>(1,s);
222 Matrix<RealScalar,Dynamic,1> d = Matrix<RealScalar,Dynamic,1>::Random(diagSize);
223 for(Index k=0; k<diagSize; ++k)
224 d(k) = d(k)*std::pow(RealScalar(10),internal::random<RealScalar>(-s,s));
225 m = Matrix<Scalar,Dynamic,Dynamic>::Random(a.rows(),diagSize) * d.asDiagonal() * Matrix<Scalar,Dynamic,Dynamic>::Random(diagSize,a.cols());
226 // cancel some coeffs
227 Index n = internal::random<Index>(0,m.size()-1);
228 for(Index i=0; i<n; ++i)
229 m(internal::random<Index>(0,m.rows()-1), internal::random<Index>(0,m.cols()-1)) = Scalar(0);
232 CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, FullPivHouseholderQRPreconditioner>(m) ));
233 CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, ColPivHouseholderQRPreconditioner>(m) ));
234 CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, HouseholderQRPreconditioner>(m) ));
235 CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, NoQRPreconditioner>(m) ));
238 template<typename MatrixType> void jacobisvd_verify_assert(const MatrixType& m)
240 typedef typename MatrixType::Scalar Scalar;
241 typedef typename MatrixType::Index Index;
242 Index rows = m.rows();
243 Index cols = m.cols();
245 enum {
246 RowsAtCompileTime = MatrixType::RowsAtCompileTime,
247 ColsAtCompileTime = MatrixType::ColsAtCompileTime
250 typedef Matrix<Scalar, RowsAtCompileTime, 1> RhsType;
252 RhsType rhs(rows);
254 JacobiSVD<MatrixType> svd;
255 VERIFY_RAISES_ASSERT(svd.matrixU())
256 VERIFY_RAISES_ASSERT(svd.singularValues())
257 VERIFY_RAISES_ASSERT(svd.matrixV())
258 VERIFY_RAISES_ASSERT(svd.solve(rhs))
260 MatrixType a = MatrixType::Zero(rows, cols);
261 a.setZero();
262 svd.compute(a, 0);
263 VERIFY_RAISES_ASSERT(svd.matrixU())
264 VERIFY_RAISES_ASSERT(svd.matrixV())
265 svd.singularValues();
266 VERIFY_RAISES_ASSERT(svd.solve(rhs))
268 if (ColsAtCompileTime == Dynamic)
270 svd.compute(a, ComputeThinU);
271 svd.matrixU();
272 VERIFY_RAISES_ASSERT(svd.matrixV())
273 VERIFY_RAISES_ASSERT(svd.solve(rhs))
275 svd.compute(a, ComputeThinV);
276 svd.matrixV();
277 VERIFY_RAISES_ASSERT(svd.matrixU())
278 VERIFY_RAISES_ASSERT(svd.solve(rhs))
280 JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner> svd_fullqr;
281 VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeFullU|ComputeThinV))
282 VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeThinV))
283 VERIFY_RAISES_ASSERT(svd_fullqr.compute(a, ComputeThinU|ComputeFullV))
285 else
287 VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinU))
288 VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinV))
292 template<typename MatrixType>
293 void jacobisvd_method()
295 enum { Size = MatrixType::RowsAtCompileTime };
296 typedef typename MatrixType::RealScalar RealScalar;
297 typedef Matrix<RealScalar, Size, 1> RealVecType;
298 MatrixType m = MatrixType::Identity();
299 VERIFY_IS_APPROX(m.jacobiSvd().singularValues(), RealVecType::Ones());
300 VERIFY_RAISES_ASSERT(m.jacobiSvd().matrixU());
301 VERIFY_RAISES_ASSERT(m.jacobiSvd().matrixV());
302 VERIFY_IS_APPROX(m.jacobiSvd(ComputeFullU|ComputeFullV).solve(m), m);
305 // work around stupid msvc error when constructing at compile time an expression that involves
306 // a division by zero, even if the numeric type has floating point
307 template<typename Scalar>
308 EIGEN_DONT_INLINE Scalar zero() { return Scalar(0); }
310 // workaround aggressive optimization in ICC
311 template<typename T> EIGEN_DONT_INLINE T sub(T a, T b) { return a - b; }
313 template<typename MatrixType>
314 void jacobisvd_inf_nan()
316 // all this function does is verify we don't iterate infinitely on nan/inf values
318 JacobiSVD<MatrixType> svd;
319 typedef typename MatrixType::Scalar Scalar;
320 Scalar some_inf = Scalar(1) / zero<Scalar>();
321 VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf));
322 svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV);
324 Scalar nan = std::numeric_limits<Scalar>::quiet_NaN();
325 VERIFY(nan != nan);
326 svd.compute(MatrixType::Constant(10,10,nan), ComputeFullU | ComputeFullV);
328 MatrixType m = MatrixType::Zero(10,10);
329 m(internal::random<int>(0,9), internal::random<int>(0,9)) = some_inf;
330 svd.compute(m, ComputeFullU | ComputeFullV);
332 m = MatrixType::Zero(10,10);
333 m(internal::random<int>(0,9), internal::random<int>(0,9)) = nan;
334 svd.compute(m, ComputeFullU | ComputeFullV);
336 // regression test for bug 791
337 m.resize(3,3);
338 m << 0, 2*NumTraits<Scalar>::epsilon(), 0.5,
339 0, -0.5, 0,
340 nan, 0, 0;
341 svd.compute(m, ComputeFullU | ComputeFullV);
344 // Regression test for bug 286: JacobiSVD loops indefinitely with some
345 // matrices containing denormal numbers.
346 void jacobisvd_bug286()
348 #if defined __INTEL_COMPILER
349 // shut up warning #239: floating point underflow
350 #pragma warning push
351 #pragma warning disable 239
352 #endif
353 Matrix2d M;
354 M << -7.90884e-313, -4.94e-324,
355 0, 5.60844e-313;
356 #if defined __INTEL_COMPILER
357 #pragma warning pop
358 #endif
359 JacobiSVD<Matrix2d> svd;
360 svd.compute(M); // just check we don't loop indefinitely
363 void jacobisvd_preallocate()
365 Vector3f v(3.f, 2.f, 1.f);
366 MatrixXf m = v.asDiagonal();
368 internal::set_is_malloc_allowed(false);
369 VERIFY_RAISES_ASSERT(VectorXf tmp(10);)
370 JacobiSVD<MatrixXf> svd;
371 internal::set_is_malloc_allowed(true);
372 svd.compute(m);
373 VERIFY_IS_APPROX(svd.singularValues(), v);
375 JacobiSVD<MatrixXf> svd2(3,3);
376 internal::set_is_malloc_allowed(false);
377 svd2.compute(m);
378 internal::set_is_malloc_allowed(true);
379 VERIFY_IS_APPROX(svd2.singularValues(), v);
380 VERIFY_RAISES_ASSERT(svd2.matrixU());
381 VERIFY_RAISES_ASSERT(svd2.matrixV());
382 svd2.compute(m, ComputeFullU | ComputeFullV);
383 VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity());
384 VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity());
385 internal::set_is_malloc_allowed(false);
386 svd2.compute(m);
387 internal::set_is_malloc_allowed(true);
389 JacobiSVD<MatrixXf> svd3(3,3,ComputeFullU|ComputeFullV);
390 internal::set_is_malloc_allowed(false);
391 svd2.compute(m);
392 internal::set_is_malloc_allowed(true);
393 VERIFY_IS_APPROX(svd2.singularValues(), v);
394 VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity());
395 VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity());
396 internal::set_is_malloc_allowed(false);
397 svd2.compute(m, ComputeFullU|ComputeFullV);
398 internal::set_is_malloc_allowed(true);
401 void test_jacobisvd()
403 CALL_SUBTEST_3(( jacobisvd_verify_assert(Matrix3f()) ));
404 CALL_SUBTEST_4(( jacobisvd_verify_assert(Matrix4d()) ));
405 CALL_SUBTEST_7(( jacobisvd_verify_assert(MatrixXf(10,12)) ));
406 CALL_SUBTEST_8(( jacobisvd_verify_assert(MatrixXcd(7,5)) ));
408 for(int i = 0; i < g_repeat; i++) {
409 Matrix2cd m;
410 m << 0, 1,
411 0, 1;
412 CALL_SUBTEST_1(( jacobisvd(m, false) ));
413 m << 1, 0,
414 1, 0;
415 CALL_SUBTEST_1(( jacobisvd(m, false) ));
417 Matrix2d n;
418 n << 0, 0,
419 0, 0;
420 CALL_SUBTEST_2(( jacobisvd(n, false) ));
421 n << 0, 0,
422 0, 1;
423 CALL_SUBTEST_2(( jacobisvd(n, false) ));
425 CALL_SUBTEST_3(( jacobisvd<Matrix3f>() ));
426 CALL_SUBTEST_4(( jacobisvd<Matrix4d>() ));
427 CALL_SUBTEST_5(( jacobisvd<Matrix<float,3,5> >() ));
428 CALL_SUBTEST_6(( jacobisvd<Matrix<double,Dynamic,2> >(Matrix<double,Dynamic,2>(10,2)) ));
430 int r = internal::random<int>(1, 30),
431 c = internal::random<int>(1, 30);
433 TEST_SET_BUT_UNUSED_VARIABLE(r)
434 TEST_SET_BUT_UNUSED_VARIABLE(c)
436 CALL_SUBTEST_10(( jacobisvd<MatrixXd>(MatrixXd(r,c)) ));
437 CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(r,c)) ));
438 CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(r,c)) ));
439 (void) r;
440 (void) c;
442 // Test on inf/nan matrix
443 CALL_SUBTEST_7( jacobisvd_inf_nan<MatrixXf>() );
444 CALL_SUBTEST_10( jacobisvd_inf_nan<MatrixXd>() );
447 CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) ));
448 CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3))) ));
450 // test matrixbase method
451 CALL_SUBTEST_1(( jacobisvd_method<Matrix2cd>() ));
452 CALL_SUBTEST_3(( jacobisvd_method<Matrix3f>() ));
454 // Test problem size constructors
455 CALL_SUBTEST_7( JacobiSVD<MatrixXf>(10,10) );
457 // Check that preallocation avoids subsequent mallocs
458 CALL_SUBTEST_9( jacobisvd_preallocate() );
460 // Regression check for bug 286
461 CALL_SUBTEST_2( jacobisvd_bug286() );