ADD: new track message, Entity class and Position class

2022-12-20 17:20:35 +01:00
parent 469ecfb099
commit 98ebb563a8
2114 changed files with 482360 additions and 24 deletions
--- a/libs/eigen/test/sparseqr.cpp
+++ b/libs/eigen/test/sparseqr.cpp
@@ -0,0 +1,149 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Desire Nuentsa Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+#include "sparse.h"
+#include <Eigen/SparseQR>
+
+template<typename MatrixType,typename DenseMat>
+int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150)
+{
+  eigen_assert(maxRows >= maxCols);
+  typedef typename MatrixType::Scalar Scalar;
+  int rows = internal::random<int>(1,maxRows);
+  int cols = internal::random<int>(1,maxCols);
+  double density = (std::max)(8./(rows*cols), 0.01);
+  
+  A.resize(rows,cols);
+  dA.resize(rows,cols);
+  initSparse<Scalar>(density, dA, A,ForceNonZeroDiag);
+  A.makeCompressed();
+  int nop = internal::random<int>(0, internal::random<double>(0,1) > 0.5 ? cols/2 : 0);
+  for(int k=0; k<nop; ++k)
+  {
+    int j0 = internal::random<int>(0,cols-1);
+    int j1 = internal::random<int>(0,cols-1);
+    Scalar s = internal::random<Scalar>();
+    A.col(j0)  = s * A.col(j1);
+    dA.col(j0) = s * dA.col(j1);
+  }
+  
+//   if(rows<cols) {
+//     A.conservativeResize(cols,cols);
+//     dA.conservativeResize(cols,cols);
+//     dA.bottomRows(cols-rows).setZero();
+//   }
+  
+  return rows;
+}
+
+template<typename Scalar> void test_sparseqr_scalar()
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef SparseMatrix<Scalar,ColMajor> MatrixType; 
+  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMat;
+  typedef Matrix<Scalar,Dynamic,1> DenseVector;
+  MatrixType A;
+  DenseMat dA;
+  DenseVector refX,x,b; 
+  SparseQR<MatrixType, COLAMDOrdering<int> > solver; 
+  generate_sparse_rectangular_problem(A,dA);
+  
+  b = dA * DenseVector::Random(A.cols());
+  solver.compute(A);
+
+  // Q should be MxM
+  VERIFY_IS_EQUAL(solver.matrixQ().rows(), A.rows());
+  VERIFY_IS_EQUAL(solver.matrixQ().cols(), A.rows());
+
+  // R should be MxN
+  VERIFY_IS_EQUAL(solver.matrixR().rows(), A.rows());
+  VERIFY_IS_EQUAL(solver.matrixR().cols(), A.cols());
+
+  // Q and R can be multiplied
+  DenseMat recoveredA = solver.matrixQ()
+                      * DenseMat(solver.matrixR().template triangularView<Upper>())
+                      * solver.colsPermutation().transpose();
+  VERIFY_IS_EQUAL(recoveredA.rows(), A.rows());
+  VERIFY_IS_EQUAL(recoveredA.cols(), A.cols());
+
+  // and in the full rank case the original matrix is recovered
+  if (solver.rank() == A.cols())
+  {
+      VERIFY_IS_APPROX(A, recoveredA);
+  }
+
+  if(internal::random<float>(0,1)>0.5f)
+    solver.factorize(A);  // this checks that calling analyzePattern is not needed if the pattern do not change.
+  if (solver.info() != Success)
+  {
+    std::cerr << "sparse QR factorization failed\n";
+    exit(0);
+    return;
+  }
+  x = solver.solve(b);
+  if (solver.info() != Success)
+  {
+    std::cerr << "sparse QR factorization failed\n";
+    exit(0);
+    return;
+  }
+
+  // Compare with a dense QR solver
+  ColPivHouseholderQR<DenseMat> dqr(dA);
+  refX = dqr.solve(b);
+  
+  bool rank_deficient = A.cols()>A.rows() || dqr.rank()<A.cols();
+  if(rank_deficient)
+  {
+    // rank deficient problem -> we might have to increase the threshold
+    // to get a correct solution.
+    RealScalar th = RealScalar(20)*dA.colwise().norm().maxCoeff()*(A.rows()+A.cols()) * NumTraits<RealScalar>::epsilon();
+    for(Index k=0; (k<16) && !test_isApprox(A*x,b); ++k)
+    {
+      th *= RealScalar(10);
+      solver.setPivotThreshold(th);
+      solver.compute(A);
+      x = solver.solve(b);
+    }
+  }
+
+  VERIFY_IS_APPROX(A * x, b);
+  
+  // For rank deficient problem, the estimated rank might
+  // be slightly off, so let's only raise a warning in such cases.
+  if(rank_deficient) ++g_test_level;
+  VERIFY_IS_EQUAL(solver.rank(), dqr.rank());
+  if(rank_deficient) --g_test_level;
+
+  if(solver.rank()==A.cols()) // full rank
+    VERIFY_IS_APPROX(x, refX);
+//   else
+//     VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );
+
+  // Compute explicitly the matrix Q
+  MatrixType Q, QtQ, idM;
+  Q = solver.matrixQ();
+  //Check  ||Q' * Q - I ||
+  QtQ = Q * Q.adjoint();
+  idM.resize(Q.rows(), Q.rows()); idM.setIdentity();
+  VERIFY(idM.isApprox(QtQ));
+  
+  // Q to dense
+  DenseMat dQ;
+  dQ = solver.matrixQ();
+  VERIFY_IS_APPROX(Q, dQ);
+}
+EIGEN_DECLARE_TEST(sparseqr)
+{
+  for(int i=0; i<g_repeat; ++i)
+  {
+    CALL_SUBTEST_1(test_sparseqr_scalar<double>());
+    CALL_SUBTEST_2(test_sparseqr_scalar<std::complex<double> >());
+  }
+}
+