LP-311 Remove basic/advanced stabilization tab auto-switch (autotune/txpid lock issues)
[librepilot.git] / ground / gcs / src / libs / eigen / test / sparse_product.cpp
bloba2ea9d5b7922f1bcd464695b8409aa4a56d5cd69
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2011 Gael Guennebaud <gael.guennebaud@inria.fr>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10 #include "sparse.h"
12 template<typename SparseMatrixType, typename DenseMatrix, bool IsRowMajor=SparseMatrixType::IsRowMajor> struct test_outer;
14 template<typename SparseMatrixType, typename DenseMatrix> struct test_outer<SparseMatrixType,DenseMatrix,false> {
15 static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) {
16 typedef typename SparseMatrixType::Index Index;
17 Index c = internal::random<Index>(0,m2.cols()-1);
18 Index c1 = internal::random<Index>(0,m2.cols()-1);
19 VERIFY_IS_APPROX(m4=m2.col(c)*refMat2.col(c1).transpose(), refMat4=refMat2.col(c)*refMat2.col(c1).transpose());
20 VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.col(c).transpose(), refMat4=refMat2.col(c1)*refMat2.col(c).transpose());
24 template<typename SparseMatrixType, typename DenseMatrix> struct test_outer<SparseMatrixType,DenseMatrix,true> {
25 static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) {
26 typedef typename SparseMatrixType::Index Index;
27 Index r = internal::random<Index>(0,m2.rows()-1);
28 Index c1 = internal::random<Index>(0,m2.cols()-1);
29 VERIFY_IS_APPROX(m4=m2.row(r).transpose()*refMat2.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*refMat2.col(c1).transpose());
30 VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.row(r), refMat4=refMat2.col(c1)*refMat2.row(r));
34 // (m2,m4,refMat2,refMat4,dv1);
35 // VERIFY_IS_APPROX(m4=m2.innerVector(c)*dv1.transpose(), refMat4=refMat2.colVector(c)*dv1.transpose());
36 // VERIFY_IS_APPROX(m4=dv1*mcm.col(c).transpose(), refMat4=dv1*refMat2.col(c).transpose());
38 template<typename SparseMatrixType> void sparse_product()
40 typedef typename SparseMatrixType::Index Index;
41 Index n = 100;
42 const Index rows = internal::random<Index>(1,n);
43 const Index cols = internal::random<Index>(1,n);
44 const Index depth = internal::random<Index>(1,n);
45 typedef typename SparseMatrixType::Scalar Scalar;
46 enum { Flags = SparseMatrixType::Flags };
48 double density = (std::max)(8./(rows*cols), 0.1);
49 typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
50 typedef Matrix<Scalar,Dynamic,1> DenseVector;
51 typedef Matrix<Scalar,1,Dynamic> RowDenseVector;
52 typedef SparseVector<Scalar,0,Index> ColSpVector;
53 typedef SparseVector<Scalar,RowMajor,Index> RowSpVector;
55 Scalar s1 = internal::random<Scalar>();
56 Scalar s2 = internal::random<Scalar>();
58 // test matrix-matrix product
60 DenseMatrix refMat2 = DenseMatrix::Zero(rows, depth);
61 DenseMatrix refMat2t = DenseMatrix::Zero(depth, rows);
62 DenseMatrix refMat3 = DenseMatrix::Zero(depth, cols);
63 DenseMatrix refMat3t = DenseMatrix::Zero(cols, depth);
64 DenseMatrix refMat4 = DenseMatrix::Zero(rows, cols);
65 DenseMatrix refMat4t = DenseMatrix::Zero(cols, rows);
66 DenseMatrix refMat5 = DenseMatrix::Random(depth, cols);
67 DenseMatrix refMat6 = DenseMatrix::Random(rows, rows);
68 DenseMatrix dm4 = DenseMatrix::Zero(rows, rows);
69 // DenseVector dv1 = DenseVector::Random(rows);
70 SparseMatrixType m2 (rows, depth);
71 SparseMatrixType m2t(depth, rows);
72 SparseMatrixType m3 (depth, cols);
73 SparseMatrixType m3t(cols, depth);
74 SparseMatrixType m4 (rows, cols);
75 SparseMatrixType m4t(cols, rows);
76 SparseMatrixType m6(rows, rows);
77 initSparse(density, refMat2, m2);
78 initSparse(density, refMat2t, m2t);
79 initSparse(density, refMat3, m3);
80 initSparse(density, refMat3t, m3t);
81 initSparse(density, refMat4, m4);
82 initSparse(density, refMat4t, m4t);
83 initSparse(density, refMat6, m6);
85 // int c = internal::random<int>(0,depth-1);
87 // sparse * sparse
88 VERIFY_IS_APPROX(m4=m2*m3, refMat4=refMat2*refMat3);
89 VERIFY_IS_APPROX(m4=m2t.transpose()*m3, refMat4=refMat2t.transpose()*refMat3);
90 VERIFY_IS_APPROX(m4=m2t.transpose()*m3t.transpose(), refMat4=refMat2t.transpose()*refMat3t.transpose());
91 VERIFY_IS_APPROX(m4=m2*m3t.transpose(), refMat4=refMat2*refMat3t.transpose());
93 VERIFY_IS_APPROX(m4 = m2*m3/s1, refMat4 = refMat2*refMat3/s1);
94 VERIFY_IS_APPROX(m4 = m2*m3*s1, refMat4 = refMat2*refMat3*s1);
95 VERIFY_IS_APPROX(m4 = s2*m2*m3*s1, refMat4 = s2*refMat2*refMat3*s1);
97 VERIFY_IS_APPROX(m4=(m2*m3).pruned(0), refMat4=refMat2*refMat3);
98 VERIFY_IS_APPROX(m4=(m2t.transpose()*m3).pruned(0), refMat4=refMat2t.transpose()*refMat3);
99 VERIFY_IS_APPROX(m4=(m2t.transpose()*m3t.transpose()).pruned(0), refMat4=refMat2t.transpose()*refMat3t.transpose());
100 VERIFY_IS_APPROX(m4=(m2*m3t.transpose()).pruned(0), refMat4=refMat2*refMat3t.transpose());
102 // test aliasing
103 m4 = m2; refMat4 = refMat2;
104 VERIFY_IS_APPROX(m4=m4*m3, refMat4=refMat4*refMat3);
106 // sparse * dense
107 VERIFY_IS_APPROX(dm4=m2*refMat3, refMat4=refMat2*refMat3);
108 VERIFY_IS_APPROX(dm4=m2*refMat3t.transpose(), refMat4=refMat2*refMat3t.transpose());
109 VERIFY_IS_APPROX(dm4=m2t.transpose()*refMat3, refMat4=refMat2t.transpose()*refMat3);
110 VERIFY_IS_APPROX(dm4=m2t.transpose()*refMat3t.transpose(), refMat4=refMat2t.transpose()*refMat3t.transpose());
112 VERIFY_IS_APPROX(dm4=m2*(refMat3+refMat3), refMat4=refMat2*(refMat3+refMat3));
113 VERIFY_IS_APPROX(dm4=m2t.transpose()*(refMat3+refMat5)*0.5, refMat4=refMat2t.transpose()*(refMat3+refMat5)*0.5);
115 // dense * sparse
116 VERIFY_IS_APPROX(dm4=refMat2*m3, refMat4=refMat2*refMat3);
117 VERIFY_IS_APPROX(dm4=refMat2*m3t.transpose(), refMat4=refMat2*refMat3t.transpose());
118 VERIFY_IS_APPROX(dm4=refMat2t.transpose()*m3, refMat4=refMat2t.transpose()*refMat3);
119 VERIFY_IS_APPROX(dm4=refMat2t.transpose()*m3t.transpose(), refMat4=refMat2t.transpose()*refMat3t.transpose());
121 // sparse * dense and dense * sparse outer product
122 test_outer<SparseMatrixType,DenseMatrix>::run(m2,m4,refMat2,refMat4);
124 VERIFY_IS_APPROX(m6=m6*m6, refMat6=refMat6*refMat6);
126 // sparse matrix * sparse vector
127 ColSpVector cv0(cols), cv1;
128 DenseVector dcv0(cols), dcv1;
129 initSparse(2*density,dcv0, cv0);
131 RowSpVector rv0(depth), rv1;
132 RowDenseVector drv0(depth), drv1(rv1);
133 initSparse(2*density,drv0, rv0);
135 VERIFY_IS_APPROX(cv1=rv0*m3, dcv1=drv0*refMat3);
136 VERIFY_IS_APPROX(rv1=rv0*m3, drv1=drv0*refMat3);
137 VERIFY_IS_APPROX(cv1=m3*cv0, dcv1=refMat3*dcv0);
138 VERIFY_IS_APPROX(cv1=m3t.adjoint()*cv0, dcv1=refMat3t.adjoint()*dcv0);
139 VERIFY_IS_APPROX(rv1=m3*cv0, drv1=refMat3*dcv0);
142 // test matrix - diagonal product
144 DenseMatrix refM2 = DenseMatrix::Zero(rows, cols);
145 DenseMatrix refM3 = DenseMatrix::Zero(rows, cols);
146 DenseMatrix d3 = DenseMatrix::Zero(rows, cols);
147 DiagonalMatrix<Scalar,Dynamic> d1(DenseVector::Random(cols));
148 DiagonalMatrix<Scalar,Dynamic> d2(DenseVector::Random(rows));
149 SparseMatrixType m2(rows, cols);
150 SparseMatrixType m3(rows, cols);
151 initSparse<Scalar>(density, refM2, m2);
152 initSparse<Scalar>(density, refM3, m3);
153 VERIFY_IS_APPROX(m3=m2*d1, refM3=refM2*d1);
154 VERIFY_IS_APPROX(m3=m2.transpose()*d2, refM3=refM2.transpose()*d2);
155 VERIFY_IS_APPROX(m3=d2*m2, refM3=d2*refM2);
156 VERIFY_IS_APPROX(m3=d1*m2.transpose(), refM3=d1*refM2.transpose());
158 // also check with a SparseWrapper:
159 DenseVector v1 = DenseVector::Random(cols);
160 DenseVector v2 = DenseVector::Random(rows);
161 VERIFY_IS_APPROX(m3=m2*v1.asDiagonal(), refM3=refM2*v1.asDiagonal());
162 VERIFY_IS_APPROX(m3=m2.transpose()*v2.asDiagonal(), refM3=refM2.transpose()*v2.asDiagonal());
163 VERIFY_IS_APPROX(m3=v2.asDiagonal()*m2, refM3=v2.asDiagonal()*refM2);
164 VERIFY_IS_APPROX(m3=v1.asDiagonal()*m2.transpose(), refM3=v1.asDiagonal()*refM2.transpose());
166 VERIFY_IS_APPROX(m3=v2.asDiagonal()*m2*v1.asDiagonal(), refM3=v2.asDiagonal()*refM2*v1.asDiagonal());
168 // evaluate to a dense matrix to check the .row() and .col() iterator functions
169 VERIFY_IS_APPROX(d3=m2*d1, refM3=refM2*d1);
170 VERIFY_IS_APPROX(d3=m2.transpose()*d2, refM3=refM2.transpose()*d2);
171 VERIFY_IS_APPROX(d3=d2*m2, refM3=d2*refM2);
172 VERIFY_IS_APPROX(d3=d1*m2.transpose(), refM3=d1*refM2.transpose());
175 // test self adjoint products
177 DenseMatrix b = DenseMatrix::Random(rows, rows);
178 DenseMatrix x = DenseMatrix::Random(rows, rows);
179 DenseMatrix refX = DenseMatrix::Random(rows, rows);
180 DenseMatrix refUp = DenseMatrix::Zero(rows, rows);
181 DenseMatrix refLo = DenseMatrix::Zero(rows, rows);
182 DenseMatrix refS = DenseMatrix::Zero(rows, rows);
183 SparseMatrixType mUp(rows, rows);
184 SparseMatrixType mLo(rows, rows);
185 SparseMatrixType mS(rows, rows);
186 do {
187 initSparse<Scalar>(density, refUp, mUp, ForceRealDiag|/*ForceNonZeroDiag|*/MakeUpperTriangular);
188 } while (refUp.isZero());
189 refLo = refUp.adjoint();
190 mLo = mUp.adjoint();
191 refS = refUp + refLo;
192 refS.diagonal() *= 0.5;
193 mS = mUp + mLo;
194 // TODO be able to address the diagonal....
195 for (int k=0; k<mS.outerSize(); ++k)
196 for (typename SparseMatrixType::InnerIterator it(mS,k); it; ++it)
197 if (it.index() == k)
198 it.valueRef() *= 0.5;
200 VERIFY_IS_APPROX(refS.adjoint(), refS);
201 VERIFY_IS_APPROX(mS.adjoint(), mS);
202 VERIFY_IS_APPROX(mS, refS);
203 VERIFY_IS_APPROX(x=mS*b, refX=refS*b);
205 VERIFY_IS_APPROX(x=mUp.template selfadjointView<Upper>()*b, refX=refS*b);
206 VERIFY_IS_APPROX(x=mLo.template selfadjointView<Lower>()*b, refX=refS*b);
207 VERIFY_IS_APPROX(x=mS.template selfadjointView<Upper|Lower>()*b, refX=refS*b);
209 // sparse selfadjointView * sparse
210 SparseMatrixType mSres(rows,rows);
211 VERIFY_IS_APPROX(mSres = mLo.template selfadjointView<Lower>()*mS,
212 refX = refLo.template selfadjointView<Lower>()*refS);
213 // sparse * sparse selfadjointview
214 VERIFY_IS_APPROX(mSres = mS * mLo.template selfadjointView<Lower>(),
215 refX = refS * refLo.template selfadjointView<Lower>());
220 // New test for Bug in SparseTimeDenseProduct
221 template<typename SparseMatrixType, typename DenseMatrixType> void sparse_product_regression_test()
223 // This code does not compile with afflicted versions of the bug
224 SparseMatrixType sm1(3,2);
225 DenseMatrixType m2(2,2);
226 sm1.setZero();
227 m2.setZero();
229 DenseMatrixType m3 = sm1*m2;
232 // This code produces a segfault with afflicted versions of another SparseTimeDenseProduct
233 // bug
235 SparseMatrixType sm2(20000,2);
236 sm2.setZero();
237 DenseMatrixType m4(sm2*m2);
239 VERIFY_IS_APPROX( m4(0,0), 0.0 );
242 void test_sparse_product()
244 for(int i = 0; i < g_repeat; i++) {
245 CALL_SUBTEST_1( (sparse_product<SparseMatrix<double,ColMajor> >()) );
246 CALL_SUBTEST_1( (sparse_product<SparseMatrix<double,RowMajor> >()) );
247 CALL_SUBTEST_2( (sparse_product<SparseMatrix<std::complex<double>, ColMajor > >()) );
248 CALL_SUBTEST_2( (sparse_product<SparseMatrix<std::complex<double>, RowMajor > >()) );
249 CALL_SUBTEST_3( (sparse_product<SparseMatrix<float,ColMajor,long int> >()) );
250 CALL_SUBTEST_4( (sparse_product_regression_test<SparseMatrix<double,RowMajor>, Matrix<double, Dynamic, Dynamic, RowMajor> >()) );