├── .gitignore
├── README.md
├── truncated_svd_solver
    ├── src
    │   ├── tsvd-solver-options.cc
    │   ├── timing.cc
    │   ├── marginalization.cc
    │   ├── tsvd-solver.cc
    │   └── linear-algebra-helpers.cc
    ├── CMakeLists.txt
    ├── package.xml
    ├── include
    │   └── truncated-svd-solver
    │   │   ├── tsvd-solver-options.h
    │   │   ├── cholmod-helpers.h
    │   │   ├── marginalization.h
    │   │   ├── internal
    │   │       └── linear-algebra-helpers-inl.h
    │   │   ├── timing.h
    │   │   ├── tsvd-solver.h
    │   │   └── linear-algebra-helpers.h
    └── test
    │   ├── test-marginalization.cc
    │   └── test-tsvd-solver.cc
└── LICENSE


/.gitignore:
--------------------------------------------------------------------------------
1 | .cproject
2 | .project
3 | .settings
4 | 
5 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # truncated_svd_solver
2 | Truncated QR/SVD linear solver as described in http://e-collection.library.ethz.ch/eserv/eth:14434/eth-14434-02.pdf.
3 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/src/tsvd-solver-options.cc:
--------------------------------------------------------------------------------
 1 | #include "truncated-svd-solver/tsvd-solver-options.h"
 2 | 
 3 | #include <limits>
 4 | 
 5 | namespace truncated_svd_solver {
 6 | 
 7 | TruncatedSvdSolverOptions::TruncatedSvdSolverOptions()
 8 |     : columnScaling(false),
 9 |       epsNorm(std::numeric_limits<double>::epsilon()),
10 |       epsSVD(std::numeric_limits<double>::epsilon()),
11 |       epsQR(std::numeric_limits<double>::epsilon()),
12 |       svdTol(-1.0),
13 |       qrTol(-1.0),
14 |       verbose(false) {}
15 | 
16 | }  // namespace truncated_svd_solver
17 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(truncated_svd_solver)
 3 | 
 4 | find_package(catkin_simple REQUIRED)
 5 | catkin_simple()
 6 | 
 7 | add_definitions("-std=c++11")
 8 | 
 9 | ###########
10 | # LIBRARY #
11 | ###########
12 | cs_add_library(${PROJECT_NAME}
13 |   src/linear-algebra-helpers.cc
14 |   src/marginalization.cc
15 |   src/timing.cc
16 |   src/tsvd-solver.cc
17 |   src/tsvd-solver-options.cc
18 | )
19 | target_link_libraries(${PROJECT_NAME} pthread ${TBB_LIBRARIES})
20 | 
21 | #########
22 | # TESTS #
23 | #########
24 | catkin_add_gtest(test_svd_solver
25 |   test/test-tsvd-solver.cc
26 | )
27 | target_link_libraries(test_svd_solver ${PROJECT_NAME})
28 | 
29 | catkin_add_gtest(test_marginalization
30 |   test/test-marginalization.cc
31 | )
32 | target_link_libraries(test_marginalization ${PROJECT_NAME})
33 | 
34 | ###########
35 | # EXPORTS #
36 | ###########
37 | cs_install()
38 | cs_export()
39 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="utf-8"?>
 2 | <package format="2">
 3 |   <name>truncated_svd_solver</name>
 4 |   <version>0.0.1</version>
 5 |   <description>Truncated-SVD linear solver:
 6 |    It uses a combination of SP-QR and SVD.
 7 |    The right part of the input matrix can be marginalized out and solved
 8 |    by SVD, while the rest of the variables are solved with SPQR.
 9 |   </description>
10 |   
11 |   <license>BSD</license>
12 |   <url type="website">https://github.com/ethz-asl/truncated_svd_solver</url>
13 |   <author email="jerome.maye@mavt.ethz.ch">Jerome Maye</author>
14 |   <maintainer email="schneith@ethz.ch">Thomas Schneider</maintainer>
15 | 
16 |   <buildtool_depend>catkin</buildtool_depend>
17 |   <buildtool_depend>catkin_simple</buildtool_depend>
18 | 
19 |   <depend>eigen_catkin</depend>
20 |   <depend>eigen_checks</depend>
21 |   <depend>glog_catkin</depend>
22 |   <depend>suitesparse</depend>
23 |   <depend>TBB</depend>
24 | </package>
25 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/include/truncated-svd-solver/tsvd-solver-options.h:
--------------------------------------------------------------------------------
 1 | #ifndef TRUNCATED_SVD_SOLVER_TSVD_SOLVER_OPTIONS_H
 2 | #define TRUNCATED_SVD_SOLVER_TSVD_SOLVER_OPTIONS_H
 3 | 
 4 | namespace truncated_svd_solver {
 5 | 
 6 | /** The structure LinearSolverOptions defines the options for the
 7 |     LinearSolver class.
 8 |     \brief Linear solver options
 9 |   */
10 | struct TruncatedSvdSolverOptions {
11 |   TruncatedSvdSolverOptions();
12 | 
13 |   /// Perform column scaling/normalization
14 |   bool columnScaling;
15 |   /// Epsilon for when to consider an element being zero in the norm
16 |   double epsNorm;
17 |   /// Epsilon for SVD numerical rank
18 |   double epsSVD;
19 |   /// Epsilon for QR tolerance computation
20 |   double epsQR;
21 |   /// Fixed tolerance for SVD numerical rank
22 |   double svdTol;
23 |   /// Fixed tolerance for QR
24 |   double qrTol;
25 |   /// Verbose mode
26 |   bool verbose;
27 | };
28 | 
29 | }  // namespace truncated_svd_solver
30 | 
31 | #endif // TRUNCATED_SVD_SOLVER_TSVD_SOLVER_OPTIONS_H
32 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/include/truncated-svd-solver/cholmod-helpers.h:
--------------------------------------------------------------------------------
 1 | #ifndef TRUNCATED_SVD_SOLVER_CHOLMOD_HELPERS_H
 2 | #define TRUNCATED_SVD_SOLVER_CHOLMOD_HELPERS_H
 3 | 
 4 | #include <vector>
 5 | 
 6 | #include <Eigen/CholmodSupport>
 7 | #include <Eigen/Sparse>
 8 | 
 9 | namespace truncated_svd_solver {
10 | 
11 | inline void deleteCholdmodPtr(cholmod_sparse* & ptr,
12 |                               cholmod_common& cholmod) {
13 |   if (ptr) {
14 |     cholmod_l_free_sparse(&ptr, &cholmod);
15 |   }
16 | }
17 | inline void deleteCholdmodPtr(cholmod_dense* & ptr,
18 |                               cholmod_common& cholmod) {
19 |   if (ptr) {
20 |     cholmod_l_free_dense(&ptr, &cholmod);
21 |   }
22 | }
23 | 
24 | template<typename T>
25 | struct SelfFreeingCholmodPtr {
26 |   explicit SelfFreeingCholmodPtr(T* ptr,
27 |                                  cholmod_common& cholmod)
28 |       : cholmod_(cholmod),
29 |         ptr_(ptr) {}
30 | 
31 |   ~SelfFreeingCholmodPtr() {
32 |     reset(NULL);
33 |   }
34 | 
35 |   void reset(T* ptr = nullptr) {
36 |     deleteCholdmodPtr(ptr_, cholmod_);
37 |     ptr_ = ptr;
38 |   }
39 | 
40 |   SelfFreeingCholmodPtr & operator=(T* ptr) {
41 |     reset(ptr);
42 |     return *this;
43 |   }
44 | 
45 |   operator T*() {
46 |     return ptr_;
47 |   }
48 | 
49 |   T* operator->() {
50 |     return ptr_;
51 |   }
52 | 
53 |   T** operator&() {
54 |     return &ptr_;
55 |   }
56 | 
57 |  private:
58 |   cholmod_common & cholmod_;
59 |   T* ptr_;
60 | };
61 | 
62 | }  // namespace truncated_svd_solver
63 | 
64 | #endif // TRUNCATED_SVD_SOLVER_TSVD_SOLVER_H
65 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2016, Autonomous Systems Lab, ETH Zurich
 2 | All rights reserved.
 3 | 
 4 | Redistribution and use in source and binary forms, with or without
 5 | modification, are permitted provided that the following conditions are met:
 6 | 
 7 | 1. Redistributions of source code must retain the above copyright
 8 | notice, this list of conditions and the following disclaimer.
 9 | 
10 | 2. Redistributions in binary form must reproduce the above copyright
11 | notice, this list of conditions and the following disclaimer in the
12 | documentation and/or other materials provided with the distribution.
13 | 
14 | 3. Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
15 | names of its contributors may be used to endorse or promote products
16 | derived from this software without specific prior written permission.
17 | 
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/include/truncated-svd-solver/marginalization.h:
--------------------------------------------------------------------------------
 1 | #ifndef TRUNCATED_SVD_SOLVER_MARGINALIZATION_H
 2 | #define TRUNCATED_SVD_SOLVER_MARGINALIZATION_H
 3 | 
 4 | #include <cstdlib>
 5 | #include <cstddef>
 6 | 
 7 | #include <cholmod.h>
 8 | #include <Eigen/Core>
 9 | 
10 | namespace truncated_svd_solver {
11 | /**
12 |  * This function marginalizes variables from a sparse Jacobian. The Jacobian
13 |  * is assumed to be ordered in such a way that the variables to be
14 |  * marginalized are located to the right and start with index j.
15 |  * \brief Variables marginalization
16 |  *
17 |  * \param[in] Jt Jacobian transpose as outputted by linear solvers
18 |  * \param[in] j index from where to marginalize
19 |  * \param[in] normTol tolerance for a zero norm column
20 |  * \param[in] epsTol tolerance for SVD tolerance computation
21 |  * \param[out] NS null space of the marginalized system
22 |  * \param[out] CS column space of the marginalized system
23 |  * \param[out] Sigma covariance of the marginalized system
24 |  * \param[out] SigmaP projected covariance of the marginalized system
25 |  * \param[out] Omega marginalized Fisher information matrix
26 |  * \return sum of the log of the singular values of the marginalized system
27 |  */
28 | double marginalize(cholmod_sparse* Jt, size_t j, Eigen::MatrixXd& NS,
29 |                    Eigen::MatrixXd& CS, Eigen::MatrixXd& Sigma,
30 |                    Eigen::MatrixXd& SigmaP, Eigen::MatrixXd& Omega,
31 |                    double normTol = 1e-8, double epsTol = 1e-4);
32 | 
33 | /**
34 |  * This function returns the marginal Jacobian from two submatrices.
35 |  * \brief Marginal Jacobian recovery
36 |  *
37 |  * \return marginal Jacobian
38 |  * \param[in] J_x is the Jacobian containing the state variables
39 |  * \param[in] J_thetat is the tranposed Jacobian containing the calibration
40 |  *            variables
41 |  */
42 | Eigen::MatrixXd marginalJacobian(cholmod_sparse* J_x, cholmod_sparse*
43 |   J_thetat, cholmod_common* cholmod);
44 | 
45 | }  // namespace truncated_svd_solver
46 | 
47 | #endif // TRUNCATED_SVD_SOLVER_MARGINALIZATION_H
48 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/include/truncated-svd-solver/internal/linear-algebra-helpers-inl.h:
--------------------------------------------------------------------------------
 1 | #ifndef ASLAM_CALIBRATION_ALGORITHMS_LINALG_INL_H
 2 | #define ASLAM_CALIBRATION_ALGORITHMS_LINALG_INL_H
 3 | 
 4 | #include <cmath>
 5 | #include <cstddef>
 6 | #include <limits>
 7 | 
 8 | #include <Eigen/Core>
 9 | #include <glog/logging.h>
10 | 
11 | namespace truncated_svd_solver {
12 | 
13 | template <typename T>
14 | Eigen::MatrixXd computeCovariance(const T& R, size_t colBegin,
15 |     size_t colEnd) {
16 |   checkColumnIndices(R, colBegin, colEnd);
17 |   CHECK_LT(colBegin, colEnd);
18 |   CHECK_LT(colEnd, static_cast<size_t>(R.cols()));
19 | 
20 |   // NOTE: What about checking the form of R? Upper triangular matrix
21 |   const size_t numCols = R.cols();
22 |   const size_t dim = numCols - colBegin;
23 |   Eigen::MatrixXd covariance = Eigen::MatrixXd::Zero(dim, dim);
24 |   for (std::ptrdiff_t l = numCols - 1, Sigma_l = dim - 1;
25 |       l >= (std::ptrdiff_t)(numCols - dim); --l, --Sigma_l) {
26 |     double temp1 = 0;
27 |     for (std::ptrdiff_t j = l + 1, Sigma_j = Sigma_l + 1;
28 |         j < (std::ptrdiff_t)numCols; ++j, ++Sigma_j)
29 |       temp1 += R(l, j) * covariance(Sigma_j, Sigma_l);
30 |     const double R_ll = R(l, l);
31 |     covariance(Sigma_l, Sigma_l) = 1 / R_ll * (1 / R_ll - temp1);
32 |     for (std::ptrdiff_t i = l - 1, Sigma_i = Sigma_l - 1;
33 |         i >= std::ptrdiff_t(numCols - dim); --i, --Sigma_i) {
34 |       temp1 = 0;
35 |       for (std::ptrdiff_t j = i + 1, Sigma_j = Sigma_i + 1;
36 |           j <= l; ++j, ++Sigma_j)
37 |         temp1 += R(i, j) * covariance(Sigma_j, Sigma_l);
38 |       double temp2 = 0;
39 |       for (std::ptrdiff_t j = l + 1, Sigma_j = Sigma_l + 1;
40 |           j < (std::ptrdiff_t)numCols; ++j, ++Sigma_j)
41 |         temp2 += R(i, j) * covariance(Sigma_l, Sigma_j);
42 |       covariance(Sigma_i, Sigma_l) = 1 / R(i, i) * (-temp1 - temp2);
43 |       covariance(Sigma_l, Sigma_i) = covariance(Sigma_i, Sigma_l);
44 |     }
45 |   }
46 |   const size_t block_dim = colEnd - colBegin + 1;
47 |   return covariance.block(0, 0, block_dim, block_dim);
48 | }
49 | 
50 | }  // namespace truncated_svd_solver
51 | #endif // ASLAM_CALIBRATION_ALGORITHMS_LINALG_INL_H
52 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/test/test-marginalization.cc:
--------------------------------------------------------------------------------
 1 | #include <cmath>
 2 | #include <cstddef>
 3 | #include <vector>
 4 | 
 5 | #include <Eigen/Core>
 6 | #include <eigen-checks/gtest.h>
 7 | #include <gtest/gtest.h>
 8 | #include <SuiteSparseQR.hpp>
 9 | 
10 | #include "truncated-svd-solver/linear-algebra-helpers.h"
11 | #include "truncated-svd-solver/marginalization.h"
12 | 
13 | namespace truncated_svd_solver {
14 | 
15 | bool getR(cholmod_sparse* A, cholmod_sparse** R, cholmod_common* cholmod) {
16 |   CHECK_NOTNULL(A);
17 |   CHECK_NOTNULL(R);
18 |   CHECK_NOTNULL(cholmod);
19 | 
20 |   cholmod_sparse* qr_A = cholmod_l_transpose(A, 1, cholmod);
21 |   SuiteSparseQR<double>(SPQR_ORDERING_FIXED, SPQR_NO_TOL, qr_A->ncol, 0, qr_A,
22 |                         nullptr, nullptr, nullptr, nullptr, R, nullptr, nullptr,
23 |                         nullptr, nullptr, cholmod);
24 |   cholmod_l_free_sparse(&qr_A, cholmod);
25 |   return cholmod->status;
26 | }
27 | 
28 | TEST(TruncatedSvdSolver, Marginalization) {
29 |   cholmod_common cholmod;
30 |   cholmod_l_start(&cholmod);
31 | 
32 |   // Create a random jacobian.
33 |   Eigen::MatrixXd J = Eigen::MatrixXd::Random(5, 5);
34 |   Eigen::MatrixXd J_cov_expected = (J.transpose() * J).inverse();
35 | 
36 |   // Convert to cholmod sparse format.
37 |   cholmod_sparse* Jt_cholmod =
38 |       eigenDenseToCholmodSparseCopy(J.transpose(), &cholmod, 1e-16);
39 | 
40 |   // Test the results.
41 |   Eigen::MatrixXd NS, CS, Sigma, SigmaP, Omega;
42 |   const double svLogSum = marginalize(Jt_cholmod, 0, NS, CS, Sigma,
43 |     SigmaP, Omega);
44 | 
45 |   EXPECT_NEAR(std::fabs(svLogSum),
46 |               std::fabs(std::log2(std::fabs(J_cov_expected.determinant()))),
47 |               1e-8);
48 |   EXPECT_TRUE(EIGEN_MATRIX_NEAR(Sigma, J_cov_expected, 1e-12));
49 | 
50 |   cholmod_l_finish(&cholmod);
51 | }
52 | 
53 | }  // namespace truncated_svd_solver
54 | 
55 | int main(int argc, char** argv) {
56 |   ::testing::InitGoogleTest(&argc, argv);
57 |   google::InitGoogleLogging(argv[0]);
58 |   google::InstallFailureSignalHandler();\
59 |   ::testing::FLAGS_gtest_death_test_style = "threadsafe";
60 |   FLAGS_alsologtostderr = true;
61 |   FLAGS_colorlogtostderr = true;
62 |   return RUN_ALL_TESTS();
63 | }
64 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/include/truncated-svd-solver/timing.h:
--------------------------------------------------------------------------------
 1 | #ifndef TRUNCATED_SVD_SOLVER_TSVD_TIMING_H
 2 | #define TRUNCATED_SVD_SOLVER_TSVD_TIMING_H
 3 | 
 4 | #include <string>
 5 | 
 6 | #include <time.h>
 7 | 
 8 | namespace truncated_svd_solver {
 9 | 
10 | /** The class Timestamp implements timestamping facilities.
11 |     \brief Timestamping facilities
12 |   */
13 | class Timestamp {
14 |  public:
15 |   /// Constructs timestamp object from parameter
16 |   Timestamp(double seconds = now());
17 |   /// Copy constructor
18 |   Timestamp(const Timestamp& other);
19 |   /// Assignment operator
20 |   Timestamp& operator = (const Timestamp& other);
21 |   /// Destructor
22 |   virtual ~Timestamp();
23 | 
24 |   /// Access the timestamp's value in seconds
25 |   double getSeconds() const;
26 | 
27 |   /// Return the time in seconds from the timestamp
28 |   operator double() const;
29 |   /// Return the timespec object from the timestamp
30 |   operator timespec() const;
31 |   /// Equal comparison
32 |   bool operator == (const Timestamp& timestamp) const;
33 |   /// Equal comparison
34 |   bool operator == (double seconds) const;
35 |   /// Not equal comparison
36 |   bool operator != (const Timestamp& timestamp) const;
37 |   /// Not equal comparison
38 |   bool operator != (double seconds) const;
39 |   /// Bigger comparison
40 |   bool operator > (const Timestamp& timestamp) const;
41 |   /// Bigger comparison
42 |   bool operator > (double seconds) const;
43 |   /// Smaller comparison
44 |   bool operator < (const Timestamp& timestamp) const;
45 |   /// Smaller comparison
46 |   bool operator < (double seconds) const;
47 |   /// Bigger or equal comparison
48 |   bool operator >= (const Timestamp& timestamp) const;
49 |   /// Bigger or equal comparison
50 |   bool operator >= (double seconds) const;
51 |   /// Smaller or equal comparison
52 |   bool operator <= (const Timestamp& timestamp) const;
53 |   /// Smaller or equal comparison
54 |   bool operator <= (double seconds) const;
55 |   /// Add 2 timestamps
56 |   Timestamp& operator += (double seconds);
57 |   /// Substract 2 timestamps
58 |   Timestamp& operator -= (double seconds);
59 |   /// Add seconds to timestamp
60 |   double operator + (double seconds) const;
61 |   /// Add another timestamp
62 |   double operator + (const Timestamp& timestamp) const;
63 |   /// Substract another timestamp
64 |   double operator - (const Timestamp& timestamp) const;
65 |   /// Substract seconds
66 |   double operator - (double seconds) const;
67 |   /// Returns the system time in s
68 |   static double now();
69 |   /// Returns the date of the system in string
70 |   static std::string getDate();
71 |   /// Returns the date from timestamp in seconds
72 |   static std::string getDate(double seconds);
73 | 
74 | 
75 |  private:
76 |   /// Seconds in the timestamp
77 |   double seconds_;
78 | };
79 | 
80 | }  // namespace truncated_svd_solver
81 | 
82 | #endif // TRUNCATED_SVD_SOLVER_TSVD_TIMING_H
83 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/src/timing.cc:
--------------------------------------------------------------------------------
  1 | #include "truncated-svd-solver/timing.h"
  2 | 
  3 | #include <sys/time.h>
  4 | 
  5 | namespace truncated_svd_solver {
  6 | 
  7 | Timestamp::Timestamp(double seconds) :
  8 |     seconds_(seconds) {
  9 | }
 10 | 
 11 | Timestamp::Timestamp(const Timestamp& other) :
 12 |     seconds_(other.seconds_) {
 13 | }
 14 | 
 15 | Timestamp& Timestamp::operator = (const Timestamp& other) {
 16 |   if (this != &other) {
 17 |     seconds_ = other.seconds_;
 18 |   }
 19 |   return *this;
 20 | }
 21 | 
 22 | Timestamp::~Timestamp() {}
 23 | 
 24 | double Timestamp::getSeconds() const {
 25 |   return seconds_;
 26 | }
 27 | 
 28 | double Timestamp::now() {
 29 |   struct timeval time;
 30 |   gettimeofday(&time, 0);
 31 |   return time.tv_sec + time.tv_usec / 1e6;
 32 | }
 33 | 
 34 | std::string Timestamp::getDate() {
 35 |   struct timeval time;
 36 |   gettimeofday(&time, 0);
 37 |   struct tm* ptm;
 38 |   ptm = localtime(&time.tv_sec);
 39 |   char timeString[40];
 40 |   strftime(timeString, sizeof (timeString), "%Y-%m-%d %H:%M:%S", ptm);
 41 |   return std::string(timeString);
 42 | }
 43 | 
 44 | std::string Timestamp::getDate(double seconds) {
 45 |   struct timeval time;
 46 |   time.tv_sec = seconds;
 47 |   struct tm* ptm;
 48 |   ptm = localtime(&time.tv_sec);
 49 |   char timeString[40];
 50 |   strftime(timeString, sizeof (timeString), "%Y-%m-%d-%H-%M-%S", ptm);
 51 |   return std::string(timeString);
 52 | }
 53 | 
 54 | Timestamp::operator double() const {
 55 |   return seconds_;
 56 | }
 57 | 
 58 | Timestamp::operator timespec() const {
 59 |   timespec time;
 60 |   time.tv_sec = (time_t)seconds_;
 61 |   time.tv_nsec = (seconds_ - (time_t)seconds_) * 1e9;
 62 |   return time;
 63 | }
 64 | 
 65 | bool Timestamp::operator == (const Timestamp& timestamp) const {
 66 |   return (seconds_ == timestamp.seconds_);
 67 | }
 68 | 
 69 | bool Timestamp::operator == (double seconds) const {
 70 |   return (seconds_ == seconds);
 71 | }
 72 | 
 73 | bool Timestamp::operator != (const Timestamp& timestamp) const {
 74 |   return (seconds_ != timestamp.seconds_);
 75 | }
 76 | 
 77 | bool Timestamp::operator != (double seconds) const {
 78 |   return (seconds_ != seconds);
 79 | }
 80 | 
 81 | bool Timestamp::operator > (const Timestamp& timestamp) const {
 82 |   return (seconds_ > timestamp.seconds_);
 83 | }
 84 | 
 85 | bool Timestamp::operator > (double seconds) const {
 86 |   return (seconds_ > seconds);
 87 | }
 88 | 
 89 | bool Timestamp::operator < (const Timestamp& timestamp) const {
 90 |   return (seconds_ < timestamp.seconds_);
 91 | }
 92 | 
 93 | bool Timestamp::operator < (double seconds) const {
 94 |   return (seconds_ < seconds);
 95 | }
 96 | 
 97 | bool Timestamp::operator >= (const Timestamp& timestamp) const {
 98 |   return (seconds_ >= timestamp.seconds_);
 99 | }
100 | 
101 | bool Timestamp::operator >= (double seconds) const {
102 |   return (seconds_ >= seconds);
103 | }
104 | 
105 | bool Timestamp::operator <= (const Timestamp& timestamp) const {
106 |   return (seconds_ <= timestamp.seconds_);
107 | }
108 | 
109 | bool Timestamp::operator <= (double seconds) const {
110 |   return (seconds_ <= seconds);
111 | }
112 | 
113 | Timestamp& Timestamp::operator += (double seconds) {
114 |   seconds_ += seconds;
115 |   return *this;
116 | }
117 | 
118 | Timestamp& Timestamp::operator -= (double seconds) {
119 |   seconds_ -= seconds;
120 |   return *this;
121 | }
122 | 
123 | double Timestamp::operator + (double seconds) const {
124 |   return seconds_ + seconds;
125 | }
126 | 
127 | double Timestamp::operator + (const Timestamp& timestamp) const {
128 |   return seconds_ + timestamp.seconds_;
129 | }
130 | 
131 | double Timestamp::operator - (const Timestamp& timestamp) const {
132 |   return seconds_ - timestamp.seconds_;
133 | }
134 | 
135 | double Timestamp::operator - (double seconds) const {
136 |   return seconds_ - seconds;
137 | }
138 | 
139 | }  // namespace truncated_svd_solver
140 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/include/truncated-svd-solver/tsvd-solver.h:
--------------------------------------------------------------------------------
  1 | #ifndef TRUNCATED_SVD_SOLVER_TSVD_SOLVER_H
  2 | #define TRUNCATED_SVD_SOLVER_TSVD_SOLVER_H
  3 | 
  4 | #include <cstddef>
  5 | #include <string>
  6 | #include <vector>
  7 | 
  8 | #include <cholmod.h>
  9 | #include <Eigen/Core>
 10 | 
 11 | #include "truncated-svd-solver/tsvd-solver-options.h"
 12 | 
 13 | template <typename Entry> struct SuiteSparseQR_factorization;
 14 | 
 15 | namespace truncated_svd_solver {
 16 | 
 17 | /** The class LinearSolver implements a specific linear solver as a
 18 |     combination of SPQR and SVD.The right part of the input matrix can be
 19 |     marginalized-out and solved by SVD, while the rest of the variables are
 20 |     solved with SPQR.
 21 |     \brief Linear solver for incremental calibration
 22 |   */
 23 | class TruncatedSvdSolver {
 24 |  public:
 25 |   typedef TruncatedSvdSolverOptions Options;
 26 | 
 27 |   TruncatedSvdSolver(const Options& options = Options());
 28 |   TruncatedSvdSolver(const TruncatedSvdSolver& other) = delete;
 29 |   TruncatedSvdSolver& operator= (const TruncatedSvdSolver& other) = delete;
 30 |   TruncatedSvdSolver(TruncatedSvdSolver&& other) = delete;
 31 |   TruncatedSvdSolver& operator= (TruncatedSvdSolver&& other) = delete;
 32 |   virtual ~TruncatedSvdSolver();
 33 | 
 34 |   /**
 35 |    * This function solves a system of equation using marginalization.
 36 |    * \brief Marginalization solver
 37 |    *
 38 |    * \return void
 39 |    * \param[in] A sparse matrix left-hand side
 40 |    * \param[in] b dense vector right-hand side
 41 |    * \param[in] j starting column index for the marginalization
 42 |    * \param[out] x solution
 43 |    */
 44 |   void solve(cholmod_sparse* A, cholmod_dense* b, size_t j,
 45 |     Eigen::VectorXd& x);
 46 | 
 47 |   /**
 48 |    * This function analyzes the marginalized matrix at index j. The goal
 49 |    * is to estimate the numerical null and column space, the covariance, the
 50 |    * covariance on the column space. All the results are cached in the
 51 |    * solver.
 52 |    * \brief Marginalization analyzer
 53 |    *
 54 |    * \return void
 55 |    * \param[in] A sparse matrix left-hand side
 56 |    * \param[in] j starting column index for the marginalization
 57 |    */
 58 |   void analyzeMarginal(cholmod_sparse* A, size_t j);
 59 | 
 60 |   /// Clear the cached variables
 61 |   void clear();
 62 | 
 63 |   /// Returns the options
 64 |   const Options& getOptions() const;
 65 |   /// Returns the options
 66 |   Options& getOptions();
 67 |   /// Returns the current SVD rank
 68 |   std::ptrdiff_t getSVDRank() const;
 69 |   /// Returns the current SVD rank deficiency
 70 |   std::ptrdiff_t getSVDRankDeficiency() const;
 71 |   /// Returns the current gap in the singular values at the rank
 72 |   double getSvGap() const;
 73 |   /// Returns the current QR rank
 74 |   std::ptrdiff_t getQRRank() const;
 75 |   /// Returns the current QR rank deficiency
 76 |   std::ptrdiff_t getQRRankDeficiency() const;
 77 |   /// Returns the marginalization start index
 78 |   std::ptrdiff_t getMargStartIndex() const;
 79 |   /// Sets the marginalization start index
 80 |   void setMargStartIndex(std::ptrdiff_t index);
 81 |   /// Returns the current tolerance used by SPQR
 82 |   double getQRTolerance() const;
 83 |   /// Returns the current tolerance used by SVD
 84 |   double getSVDTolerance() const;
 85 |   /// Returns the current singular values
 86 |   const Eigen::VectorXd& getSingularValues() const;
 87 |   /// Returns the current left-singular vectors
 88 |   const Eigen::MatrixXd& getMatrixU() const;
 89 |   /// Returns the current right-singular vectors
 90 |   const Eigen::MatrixXd& getMatrixV() const;
 91 |   /// Returns the current null space for SVD
 92 |   Eigen::MatrixXd getNullSpace() const;
 93 |   /// Returns the current row space for SVD
 94 |   Eigen::MatrixXd getRowSpace() const;
 95 |   /// Returns the current covariance matrix for SVD
 96 |   Eigen::MatrixXd getCovariance() const;
 97 |   /// Returns the current covariance matrix on the row space for SVD
 98 |   Eigen::MatrixXd getRowSpaceCovariance() const;
 99 |   /// Returns the current log2 sum of the singular values
100 |   double getSingularValuesLog2Sum() const;
101 |   /// Returns the peak memory usage in bytes
102 |   size_t getPeakMemoryUsage() const;
103 |   /// Returns the current memory usage in bytes
104 |   size_t getMemoryUsage() const;
105 |   /// Returns the current number of flops
106 |   double getNumFlops() const;
107 |   /// Returns the current time for linear solving
108 |   double getLinearSolverTime() const;
109 |   /// Returns the current time for analyzing marginal
110 |   double getMarginalAnalysisTime() const;
111 |   /// Returns the current time for symbolic factorization
112 |   double getSymbolicFactorizationTime() const;
113 |   /// Returns the current time for numeric factorization
114 |   double getNumericFactorizationTime() const;
115 |   // TODO(schneith): we are missing here the Frobenius norm, available in
116 |   //                 SPQR 3.4
117 | 
118 |   /**
119 |    * Set cholmod SPQR threads.
120 |    * @param n number of threads. (-1 = chosen automatically)
121 |    */
122 |   void setNThreads(int n);
123 |  protected:
124 |   void clearSvdAnalysisResultMembers();
125 |   void analyzeSVD(cholmod_sparse * Omega);
126 | 
127 |   /// Linear solver options
128 |   Options tsvd_options_;
129 |   /// Cholmod common structure
130 |   cholmod_common cholmod_;
131 |   /// Caching current factorization if needed
132 |   SuiteSparseQR_factorization<double>* factor_;
133 |   /// Caching current estimated numerical rank for SVD
134 |   std::ptrdiff_t svdRank_;
135 |   /// Caching current gap in estimated singular values at the rank
136 |   double svGap_;
137 |   /// Caching current estimated numerical rank deficiency for SVD
138 |   std::ptrdiff_t svdRankDeficiency_;
139 |   /// Marginalization start index
140 |   std::ptrdiff_t margStartIndex_;
141 |   /// Caching current SVD tolerance
142 |   double svdTolerance_;
143 |   /// Caching current singular values
144 |   Eigen::VectorXd singularValues_;
145 |   /// Caching left-singular vectors
146 |   Eigen::MatrixXd matrixU_;
147 |   /// Caching right-singular vectors
148 |   Eigen::MatrixXd matrixV_;
149 |   /// Linear solver time
150 |   double linearSolverTime_;
151 |   /// Marginal analysis time
152 |   double marginalAnalysisTime_;
153 | };
154 | 
155 | }  // namespace truncated_svd_solver
156 | 
157 | #endif // TRUNCATED_SVD_SOLVER_TSVD_SOLVER_H
158 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/src/marginalization.cc:
--------------------------------------------------------------------------------
  1 | #include "truncated-svd-solver/marginalization.h"
  2 | 
  3 | #include <cmath>
  4 | 
  5 | #include <cholmod.h>
  6 | #include <Eigen/Dense>
  7 | #include <glog/logging.h>
  8 | #include <spqr.hpp>
  9 | #include <SuiteSparseQR.hpp>
 10 | 
 11 | #include "truncated-svd-solver/linear-algebra-helpers.h"
 12 | 
 13 | namespace truncated_svd_solver {
 14 | 
 15 | Eigen::MatrixXd marginalJacobian(cholmod_sparse* J_x, cholmod_sparse*
 16 |     J_thetat, cholmod_common* cholmod) {
 17 |   // Compute the QR factorization of J_x.
 18 |   SuiteSparseQR_factorization<double>* QR = SuiteSparseQR_factorize<double>(
 19 |     SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, J_x, cholmod);
 20 |   CHECK(QR != nullptr) << "SuiteSparseQR_factorize failed.";
 21 | 
 22 |   // Compute the Jacobian of the reduced system.
 23 |   cholmod_sparse* J_thetatQFull = SuiteSparseQR_qmult<double>(SPQR_XQ, QR,
 24 |     J_thetat, cholmod);
 25 |   CHECK(QR != nullptr) << "SuiteSparseQR_qmult failed.";
 26 | 
 27 |   std::ptrdiff_t* col_indices = new std::ptrdiff_t[J_x->ncol];
 28 |   for (size_t i = 0; i < J_x->ncol; ++i)
 29 |    col_indices[i] = i;
 30 |   cholmod_sparse* J_thetatQ = cholmod_l_submatrix(J_thetatQFull, NULL, -1,
 31 |     col_indices, J_x->ncol, 1, 0, cholmod);
 32 |   delete [] col_indices;
 33 |   CHECK(J_thetatQ != nullptr) << "cholmod_l_submatrix failed.";
 34 | 
 35 |   cholmod_sparse* J_thetat2 = cholmod_l_aat(J_thetat, NULL, 0, 1, cholmod);
 36 |   CHECK(J_thetat2 != nullptr) << "cholmod_l_aat failed.";
 37 | 
 38 |   cholmod_sparse* J_thetatQ2 = cholmod_l_aat(J_thetatQ, NULL, 0, 1,
 39 |     cholmod);
 40 |   CHECK(J_thetatQ2 != nullptr) << "cholmod_l_aat failed.";
 41 | 
 42 |   double alpha[2];
 43 |   alpha[0] = 1;
 44 |   double beta[2];
 45 |   beta[0] = -1;
 46 |   cholmod_sparse* Omega = cholmod_l_add(J_thetat2, J_thetatQ2, alpha, beta,
 47 |     1, 0, cholmod);
 48 |   CHECK(Omega != nullptr) << "cholmod_l_add failed.";
 49 | 
 50 |   Eigen::MatrixXd OmegaDense;
 51 |   cholmodSparseToEigenDenseCopy(Omega, OmegaDense);
 52 | 
 53 |   // Clean allocated memory.
 54 |   SuiteSparseQR_free(&QR, cholmod);
 55 |   cholmod_l_free_sparse(&J_thetatQFull, cholmod);
 56 |   cholmod_l_free_sparse(&J_thetatQ, cholmod);
 57 |   cholmod_l_free_sparse(&J_thetat2, cholmod);
 58 |   cholmod_l_free_sparse(&J_thetatQ2, cholmod);
 59 |   cholmod_l_free_sparse(&Omega, cholmod);
 60 | 
 61 |   return OmegaDense;
 62 | }
 63 | 
 64 | double marginalize(cholmod_sparse* Jt, size_t j, Eigen::MatrixXd& NS,
 65 |                    Eigen::MatrixXd& CS, Eigen::MatrixXd& Sigma,
 66 |                    Eigen::MatrixXd& SigmaP, Eigen::MatrixXd& Omega,
 67 |                    double norm_tol, double eps_tol) {
 68 |   CHECK_NOTNULL(Jt);
 69 | 
 70 |   // Initialize cholmod.
 71 |   cholmod_common cholmod;
 72 |   cholmod_l_start(&cholmod);
 73 | 
 74 |   // Convert to cholmod_sparse.
 75 |   cholmod_sparse* J = cholmod_l_transpose(Jt, 1, &cholmod);
 76 |   CHECK(J != nullptr) << "cholmod_l_transpose failed.";
 77 | 
 78 |   // Extract the part corresponding to the state/landmarks/...
 79 |   std::ptrdiff_t* col_indices = new std::ptrdiff_t[j];
 80 |   for (size_t i = 0; i < j; ++i)
 81 |    col_indices[i] = i;
 82 |   cholmod_sparse* J_x = cholmod_l_submatrix(J, NULL, -1, col_indices, j, 1,
 83 |     0, &cholmod);
 84 |   delete [] col_indices;
 85 |   CHECK(J_x != nullptr) << "cholmod_l_submatrix failed.";
 86 | 
 87 |   // Extract the part corresponding to the calibration parameters.
 88 |   col_indices = new std::ptrdiff_t[J->ncol - j];
 89 |   for (size_t i = j; i < J->ncol; ++i)
 90 |    col_indices[i - j] = i;
 91 |   cholmod_sparse* J_theta = cholmod_l_submatrix(J, NULL, -1, col_indices,
 92 |     J->ncol - j, 1, 0, &cholmod);
 93 |   delete [] col_indices;
 94 |   CHECK(J_theta != nullptr) << "cholmod_l_submatrix failed.";
 95 | 
 96 |   cholmod_sparse* J_thetat = cholmod_l_transpose(J_theta, 1, &cholmod);
 97 |   CHECK(J_thetat != nullptr) << "cholmod_l_transpose failed.";
 98 | 
 99 |   // Compute the marginal Jacobian.
100 |   Omega = marginalJacobian(J_x, J_thetat, &cholmod);
101 | 
102 |   // Scale J_x.
103 |   cholmod_dense* G_x = cholmod_l_allocate_dense(J_x->ncol, 1, J_x->ncol,
104 |     CHOLMOD_REAL, &cholmod);
105 |   CHECK(G_x != nullptr) << "cholmod_l_allocate_dense failed.";
106 |   double* values = reinterpret_cast<double*>(G_x->x);
107 | 
108 |   for (size_t j = 0; j < J_x->ncol; ++j) {
109 |     const double norm_col = colNorm(J_x, j);
110 |     if (norm_col < norm_tol)
111 |       values[j] = 0.0;
112 |     else
113 |       values[j] = 1.0 / norm_col;
114 |   }
115 | 
116 |   bool success = cholmod_l_scale(G_x, CHOLMOD_COL, J_x, &cholmod);
117 |   CHECK(success) << "cholmod_l_scale failed.";
118 |   cholmod_l_free_dense(&G_x, &cholmod) ;
119 | 
120 |   // Scale J_thetat.
121 |   cholmod_dense* G_theta = cholmod_l_allocate_dense(J_theta->ncol, 1,
122 |     J_theta->ncol, CHOLMOD_REAL, &cholmod);
123 |   CHECK(G_theta != nullptr) << "cholmod_l_allocate_dense failed.";
124 |   values = reinterpret_cast<double*>(G_theta->x);
125 | 
126 |   for (size_t j = 0; j < J_theta->ncol; ++j) {
127 |     const double normCol = colNorm(J_theta, j);
128 |     if (normCol < norm_tol)
129 |       values[j] = 0.0;
130 |     else
131 |       values[j] = 1.0 / normCol;
132 |   }
133 | 
134 |   success = cholmod_l_scale(G_theta, CHOLMOD_ROW, J_thetat, &cholmod);
135 |   CHECK(success) << "cholmod_l_scale failed.";
136 |   cholmod_l_free_dense(&G_theta, &cholmod);
137 | 
138 |   // Compute the scaled marginal Jacobian.
139 |   Eigen::MatrixXd OmegaScaled;
140 |   OmegaScaled = marginalJacobian(J_x, J_thetat, &cholmod);
141 | 
142 |   // Cleanup cholmod.
143 |   cholmod_l_free_sparse(&J, &cholmod);
144 |   cholmod_l_free_sparse(&J_x, &cholmod);
145 |   cholmod_l_free_sparse(&J_theta, &cholmod);
146 |   cholmod_l_free_sparse(&J_thetat, &cholmod);
147 |   cholmod_l_finish(&cholmod);
148 | 
149 |   // Compute the thin SVD of OmegaScaled.
150 |   const Eigen::JacobiSVD<Eigen::MatrixXd> svd_scaled(OmegaScaled,
151 |     Eigen::ComputeThinU | Eigen::ComputeThinV);
152 | 
153 |   // Compute the numerical rank.
154 |   size_t nrank = OmegaScaled.cols();
155 |   const Eigen::VectorXd& SScaled = svd_scaled.singularValues();
156 |   const double tol = OmegaScaled.rows() * SScaled(0) * eps_tol;
157 |   for (std::ptrdiff_t i = OmegaScaled.cols() - 1; i > 0; --i) {
158 |     if (SScaled(i) > tol) {
159 |       break;
160 |     } else {
161 |       nrank--;
162 |     }
163 |   }
164 | 
165 |   // Compute the thin SVD of Omega.
166 |   const Eigen::JacobiSVD<Eigen::MatrixXd> svd(Omega,
167 |     Eigen::ComputeThinU | Eigen::ComputeThinV);
168 |   const Eigen::MatrixXd& V = svd.matrixV();
169 | 
170 |   // compute the numerical column space
171 |   CS = V.block(0, 0, V.rows(), nrank);
172 | 
173 |   // compute the numerical null space
174 |   NS = V.block(0, nrank, V.rows(), Omega.cols() - nrank);
175 | 
176 |   // compute the projected covariance matrix
177 |   Eigen::MatrixXd inv_S(Eigen::MatrixXd::Zero(Omega.cols(), Omega.cols()));
178 |   SigmaP = Eigen::MatrixXd::Zero(nrank, nrank);
179 |   double sv_log_sum = 0.0;
180 |   const Eigen::VectorXd& S = svd.singularValues();
181 |   for (size_t i = 0u; i < nrank; ++i) {
182 |     SigmaP(i, i) = 1.0 / S(i);
183 |     sv_log_sum = sv_log_sum + log2(S(i));
184 |     inv_S(i, i) = SigmaP(i, i);
185 |   }
186 | 
187 |   // compute the covariance matrix
188 |   Sigma = V * inv_S * V.transpose();
189 |   return sv_log_sum;
190 | }
191 | 
192 | }  // namespace truncated_svd_solver
193 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/include/truncated-svd-solver/linear-algebra-helpers.h:
--------------------------------------------------------------------------------
  1 | #ifndef ASLAM_CALIBRATION_ALGORITHMS_LINALG_H
  2 | #define ASLAM_CALIBRATION_ALGORITHMS_LINALG_H
  3 | 
  4 | #include <cstddef>
  5 | #include <limits>
  6 | 
  7 | #include <Eigen/Core>
  8 | 
  9 | struct cholmod_sparse_struct;
 10 | typedef cholmod_sparse_struct cholmod_sparse;
 11 | struct cholmod_dense_struct;
 12 | typedef cholmod_dense_struct cholmod_dense;
 13 | struct cholmod_common_struct;
 14 | typedef cholmod_common_struct cholmod_common;
 15 | template <typename Entry> struct SuiteSparseQR_factorization;
 16 | 
 17 | namespace truncated_svd_solver {
 18 | 
 19 | /**
 20 |  * This function computes a covariance block from the R matrix of a QR
 21 |  * factorization.
 22 |  * \brief Covariance block recovery
 23 |  *
 24 |  * \param[in] R upper triangular R matrix
 25 |  * \param[in] colBegin column index where to start recovery
 26 |  * \param[in] colEnd column index where to stop recovery
 27 |  * \return covariance block
 28 |  */
 29 | template <typename T>
 30 | Eigen::MatrixXd computeCovariance(const T& R, size_t colBegin,
 31 |                                   size_t colEnd);
 32 | 
 33 | /**
 34 |  * This function extracts a submatrix based on column indices from a sparse
 35 |  * matrix.
 36 |  * \brief Submatrix column extraction
 37 |  *
 38 |  * \return column submatrix pointer (owned by caller)
 39 |  * \param[in] A sparse matrix
 40 |  * \param[in] colStartIdx starting column index
 41 |  * \param[in] colEndIdx ending column index
 42 |  * \param[in] cholmod cholmod common structure
 43 |  */
 44 | cholmod_sparse* columnSubmatrix(cholmod_sparse* A, std::ptrdiff_t
 45 |   colStartIdx, std::ptrdiff_t colEndIdx, cholmod_common* cholmod);
 46 | 
 47 | /**
 48 |  * This function extracts a submatrix based on row indices from a sparse
 49 |  * matrix.
 50 |  * \brief Submatrix row extraction
 51 |  *
 52 |  * \return row submatrix pointer (owned by caller)
 53 |  * \param[in] A sparse matrix
 54 |  * \param[in] rowStartIdx starting row index
 55 |  * \param[in] rowEndIdx ending row index
 56 |  * \param[in] cholmod cholmod workspace
 57 |  */
 58 | cholmod_sparse* rowSubmatrix(cholmod_sparse* A, std::ptrdiff_t rowStartIdx,
 59 |   std::ptrdiff_t rowEndIdx, cholmod_common* cholmod);
 60 | 
 61 | /**
 62 |  * This function returns the column 2-norm of a sparse matrix.
 63 |  * \brief Sparse matrix column 2-norm
 64 |  *
 65 |  * \return 2-norm of the specified column
 66 |  * \param[in] A sparse matrix
 67 |  * \param[in] j column index
 68 |  */
 69 | 
 70 | double colNorm(const cholmod_sparse* A, std::ptrdiff_t j);
 71 | /**
 72 |  * This function returns the column normalization scaling matrix. According
 73 |  * to Van der Sluis (1969), this matrix minimizes the condition number of A.
 74 |  * \brief Column normalization scaling matrix
 75 |  *
 76 |  * \return scaling matrix pointer (owned by caller)
 77 |  * \param[in] A sparse matrix
 78 |  * \param[in] eps tolerance for when to consider an element zero
 79 |  * \param[in] cholmod cholmod workspace
 80 |  */
 81 | cholmod_dense* columnScalingMatrix(cholmod_sparse* A, cholmod_common*
 82 |   cholmod, double eps = std::numeric_limits<double>::epsilon());
 83 | 
 84 | /**
 85 |  * This function views an Eigen vector as a cholmod dense vector.
 86 |  * \brief Eigen vector to cholmod dense vector view
 87 |  *
 88 |  * \return void
 89 |  * \param[in] in Eigen dense vector
 90 |  * \param[out] out cholmod dense vector
 91 |  */
 92 | void eigenDenseToCholmodDenseView(const Eigen::VectorXd& in, cholmod_dense*
 93 |   out);
 94 | 
 95 | /**
 96 |  * This function copies an Eigen vector into a cholmod dense vector.
 97 |  * \brief Eigen vector to cholmod dense vector copy
 98 |  *
 99 |  * \return cholmod dense vector (owned by caller)
100 |  * \param[in] in Eigen dense vector
101 |  * \param[in] cholmod cholmod workspace
102 |  */
103 | cholmod_dense* eigenDenseToCholmodDenseCopy(const Eigen::VectorXd& in,
104 |   cholmod_common* cholmod);
105 | 
106 | /**
107 |  * This function copies the data from cholmod dense vector to Eigen dense
108 |  * vector.
109 |  * \brief Cholmod dense to Eigen dense copy
110 |  *
111 |  * \return void
112 |  * \param[in] in cholmod dense vector
113 |  * \param[out] out Eigen dense vector
114 |  */
115 | void cholmodDenseToEigenDenseCopy(const cholmod_dense* in, Eigen::VectorXd&
116 |   out);
117 | 
118 | /**
119 |  * This function copies a cholmod sparse matrix into an Eigen dense matrix.
120 |  * \brief Cholmod sparse matrix to Eigen dense converter
121 |  *
122 |  * \return void
123 |  * \param[in] in Eigen dense matrix
124 |  * \param[out] out cholmod sparse matrix
125 |  */
126 | void cholmodSparseToEigenDenseCopy(const cholmod_sparse* in,
127 |   Eigen::MatrixXd& out);
128 | 
129 | /**
130 |  * This function copies an Eigen dense matrix into a cholmod sparse matrix.
131 |  * \brief Eigen dense matrix to cholmod sparse converter
132 |  *
133 |  * \return cholmod sparse matrix (owned by caller)
134 |  * \param[in] in Eigen dense matrix
135 |  * \param[in] cholmod cholmod workspace
136 |  * \param[in] eps tolerance for when to consider an element zero
137 |  */
138 | cholmod_sparse* eigenDenseToCholmodSparseCopy(const Eigen::MatrixXd& in,
139 |   cholmod_common* cholmod, double eps =
140 |   std::numeric_limits<double>::epsilon());
141 | 
142 | /**
143 |  * This function computes the numerical rank of a matrix based on a vector
144 |  * of its singular values.
145 |  * \brief Numeric rank estimate
146 |  *
147 |  * \return numerical rank
148 |  * \param[in] sv vector of singular values
149 |  * \param[in] tol tolerance for numerical rank determination
150 |  */
151 | std::ptrdiff_t estimateNumericalRank(const Eigen::VectorXd& sv, double tol);
152 | /**
153 |  * This function computes a numerical rank tolerance.
154 |  * \brief Numeric rank tolerance estimation
155 |  *
156 |  * \return numerical rank tolerance
157 |  * \param[in] sv vector of singular values
158 |  * \param[in] eps machine precision
159 |  */
160 | 
161 | double rankTol(const Eigen::VectorXd& sv, double eps =
162 |   std::numeric_limits<double>::epsilon());
163 | /**
164 |  * This function computes a tolerance for QR factorization.
165 |  * \brief QR tolerance computation
166 |  *
167 |  * \return QR tolerance
168 |  * \param[in] A matrix on which QR is applied
169 |  * \param[in] cholmod cholmod workspace
170 |  * \param[in] eps machine precision
171 |  */
172 | double qrTol(cholmod_sparse* A, cholmod_common* cholmod, double eps =
173 |   std::numeric_limits<double>::epsilon());
174 | 
175 | /**
176 |  * This function computes the gap in the singular values for a selected
177 |  * rank.
178 |  * \brief Singular values gap estimation
179 |  *
180 |  * \return gap in the singular values for a rank
181 |  * \param[in] sv vector of singular values
182 |  * \param[in] rank estimated numerical rank
183 |  */
184 | double svGap(const Eigen::VectorXd& sv, std::ptrdiff_t rank);
185 | 
186 | /**
187 |  * This function returns the matrix where the elements on the left side
188 |  * have been marginalized out.
189 |  * \brief Reduced left-hand side recover
190 |  *
191 |  * \return void
192 |  * \param[in] factor QR factorization of the left side of the original
193 |  *            matrix
194 |  * \param[in] A_rt transpose of the right side of the original matrix
195 |  * \param[in] cholmod cholmod workspace
196 |  * \param[out] Omega reduced matrix
197 |  * \param[out] A_rtQ A_r' * Q, used subsequently by reduceRightHandSide
198 |  */
199 |  void reduceLeftHandSide(SuiteSparseQR_factorization<double>* factor,
200 |   cholmod_sparse* A_rt, cholmod_sparse** Omega, cholmod_sparse** A_rtQ,
201 |   cholmod_common* cholmod);
202 | 
203 | /**
204 |  * This function returns the reduced right-hand side of a system.
205 |  * \brief Reduced right-hand side recovery
206 |  *
207 |  * \return reduced right-hand side pointer (owned by caller)
208 |  * \param[in] factor QR factorization of the left side of the original
209 |  *            matrix
210 |  * \param[in] A_rt transpose of the right side of the original matrix
211 |  * \param[in] b original right-hand side
212 |  * \param[in] A_rtQ A_r' * Q, comes from reduceLeftHandSide
213 |  * \param[in] cholmod cholmod workspace
214 |  */
215 | cholmod_dense* reduceRightHandSide(SuiteSparseQR_factorization<double>*
216 |   factor, cholmod_sparse* A_rt, cholmod_sparse* A_rtQ, cholmod_dense* b,
217 |   cholmod_common* cholmod);
218 | 
219 | /**
220 |  * This function performs SVD analysis on the input matrix.
221 |  * \brief SVD analysis
222 |  *
223 |  * \return void
224 |  * \param[in] Omega matrix to analyze
225 |  * \param[out] sv vector of singular values
226 |  * \param[out] U left-singular vectors
227 |  * \param[out] V right-singular vectors
228 |  */
229 | void analyzeSVD(const cholmod_sparse* Omega, Eigen::VectorXd& sv,
230 |   Eigen::MatrixXd& U, Eigen::MatrixXd& V);
231 | 
232 | /**
233 |  * This function solves a system with its SVD factorization, assuming the
234 |  " system comes from a square symmetric matrix.
235 |  * \brief SVD solver
236 |  *
237 |  * \return void
238 |  * \param[in] b right-hand side
239 |  * \param[in] sv vector of singular values
240 |  * \param[in] U left-singular vectors
241 |  * \param[in] V right-singular vectors
242 |  * \param[in] rank estimated numerical rank
243 |  * \param[out] x result
244 |  */
245 | void solveSVD(const cholmod_dense* b, const Eigen::VectorXd& sv, const
246 |   Eigen::MatrixXd& U, const Eigen::MatrixXd& V, std::ptrdiff_t rank,
247 |   Eigen::VectorXd& x);
248 | 
249 | /**
250 |  * This function solves the rest of the system with the QR factorization.
251 |  * \brief Solver for rest of the system
252 |  *
253 |  * \return QR solution for the left side of the original matrix (owned by
254 |  *         caller)
255 |  * \param[in] factor QR factorization of the left side of the original
256 |  *            matrix
257 |  * \param[in] b original right-hand side
258 |  * \param[in] A_r right side of the original matrix
259 |  * \param[in] x_r LS solution for the right side of the original matrix
260 |  * \param[in] cholmod cholmod workspace
261 |  */
262 | cholmod_dense* solveQR(SuiteSparseQR_factorization<double>* factor,
263 |   cholmod_dense* b, cholmod_sparse* A_r, const Eigen::VectorXd& x_r,
264 |   cholmod_common* cholmod);
265 | 
266 | }  // namespace truncated_svd_solver
267 | #include "./internal/linear-algebra-helpers-inl.h"
268 | #endif // ASLAM_CALIBRATION_ALGORITHMS_LINALG_H
269 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/test/test-tsvd-solver.cc:
--------------------------------------------------------------------------------
  1 | #include <cstddef>
  2 | #include <iostream>
  3 | #include <iomanip>
  4 | 
  5 | #include <eigen-checks/gtest.h>
  6 | #include <gtest/gtest.h>
  7 | 
  8 | #include <Eigen/Core>
  9 | #include <Eigen/Dense>
 10 | 
 11 | #include <cholmod.h>
 12 | #include <SuiteSparseQR.hpp>
 13 | 
 14 | #include "truncated-svd-solver/tsvd-solver.h"
 15 | #include "truncated-svd-solver/linear-algebra-helpers.h"
 16 | #include "truncated-svd-solver/timing.h"
 17 | 
 18 | void evaluateSVDSPQRSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b,
 19 |     const Eigen::VectorXd& x, double tol = 1e-9) {
 20 |   cholmod_common cholmod;
 21 |   cholmod_l_start(&cholmod);
 22 |   cholmod_sparse* A_CS = truncated_svd_solver::eigenDenseToCholmodSparseCopy(A,
 23 |     &cholmod);
 24 |   cholmod_dense b_CD;
 25 |   truncated_svd_solver::eigenDenseToCholmodDenseView(b, &b_CD);
 26 |   Eigen::VectorXd x_est;
 27 |   truncated_svd_solver::TruncatedSvdSolver linearSolver;
 28 |   for (std::ptrdiff_t i = 1; i < A.cols(); ++i) {
 29 |     linearSolver.solve(A_CS, &b_CD, i, x_est);
 30 |     double error = (b - A * x_est).norm();
 31 |     ASSERT_NEAR(error, 0, tol);
 32 |     linearSolver.getOptions().columnScaling = true;
 33 |     linearSolver.solve(A_CS, &b_CD, i, x_est);
 34 |     error = (b - A * x_est).norm();
 35 |     linearSolver.getOptions().columnScaling = false;
 36 |     ASSERT_NEAR(error, 0, tol);
 37 |   }
 38 |   cholmod_l_free_sparse(&A_CS, &cholmod);
 39 |   cholmod_l_finish(&cholmod);
 40 | 
 41 |   EXPECT_TRUE(EIGEN_MATRIX_NEAR(x_est, x, 1e-8));
 42 | }
 43 | 
 44 | void evaluateSVDSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b,
 45 |     const Eigen::VectorXd& x) {
 46 |   const Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,
 47 |     Eigen::ComputeThinU | Eigen::ComputeThinV);
 48 |   Eigen::VectorXd x_est = svd.solve(b);
 49 | 
 50 |   EXPECT_TRUE(EIGEN_MATRIX_NEAR(x_est, x, 1e-8));
 51 | }
 52 | 
 53 | void evaluateSPQRSolver(const Eigen::MatrixXd& A,
 54 |                                    const Eigen::VectorXd& b,
 55 |                                    const Eigen::VectorXd& x) {
 56 |   cholmod_common cholmod;
 57 |   cholmod_l_start(&cholmod);
 58 |   cholmod_sparse* A_CS =
 59 |       truncated_svd_solver::eigenDenseToCholmodSparseCopy(A, &cholmod);
 60 |   cholmod_dense b_CD;
 61 |   truncated_svd_solver::eigenDenseToCholmodDenseView(b, &b_CD);
 62 |   Eigen::VectorXd x_est;
 63 |   SuiteSparseQR_factorization<double>* factor = SuiteSparseQR_factorize<double>(
 64 |     SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, A_CS, &cholmod);
 65 |   cholmod_dense* Qtb = SuiteSparseQR_qmult<double>(SPQR_QTX, factor, &b_CD,
 66 |     &cholmod);
 67 |   cholmod_dense* x_est_cd = SuiteSparseQR_solve<double>(SPQR_RETX_EQUALS_B,
 68 |     factor, Qtb, &cholmod);
 69 |   cholmod_l_free_dense(&Qtb, &cholmod);
 70 |   truncated_svd_solver::cholmodDenseToEigenDenseCopy(x_est_cd, x_est);
 71 |   cholmod_l_free_dense(&x_est_cd, &cholmod);
 72 |   SuiteSparseQR_free(&factor, &cholmod);
 73 |   cholmod_l_free_sparse(&A_CS, &cholmod);
 74 |   cholmod_l_finish(&cholmod);
 75 | 
 76 |   EXPECT_TRUE(EIGEN_MATRIX_NEAR(x_est, x, 1e-8));
 77 | }
 78 | 
 79 | void evaluateSPQRSolverDeterminedSystem(
 80 |     const truncated_svd_solver::TruncatedSvdSolverOptions& options) {
 81 |   // Create the system.
 82 |   constexpr size_t kNumVariables = 10u;
 83 |   constexpr double kXResult = 2.0;
 84 | 
 85 |   Eigen::VectorXd Adiag(kNumVariables);
 86 |   for(size_t i = 0; i < kNumVariables; ++i){
 87 |     Adiag(i) =  kNumVariables - i;
 88 |   }
 89 | 
 90 |   Eigen::MatrixXd A = Eigen::MatrixXd::Zero(kNumVariables, kNumVariables);
 91 |   A.diagonal() = Adiag;
 92 | 
 93 |   Eigen::VectorXd b = A * Eigen::VectorXd::Constant(kNumVariables, kXResult);
 94 | 
 95 |   // Convert to cholmod types.
 96 |   cholmod_common_struct cholmod;
 97 |   cholmod_l_start(&cholmod);
 98 |   cholmod_sparse* A_cm =
 99 |       truncated_svd_solver::eigenDenseToCholmodSparseCopy(A, &cholmod);
100 | 
101 |   cholmod_dense b_cm;
102 |   truncated_svd_solver::eigenDenseToCholmodDenseView(b, &b_cm);
103 | 
104 |   // Solve this system and check the results.
105 | 
106 |   truncated_svd_solver::TruncatedSvdSolver solver(options);
107 | 
108 |   for (std::ptrdiff_t i = 0; i <= A.cols(); ++i) {
109 |     const size_t num_calib_vars = kNumVariables - i;
110 | 
111 |     Eigen::VectorXd x;
112 |     solver.clear();
113 |     EXPECT_EQ(solver.getSVDRank(), -1);
114 |     EXPECT_EQ(solver.getSVDRankDeficiency(), -1);
115 |     EXPECT_EQ(solver.getQRRank(), 0);
116 |     EXPECT_EQ(solver.getQRRankDeficiency(), 0);
117 |     EXPECT_EQ(solver.getNullSpace().size(), 0);
118 | 
119 |     solver.analyzeMarginal(A_cm, i);
120 |     EXPECT_EQ(solver.getSVDRank(), num_calib_vars);
121 |     EXPECT_EQ(solver.getQRRank(), i);
122 |     EXPECT_EQ(solver.getQRRankDeficiency(), 0);
123 |     EXPECT_EQ(solver.getSVDRankDeficiency(), 0);
124 |     EXPECT_EQ(solver.getNullSpace().size(), 0);
125 | 
126 |     solver.clear();
127 |     EXPECT_EQ(solver.getSVDRank(), -1);
128 |     EXPECT_EQ(solver.getSVDRankDeficiency(), -1);
129 |     EXPECT_EQ(solver.getQRRank(), 0);
130 |     EXPECT_EQ(solver.getQRRankDeficiency(), 0);
131 |     EXPECT_EQ(solver.getNullSpace().size(), 0);
132 | 
133 |     solver.solve(A_cm, &b_cm, i, x);
134 |     EXPECT_EQ(solver.getSVDRank(), num_calib_vars);
135 |     EXPECT_EQ(solver.getQRRank(), i);
136 |     EXPECT_EQ(solver.getQRRankDeficiency(), 0);
137 |     EXPECT_EQ(solver.getSVDRankDeficiency(), 0);
138 |     EXPECT_EQ(solver.getNullSpace().size(), 0);
139 | 
140 |     Eigen::VectorXd expectedSingularValues;
141 |     if(options.columnScaling){
142 |       expectedSingularValues = Eigen::VectorXd::Ones(num_calib_vars);
143 |     }else{
144 |       expectedSingularValues =
145 |           Adiag.tail(num_calib_vars).array() * Adiag.tail(num_calib_vars).array();
146 |     }
147 | 
148 |     EXPECT_TRUE(
149 |         EIGEN_MATRIX_NEAR(solver.getSingularValues(),
150 |                           expectedSingularValues, 1e-8));
151 |     EXPECT_TRUE(
152 |         EIGEN_MATRIX_NEAR(x, Eigen::VectorXd::Constant(kNumVariables,
153 |                                                        kXResult),
154 |                           1e-8));
155 |   }
156 | 
157 |   cholmod_l_free_sparse(&A_cm, &cholmod);
158 | }
159 | 
160 | TEST(TruncatedSvdSolver, DeterminedSystemWithoutColumnScaling) {
161 |   truncated_svd_solver::TruncatedSvdSolverOptions options;
162 |   options.columnScaling = false;
163 |   evaluateSPQRSolverDeterminedSystem(options);
164 | }
165 | 
166 | TEST(TruncatedSvdSolver, DeterminedSystemWithColumnScaling) {
167 |   truncated_svd_solver::TruncatedSvdSolverOptions options;
168 |   options.columnScaling = true;
169 |   evaluateSPQRSolverDeterminedSystem(options);
170 | }
171 | 
172 | TEST(TruncatedSvdSolver, OverdeterminedSystem) {
173 |   Eigen::MatrixXd A = Eigen::MatrixXd::Random(100, 30);
174 |   const Eigen::VectorXd x = Eigen::VectorXd::Random(30);
175 |   Eigen::VectorXd b = A * x;
176 | 
177 |   std::cout << "-------------------------------------------------" << std::endl;
178 |   std::cout << "|                  Standard case                |" << std::endl;
179 |   std::cout << "-------------------------------------------------" << std::endl;
180 |   evaluateSVDSPQRSolver(A, b, x);
181 |   evaluateSVDSolver(A, b, x);
182 |   evaluateSPQRSolver(A, b, x);
183 | 
184 |   std::cout << "-------------------------------------------------" << std::endl;
185 |   std::cout << "|                 Badly scaled case             |" << std::endl;
186 |   std::cout << "-------------------------------------------------" << std::endl;
187 |   A.col(2) = 1e6 * A.col(2);
188 |   A.col(28) = 1e6 * A.col(28);
189 |   b = A * x;
190 |   evaluateSVDSPQRSolver(A, b, x, 1e-3);
191 |   evaluateSVDSolver(A, b, x);
192 |   evaluateSPQRSolver(A, b, x);
193 | 
194 | //  std::cout << "-------------------------------------------------" << std::endl;
195 | //  std::cout << "|                 Rank-deficient case 1         |" << std::endl;
196 | //  std::cout << "-------------------------------------------------" << std::endl;
197 | //  A = Eigen::MatrixXd::Random(100, 30);
198 | //  A.col(10) = Eigen::VectorXd::Zero(A.rows());
199 | //  b = A * x;
200 | //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
201 | //  evaluateSVDSolver(A, b, x);
202 | //  evaluateSPQRSolver(A, b, x);
203 | 
204 | //  std::cout << "-------------------------------------------------" << std::endl;
205 | //  std::cout << "|                 Rank-deficient case 2         |" << std::endl;
206 | //  std::cout << "-------------------------------------------------" << std::endl;
207 | //  A = Eigen::MatrixXd::Random(100, 30);
208 | //  A.col(10) = 2 * A.col(1) + 5 * A.col(20);
209 | //  b = A * x;
210 | //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
211 | //  evaluateSVDSolver(A, b, x);
212 | //  evaluateSPQRSolver(A, b, x);
213 | 
214 | //  std::cout << "-------------------------------------------------" << std::endl;
215 | //  std::cout << "|                 Near rank-deficient case 1    |" << std::endl;
216 | //  std::cout << "-------------------------------------------------" << std::endl;
217 | //  A = Eigen::MatrixXd::Random(100, 30);
218 | //  A.col(10) = Eigen::VectorXd::Zero(A.rows());
219 | //  b = A * x;
220 | //  A.col(10) = truncated_svd_solver::NormalDistribution<100>(
221 | //    Eigen::VectorXd::Zero(A.rows()),
222 | //    1e-6 * Eigen::MatrixXd::Identity(A.rows(), A.rows())).getSample();
223 | //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
224 | //  evaluateSVDSolver(A, b, x);
225 | //  evaluateSPQRSolver(A, b, x);
226 | 
227 | //  std::cout << "-------------------------------------------------" << std::endl;
228 | //  std::cout << "|                 Near rank-deficient case 2    |" << std::endl;
229 | //  std::cout << "-------------------------------------------------" << std::endl;
230 | //  A = Eigen::MatrixXd::Random(100, 30);
231 | //  A.col(10) = 2 * A.col(1) + 5 * A.col(20);
232 | //  b = A * x;
233 | //  A.col(10) = truncated_svd_solver::NormalDistribution<100>(A.col(10),
234 | //    1e-20 * Eigen::MatrixXd::Identity(A.rows(), A.rows())).getSample();
235 | //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
236 | //  evaluateSVDSolver(A, b, x);
237 | //  evaluateSPQRSolver(A, b, x);
238 | }
239 | 
240 | int main(int argc, char** argv) {
241 |   ::testing::InitGoogleTest(&argc, argv);
242 |   google::InitGoogleLogging(argv[0]);
243 |   google::InstallFailureSignalHandler();\
244 |   ::testing::FLAGS_gtest_death_test_style = "threadsafe";
245 |   FLAGS_alsologtostderr = true;
246 |   FLAGS_colorlogtostderr = true;
247 |   return RUN_ALL_TESTS();
248 | }
249 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/src/tsvd-solver.cc:
--------------------------------------------------------------------------------
  1 | #include "truncated-svd-solver/tsvd-solver.h"
  2 | 
  3 | #include <algorithm>
  4 | #include <cmath>
  5 | #include <memory>
  6 | 
  7 | #include <glog/logging.h>
  8 | #include <SuiteSparseQR.hpp>
  9 | 
 10 | #include "truncated-svd-solver/cholmod-helpers.h"
 11 | #include "truncated-svd-solver/linear-algebra-helpers.h"
 12 | #include "truncated-svd-solver/timing.h"
 13 | 
 14 | namespace truncated_svd_solver {
 15 | 
 16 | TruncatedSvdSolver::TruncatedSvdSolver(const Options& options) :
 17 |     tsvd_options_(options),
 18 |     factor_(nullptr),
 19 |     svdRank_(-1),
 20 |     svGap_(std::numeric_limits<double>::infinity()),
 21 |     svdRankDeficiency_(-1),
 22 |     margStartIndex_(-1),
 23 |     svdTolerance_(-1.0),
 24 |     linearSolverTime_(0.0),
 25 |     marginalAnalysisTime_(0.0) {
 26 |   cholmod_l_start(&cholmod_);
 27 |   cholmod_.SPQR_grain = 16;  // Maybe useless.
 28 | }
 29 | 
 30 | TruncatedSvdSolver::~TruncatedSvdSolver() {
 31 |   clear();
 32 |   cholmod_l_finish(&cholmod_);
 33 |   if (tsvd_options_.verbose && getMemoryUsage() > 0) {
 34 |     LOG(ERROR) << "Cholmod memory leak detected.";
 35 |   }
 36 | }
 37 | 
 38 | void TruncatedSvdSolver::clear() {
 39 |   if (factor_) {
 40 |     SuiteSparseQR_free<double>(&factor_, &cholmod_);
 41 |     factor_ = nullptr;
 42 |   }
 43 |   clearSvdAnalysisResultMembers();
 44 |   linearSolverTime_ = 0.0;
 45 |   marginalAnalysisTime_ = 0.0;
 46 | }
 47 | 
 48 | const TruncatedSvdSolver::Options& TruncatedSvdSolver::getOptions() const {
 49 |   return tsvd_options_;
 50 | }
 51 | 
 52 | TruncatedSvdSolver::Options& TruncatedSvdSolver::getOptions() {
 53 |   return tsvd_options_;
 54 | }
 55 | 
 56 | std::ptrdiff_t TruncatedSvdSolver::getSVDRank() const {
 57 |   return svdRank_;
 58 | }
 59 | 
 60 | std::ptrdiff_t TruncatedSvdSolver::getSVDRankDeficiency() const {
 61 |   return svdRankDeficiency_;
 62 | }
 63 | 
 64 | double TruncatedSvdSolver::getSvGap() const {
 65 |   return svGap_;
 66 | }
 67 | 
 68 | std::ptrdiff_t TruncatedSvdSolver::getQRRank() const {
 69 |   if (factor_ && factor_->QRnum)
 70 |     return factor_->rank;
 71 |   else
 72 |     return 0;
 73 | }
 74 | 
 75 | std::ptrdiff_t TruncatedSvdSolver::getQRRankDeficiency() const {
 76 |   if (factor_ && factor_->QRsym && factor_->QRnum)
 77 |     return factor_->QRsym->n - factor_->rank;
 78 |   else
 79 |     return 0;
 80 | }
 81 | 
 82 | std::ptrdiff_t TruncatedSvdSolver::getMargStartIndex() const {
 83 |   return margStartIndex_;
 84 | }
 85 | 
 86 | void TruncatedSvdSolver::setMargStartIndex(std::ptrdiff_t index) {
 87 |   margStartIndex_ = index;
 88 | }
 89 | 
 90 | double TruncatedSvdSolver::getQRTolerance() const {
 91 |   if (factor_ && factor_->QRsym && factor_->QRnum)
 92 |     return factor_->tol;
 93 |   else
 94 |     return SPQR_DEFAULT_TOL;
 95 | }
 96 | 
 97 | double TruncatedSvdSolver::getSVDTolerance() const {
 98 |   return svdTolerance_;
 99 | }
100 | 
101 | const Eigen::VectorXd& TruncatedSvdSolver::getSingularValues() const {
102 |   return singularValues_;
103 | }
104 | 
105 | const Eigen::MatrixXd& TruncatedSvdSolver::getMatrixU() const {
106 |   return matrixU_;
107 | }
108 | 
109 | const Eigen::MatrixXd& TruncatedSvdSolver::getMatrixV() const {
110 |   return matrixV_;
111 | }
112 | 
113 | Eigen::MatrixXd TruncatedSvdSolver::getNullSpace() const {
114 |   if (svdRank_ == -1 || svdRank_ > matrixV_.cols()) {
115 |     return Eigen::MatrixXd(0, 0);
116 |   }
117 |   return matrixV_.rightCols(matrixV_.cols() - svdRank_);
118 | }
119 | 
120 | Eigen::MatrixXd TruncatedSvdSolver::getRowSpace() const {
121 |   if (svdRank_ == -1 || svdRank_ > matrixV_.cols()) {
122 |     return Eigen::MatrixXd(0, 0);
123 |   }
124 |   return matrixV_.leftCols(svdRank_);
125 | }
126 | 
127 | Eigen::MatrixXd TruncatedSvdSolver::getCovariance() const {
128 |   if (svdRank_ == -1 || svdRank_ > matrixV_.cols()) {
129 |     return Eigen::MatrixXd(0, 0);
130 |   }
131 |   return matrixV_.leftCols(svdRank_) *
132 |     singularValues_.head(svdRank_).asDiagonal().inverse() *
133 |     matrixV_.leftCols(svdRank_).adjoint();
134 | }
135 | 
136 | Eigen::MatrixXd TruncatedSvdSolver::getRowSpaceCovariance() const {
137 |   if (svdRank_ == -1) {
138 |     return Eigen::MatrixXd(0, 0);
139 |   }
140 |   return singularValues_.head(svdRank_).asDiagonal().inverse();
141 | }
142 | 
143 | double TruncatedSvdSolver::getSingularValuesLog2Sum() const {
144 |   if (svdRank_ == -1) {
145 |     return 0.0;
146 |   }
147 |   return singularValues_.head(svdRank_).array().log().sum() / std::log(2);
148 | }
149 | 
150 | size_t TruncatedSvdSolver::getPeakMemoryUsage() const {
151 |   return cholmod_.memory_usage;
152 | }
153 | 
154 | size_t TruncatedSvdSolver::getMemoryUsage() const {
155 |   return cholmod_.memory_inuse;
156 | }
157 | 
158 | double TruncatedSvdSolver::getNumFlops() const {
159 |   return cholmod_.SPQR_xstat[0];
160 | }
161 | 
162 | double TruncatedSvdSolver::getLinearSolverTime() const {
163 |   return linearSolverTime_;
164 | }
165 | 
166 | double TruncatedSvdSolver::getMarginalAnalysisTime() const {
167 |   return marginalAnalysisTime_;
168 | }
169 | 
170 | double TruncatedSvdSolver::getSymbolicFactorizationTime() const {
171 |   if (!factor_ || !factor_->QRsym) {
172 |     return 0.0;
173 |   }
174 |   return cholmod_.other1[1];
175 | }
176 | 
177 | double TruncatedSvdSolver::getNumericFactorizationTime() const {
178 |   if (!factor_ || !factor_->QRnum) {
179 |     return 0.0;
180 |   }
181 |   return cholmod_.other1[2];
182 | }
183 | 
184 | void TruncatedSvdSolver::setNThreads(int n) {
185 |   CHECK_GE(n, -1);
186 |   cholmod_.SPQR_nthreads = n;
187 | }
188 | 
189 | void TruncatedSvdSolver::solve(cholmod_sparse* A, cholmod_dense* b,
190 |                                size_t j, Eigen::VectorXd& x) {
191 |   CHECK_EQ(A->nrow, b->nrow);
192 |   const bool hasQrPart = j > 0;
193 |   const bool hasSvdPart = j < A->ncol;
194 | 
195 |   const double t0 = Timestamp::now();
196 | 
197 |   SelfFreeingCholmodPtr<cholmod_dense> G_l(nullptr, cholmod_);
198 |   if(hasQrPart) {
199 |     SelfFreeingCholmodPtr<cholmod_sparse> A_l(nullptr, cholmod_);
200 |     A_l = columnSubmatrix(A, 0, j - 1, &cholmod_);
201 | 
202 |     if (tsvd_options_.columnScaling) {
203 |       G_l = columnScalingMatrix(A_l, &cholmod_, tsvd_options_.epsNorm);
204 |       bool success = cholmod_l_scale(G_l, CHOLMOD_COL, A_l, &cholmod_);
205 |       CHECK(success) << "cholmod_l_scale failed.";
206 |     }
207 | 
208 |     // Clear the cached symbolic QR-factorization if the matrix has changed.
209 |     if (factor_ && factor_->QRsym &&
210 |         (factor_->QRsym->m != static_cast<std::ptrdiff_t>(A_l->nrow) ||
211 |         factor_->QRsym->n != static_cast<std::ptrdiff_t>(A_l->ncol) ||
212 |         factor_->QRsym->anz != static_cast<std::ptrdiff_t>(A_l->nzmax))) {
213 |       clear();
214 |     }
215 | 
216 |     // Calculate the symbolic factorization (if cache is not filled).
217 |     if (!factor_) {
218 |       const double t2 = Timestamp::now();
219 |       factor_ = SuiteSparseQR_symbolic<double>(SPQR_ORDERING_BEST,
220 |         SPQR_DEFAULT_TOL, A_l, &cholmod_);
221 |       CHECK(factor_ != nullptr) << "SuiteSparseQR_symbolic failed.";
222 | 
223 |       const double t3 = Timestamp::now();
224 |       cholmod_.other1[1] = t3 - t2;
225 |     }
226 | 
227 |     const double qrTolerance = (tsvd_options_.qrTol != -1.0) ? tsvd_options_.qrTol :
228 |       qrTol(A_l, &cholmod_, tsvd_options_.epsQR);
229 |     const double t2 = Timestamp::now();
230 |     const bool status = SuiteSparseQR_numeric<double>(qrTolerance, A_l,
231 |       factor_, &cholmod_);
232 |     CHECK(status) << "SuiteSparseQR_numeric failed.";
233 |     const double t3 = Timestamp::now();
234 |     cholmod_.other1[2] = t3 - t2;
235 |   } else {
236 |     clear();
237 |     cholmod_.other1[1] = cholmod_.other1[2] = 0.0;
238 |   }
239 | 
240 |   Eigen::VectorXd x_r;
241 |   SelfFreeingCholmodPtr<cholmod_dense> x_l(nullptr, cholmod_);
242 |   SelfFreeingCholmodPtr<cholmod_dense> G_r(nullptr, cholmod_);
243 |   if (hasSvdPart) {
244 |     SelfFreeingCholmodPtr<cholmod_sparse> A_r(nullptr, cholmod_);
245 |     A_r = columnSubmatrix(A, j, A->ncol - 1, &cholmod_);
246 | 
247 |     if (tsvd_options_.columnScaling) {
248 |       G_r = columnScalingMatrix(A_r, &cholmod_, tsvd_options_.epsNorm);
249 |       const bool success = cholmod_l_scale(G_r, CHOLMOD_COL, A_r, &cholmod_);
250 |       CHECK(success) << "cholmod_l_scale failed.";
251 |     }
252 | 
253 |     SelfFreeingCholmodPtr<cholmod_dense> b_r(nullptr, cholmod_);
254 |     {
255 |       SelfFreeingCholmodPtr<cholmod_sparse> A_rt(
256 |           cholmod_l_transpose(A_r, 1, &cholmod_), cholmod_);
257 |       CHECK(A_rt != nullptr) << "cholmod_l_transpose failed.";
258 | 
259 |       SelfFreeingCholmodPtr<cholmod_sparse> A_rtQ(nullptr, cholmod_);
260 |       SelfFreeingCholmodPtr<cholmod_sparse> Omega(nullptr, cholmod_);
261 |       reduceLeftHandSide(factor_, A_rt, &Omega, &A_rtQ, &cholmod_);
262 |       analyzeSVD(Omega);
263 |       b_r = reduceRightHandSide(factor_, A_rt, A_rtQ, b, &cholmod_);
264 |     }
265 |     solveSVD(b_r, singularValues_, matrixU_, matrixV_, svdRank_, x_r);
266 |     if (hasQrPart) {
267 |       x_l = solveQR(factor_, b, A_r, x_r, &cholmod_);
268 |     }
269 |   } else {
270 |     analyzeSVD(nullptr);
271 |     x_r.resize(0);
272 |     if (hasQrPart) {
273 |       x_l = solveQR(factor_, b, nullptr, x_r, &cholmod_);
274 |     }
275 |   }
276 | 
277 |   if (tsvd_options_.columnScaling) {
278 |     if(G_l){
279 |       Eigen::Map<const Eigen::VectorXd> G_lEigen(
280 |           reinterpret_cast<const double*>(G_l->x), G_l->nrow);
281 |       Eigen::Map<Eigen::VectorXd> x_lEigen(
282 |           reinterpret_cast<double*>(x_l->x), x_l->nrow);
283 |       x_lEigen = G_lEigen.cwiseProduct(x_lEigen);
284 |       G_l.reset(nullptr);
285 |     }
286 |     if(G_r){
287 |       Eigen::Map<const Eigen::VectorXd> G_rEigen(
288 |           reinterpret_cast<const double*>(G_r->x), G_r->nrow);
289 |       x_r = G_rEigen.array() * x_r.array();
290 |       G_r.reset(nullptr);
291 |     }
292 |   }
293 | 
294 |   x.resize(A->ncol);
295 |   if(hasQrPart){
296 |     Eigen::Map<Eigen::VectorXd> x_lEigen(
297 |         reinterpret_cast<double*>(x_l->x), x_l->nrow);
298 |     x.head(x_lEigen.size()) = x_lEigen;
299 |   }
300 |   if(hasSvdPart){
301 |     x.tail(x_r.size()) = x_r;
302 |   }
303 | 
304 |   linearSolverTime_ = Timestamp::now() - t0;
305 | }
306 | 
307 | void TruncatedSvdSolver::analyzeSVD(cholmod_sparse * Omega) {
308 |   if (Omega) {
309 |     truncated_svd_solver::analyzeSVD(Omega, singularValues_, matrixU_,
310 |                                      matrixV_);
311 |     svdTolerance_ = (tsvd_options_.svdTol != -1.0) ? tsvd_options_.svdTol :
312 |         rankTol(singularValues_, tsvd_options_.epsSVD);
313 |     svdRank_ = estimateNumericalRank(singularValues_, svdTolerance_);
314 |     svdRankDeficiency_ = singularValues_.size() - svdRank_;
315 |     svGap_ = svGap(singularValues_, svdRank_);
316 |   } else {
317 |     clearSvdAnalysisResultMembers();
318 |     svdRank_ = 0;
319 |     svdRankDeficiency_ = 0;
320 |   }
321 | }
322 | 
323 | void TruncatedSvdSolver::analyzeMarginal(cholmod_sparse* A, size_t j) {
324 |   const double t0 = Timestamp::now();
325 |   const bool hasQrPart = j > 0;
326 |   const bool hasSvdPart = j < A->ncol;
327 | 
328 |   if (hasQrPart) {
329 |     SelfFreeingCholmodPtr<cholmod_sparse> A_l(
330 |         columnSubmatrix(A, 0, j - 1, &cholmod_), cholmod_);
331 |     if (factor_ && factor_->QRsym
332 |         && (factor_->QRsym->m != static_cast<std::ptrdiff_t>(A_l->nrow)
333 |             || factor_->QRsym->n != static_cast<std::ptrdiff_t>(A_l->ncol)
334 |             || factor_->QRsym->anz != static_cast<std::ptrdiff_t>(A_l->nzmax)))
335 |       clear();
336 |     if (!factor_) {
337 |       const double t2 = Timestamp::now();
338 |       factor_ = SuiteSparseQR_symbolic<double>(SPQR_ORDERING_BEST,
339 |                                                SPQR_DEFAULT_TOL, A_l,
340 |                                                &cholmod_);
341 |       CHECK(factor_ != nullptr) << "SuiteSparseQR_symbolic failed.";
342 | 
343 |       const double t3 = Timestamp::now();
344 |       cholmod_.other1[1] = t3 - t2;
345 |     }
346 | 
347 |     const double t2 = Timestamp::now();
348 |     const double qrTolerance =
349 |         (tsvd_options_.qrTol != -1.0) ? tsvd_options_.qrTol :
350 |             qrTol(A_l, &cholmod_, tsvd_options_.epsQR);
351 |     const bool success = SuiteSparseQR_numeric<double>(
352 |         qrTolerance, A_l, factor_, &cholmod_);
353 |     CHECK(success) << "SuiteSparseQR_numeric failed.";
354 |     cholmod_.other1[2] = Timestamp::now() - t2;
355 |   } else {
356 |     clear();
357 |     cholmod_.other1[1] = cholmod_.other1[2] = 0.0;
358 |   }
359 | 
360 |   if (hasSvdPart) {
361 |     SelfFreeingCholmodPtr<cholmod_sparse> A_r(
362 |         columnSubmatrix(A, j, A->ncol - 1, &cholmod_), cholmod_);
363 |     SelfFreeingCholmodPtr<cholmod_sparse> A_rt(
364 |         cholmod_l_transpose(A_r, 1, &cholmod_), cholmod_);
365 |     CHECK(A_rt != nullptr) << "cholmod_l_transpose failed.";
366 | 
367 |     SelfFreeingCholmodPtr<cholmod_sparse> Omega(nullptr, cholmod_);
368 |     SelfFreeingCholmodPtr<cholmod_sparse> A_rtQ(nullptr, cholmod_);
369 |     reduceLeftHandSide(factor_, A_rt, &Omega, &A_rtQ, &cholmod_);
370 |     analyzeSVD(Omega);
371 |   } else {
372 |     analyzeSVD(nullptr);
373 |   }
374 | 
375 |   marginalAnalysisTime_ = Timestamp::now() - t0;
376 | }
377 | 
378 | void TruncatedSvdSolver::clearSvdAnalysisResultMembers() {
379 |   svdRank_ = -1;
380 |   svGap_ = std::numeric_limits<double>::infinity();
381 |   svdRankDeficiency_ = -1;
382 |   svdTolerance_ = -1.0;
383 |   singularValues_.resize(0);
384 |   matrixU_.resize(0, 0);
385 |   matrixV_.resize(0, 0);
386 | }
387 | 
388 | }  // namespace truncated_svd_solver
389 | 


--------------------------------------------------------------------------------
/truncated_svd_solver/src/linear-algebra-helpers.cc:
--------------------------------------------------------------------------------
  1 | #include "truncated-svd-solver/linear-algebra-helpers.h"
  2 | 
  3 | #include <algorithm>
  4 | #include <cmath>
  5 | 
  6 | #include <cholmod.h>
  7 | #include <Eigen/Dense>
  8 | #include <glog/logging.h>
  9 | #include <spqr.hpp>
 10 | #include <SuiteSparseQR.hpp>
 11 | 
 12 | namespace truncated_svd_solver {
 13 | 
 14 | cholmod_sparse* columnSubmatrix(cholmod_sparse* A, std::ptrdiff_t
 15 |     col_start_idx, std::ptrdiff_t col_end_idx, cholmod_common* cholmod) {
 16 |   CHECK_NOTNULL(A);
 17 |   CHECK_NOTNULL(cholmod);
 18 | 
 19 |   CHECK_GE(col_end_idx, col_start_idx);
 20 |   CHECK_LT(col_end_idx, static_cast<std::ptrdiff_t>(A->ncol));
 21 |   CHECK_GE(col_start_idx, 0);
 22 | 
 23 |   const std::ptrdiff_t num_indices = col_end_idx - col_start_idx + 1;
 24 |   std::ptrdiff_t* col_indices = new std::ptrdiff_t[num_indices];
 25 |   for (std::ptrdiff_t j = col_start_idx; j <= col_end_idx; ++j) {
 26 |     col_indices[j - col_start_idx] = j;
 27 |   }
 28 | 
 29 |   cholmod_sparse* A_sub = cholmod_l_submatrix(A, nullptr, -1, col_indices,
 30 |     num_indices, 1, 1, cholmod);
 31 |   delete [] col_indices;
 32 |   CHECK(A_sub != nullptr) << "cholmod_l_submatrix failed.";
 33 | 
 34 |   return A_sub;
 35 | }
 36 | 
 37 | cholmod_sparse* rowSubmatrix(cholmod_sparse* A, std::ptrdiff_t row_start_idx,
 38 |     std::ptrdiff_t row_end_idx, cholmod_common* cholmod) {
 39 |   CHECK_NOTNULL(A);
 40 |   CHECK_NOTNULL(cholmod);
 41 | 
 42 |   CHECK_GE(row_end_idx, row_start_idx);
 43 |   CHECK_LT(row_end_idx, static_cast<std::ptrdiff_t>(A->nrow));
 44 |   CHECK_GE(row_start_idx, 0);
 45 | 
 46 |   const std::ptrdiff_t num_indices = row_end_idx - row_start_idx + 1;
 47 |   std::ptrdiff_t* row_indices = new std::ptrdiff_t[num_indices];
 48 |   for (std::ptrdiff_t i = row_start_idx; i <= row_end_idx; ++i){
 49 |     row_indices[i - row_start_idx] = i;
 50 |   }
 51 | 
 52 |   cholmod_sparse* A_sub = cholmod_l_submatrix(A, row_indices, num_indices,
 53 |     nullptr, -1, 1, 1, cholmod);
 54 |   delete [] row_indices;
 55 |   CHECK(A_sub != nullptr) << "cholmod_l_submatrix failed.";
 56 | 
 57 |   return A_sub;
 58 | }
 59 | 
 60 | double colNorm(const cholmod_sparse* A, std::ptrdiff_t col_idx) {
 61 |   CHECK_NOTNULL(A);
 62 |   CHECK_GE(col_idx, 0);
 63 |   CHECK_LT(col_idx, static_cast<std::ptrdiff_t>(A->ncol));
 64 | 
 65 |   const std::ptrdiff_t* col_ptr =
 66 |     reinterpret_cast<const std::ptrdiff_t*>(A->p);
 67 |   const double* values = reinterpret_cast<const double*>(A->x);
 68 |   const std::ptrdiff_t p = col_ptr[col_idx];
 69 |   const std::ptrdiff_t num_elements = col_ptr[col_idx + 1] - p;
 70 | 
 71 |   double norm = 0.0;
 72 |   for (std::ptrdiff_t i = 0; i < num_elements; ++i) {
 73 |     norm += values[p + i] * values[p + i];
 74 |   }
 75 |   return std::sqrt(norm);
 76 | }
 77 | 
 78 | cholmod_dense* columnScalingMatrix(cholmod_sparse* A, cholmod_common*
 79 |     cholmod, double eps) {
 80 |   CHECK_NOTNULL(A);
 81 |   CHECK_NOTNULL(cholmod);
 82 |   CHECK_GT(eps, 0.0);
 83 | 
 84 |   cholmod_dense* G = cholmod_l_allocate_dense(A->ncol, 1, A->ncol,
 85 |     CHOLMOD_REAL, cholmod);
 86 |   CHECK(G != nullptr) << "cholmod_l_allocate_dense failed.";
 87 | 
 88 |   const double norm_tolerance = std::sqrt(A->nrow * eps);
 89 |   double* values = reinterpret_cast<double*>(G->x);
 90 |   for (std::ptrdiff_t col_idx = 0;
 91 |       col_idx < static_cast<std::ptrdiff_t>(A->ncol);
 92 |       ++col_idx) {
 93 |     const double norm = colNorm(A, col_idx);
 94 |     if (norm < norm_tolerance) {
 95 |       values[col_idx] = 0.0;
 96 |     } else {
 97 |       values[col_idx] = 1.0 / norm;
 98 |     }
 99 |   }
100 |   return G;
101 | }
102 | 
103 | void cholmodSparseToEigenDenseCopy(const cholmod_sparse* in,
104 |     Eigen::MatrixXd& out) {
105 |   CHECK_NOTNULL(in);
106 |   out.setZero(in->nrow, in->ncol);
107 | 
108 |   const std::ptrdiff_t* row_ind =
109 |     reinterpret_cast<const std::ptrdiff_t*>(in->i);
110 |   const std::ptrdiff_t* col_ptr =
111 |     reinterpret_cast<const std::ptrdiff_t*>(in->p);
112 |   const double* values = reinterpret_cast<const double*>(in->x);
113 |   for (std::ptrdiff_t col_idx = 0;
114 |        col_idx < static_cast<std::ptrdiff_t>(in->ncol); ++col_idx)
115 |     for (std::ptrdiff_t val_idx = col_ptr[col_idx];
116 |          val_idx < col_ptr[col_idx + 1]; ++val_idx) {
117 |       out(row_ind[val_idx], col_idx) = values[val_idx];
118 |       if (in->stype && col_idx != row_ind[val_idx]) {
119 |         out(col_idx, row_ind[val_idx]) = values[val_idx];
120 |       }
121 |     }
122 | }
123 | 
124 | void eigenDenseToCholmodDenseView(const Eigen::VectorXd& in, cholmod_dense*
125 |     out) {
126 |   CHECK_NOTNULL(out);
127 | 
128 |   out->nrow = in.size();
129 |   out->ncol = 1;
130 |   out->nzmax = in.size();
131 |   out->d = in.size();
132 |   out->x = reinterpret_cast<void*>(const_cast<double*>(in.data()));
133 |   out->z = nullptr;
134 |   out->xtype = CHOLMOD_REAL;
135 |   out->dtype = CHOLMOD_DOUBLE;
136 | }
137 | 
138 | cholmod_dense* eigenDenseToCholmodDenseCopy(const Eigen::VectorXd& in,
139 |     cholmod_common* cholmod) {
140 |   CHECK_NOTNULL(cholmod);
141 |   cholmod_dense* out = cholmod_l_allocate_dense(in.size(), 1, in.size(),
142 |     CHOLMOD_REAL, cholmod);
143 |   CHECK(out != nullptr) << "cholmod_l_allocate_dense failed.";
144 | 
145 |   double* out_val = reinterpret_cast<double*>(out->x);
146 |   const double* in_val = in.data();
147 |   std::copy(in_val, in_val + in.size(), out_val);
148 |   return out;
149 | }
150 | 
151 | void cholmodDenseToEigenDenseCopy(const cholmod_dense* in, Eigen::VectorXd&
152 |     out) {
153 |   CHECK_NOTNULL(in);
154 |   out.resize(in->nrow);
155 |   const double* in_val = reinterpret_cast<const double*>(in->x);
156 |   std::copy(in_val, in_val + in->nrow, out.data());
157 | }
158 | 
159 | cholmod_sparse* eigenDenseToCholmodSparseCopy(const Eigen::MatrixXd& A,
160 |     cholmod_common* cholmod, double eps) {
161 |   CHECK_NOTNULL(cholmod);
162 |   CHECK_GT(eps, 0.0);
163 | 
164 |   size_t nzmax = 0;
165 |   for (std::ptrdiff_t i = 0; i < A.rows(); ++i) {
166 |     for (std::ptrdiff_t j = 0; j < A.cols(); ++j) {
167 |       if (std::fabs(A(i, j)) > eps) {
168 |         nzmax++;
169 |       }
170 |     }
171 |   }
172 | 
173 |   cholmod_sparse* A_cholmod = cholmod_l_allocate_sparse(A.rows(), A.cols(),
174 |     nzmax, 1, 1, 0, CHOLMOD_REAL, cholmod);
175 |   CHECK(A_cholmod != nullptr) << "cholmod_l_allocate_sparse failed.";
176 | 
177 |   std::ptrdiff_t* row_ind = reinterpret_cast<std::ptrdiff_t*>(A_cholmod->i);
178 |   std::ptrdiff_t* col_ptr = reinterpret_cast<std::ptrdiff_t*>(A_cholmod->p);
179 |   double* values = reinterpret_cast<double*>(A_cholmod->x);
180 |   std::ptrdiff_t row_it = 0;
181 |   std::ptrdiff_t col_it = 1;
182 |   for (std::ptrdiff_t c = 0; c < A.cols(); ++c) {
183 |     for (std::ptrdiff_t r = 0; r < A.rows(); ++r)
184 |       if (std::fabs(A(r, c)) > eps) {
185 |         values[row_it] = A(r, c);
186 |         row_ind[row_it] = r;
187 |         row_it++;
188 |       }
189 |     col_ptr[col_it] = row_it;
190 |     col_it++;
191 |   }
192 |   return A_cholmod;
193 | }
194 | 
195 | std::ptrdiff_t estimateNumericalRank(const Eigen::VectorXd& singular_values,
196 |                                      double tolerance) {
197 |   CHECK_GE(tolerance, 0.0);
198 | 
199 |   std::ptrdiff_t numerical_rank = singular_values.size();
200 |   for (std::ptrdiff_t i = singular_values.size() - 1; i >= 0; --i) {
201 |     if (singular_values(i) > tolerance) {
202 |       // Assumes that the singular values are ordered.
203 |       break;
204 |     } else {
205 |       --numerical_rank;
206 |     }
207 |   }
208 |   return numerical_rank;
209 | }
210 | 
211 | double rankTol(const Eigen::VectorXd& singular_values, double eps) {
212 |   CHECK_GT(singular_values.rows(), 0) << "Empty singular values vector.";
213 |   CHECK_GT(eps, 0.0);
214 |   return singular_values(0) * eps * singular_values.size();
215 | }
216 | 
217 | double qrTol(cholmod_sparse* A, cholmod_common* cholmod, double eps) {
218 |   CHECK_NOTNULL(A);
219 |   CHECK_NOTNULL(cholmod);
220 |   CHECK_GT(eps, 0.0);
221 |   return 20.0 * static_cast<double>(A->nrow + A->ncol) * eps *
222 |     spqr_maxcolnorm<double>(A, cholmod);
223 | }
224 | 
225 | double svGap(const Eigen::VectorXd& singular_values, std::ptrdiff_t rank) {
226 |   CHECK_LE(rank, singular_values.rows());
227 |   CHECK_GE(rank, 0);
228 | 
229 |   if (rank == 0) {
230 |     return 0.0;
231 |   } else if (rank < singular_values.size()) {
232 |     return singular_values(rank - 1) / singular_values(rank);
233 |   }
234 |   return std::numeric_limits<double>::infinity();
235 | }
236 | 
237 | void reduceLeftHandSide(SuiteSparseQR_factorization<double>* factor,
238 |     cholmod_sparse* A_rt, cholmod_sparse** Omega, cholmod_sparse** A_rtQ,
239 |     cholmod_common* cholmod) {
240 |   CHECK_NOTNULL(A_rt);
241 |   CHECK_NOTNULL(cholmod);
242 |   CHECK_NOTNULL(Omega);
243 | 
244 |   // Handle the special case where the QR part is empty.
245 |   if (factor == nullptr) {
246 |     // NO QR part!
247 |     *Omega = cholmod_l_aat(A_rt, nullptr, 0, 1, cholmod);
248 |     return;
249 |   } else {
250 |     CHECK(factor->QRsym != nullptr) << "Run symbolic factorization first.";
251 |     CHECK(factor->QRnum != nullptr) << "Run QR decomposition first.";
252 |   }
253 |   CHECK_NOTNULL(factor);
254 |   CHECK_NOTNULL(A_rtQ);
255 | 
256 |   cholmod_sparse* A_rtQFull = SuiteSparseQR_qmult<double>(SPQR_XQ, factor,
257 |     A_rt, cholmod);
258 |   CHECK(A_rtQFull != nullptr) << "SuiteSparseQR_qmult failed.";
259 | 
260 | 
261 |   *A_rtQ = columnSubmatrix(A_rtQFull, 0, factor->QRsym->n - 1, cholmod);
262 |   cholmod_l_free_sparse(&A_rtQFull, cholmod);
263 | 
264 |   cholmod_sparse* A_rtQ2 = cholmod_l_aat(*A_rtQ, nullptr, 0, 1, cholmod);
265 |   CHECK(A_rtQ2 != nullptr) << "cholmod_l_aat failed.";
266 |   A_rtQ2->stype = 1;
267 | 
268 |   cholmod_sparse* A_rt2 = cholmod_l_aat(A_rt, nullptr, 0, 1, cholmod);
269 |   CHECK(A_rt2 != nullptr) << "cholmod_l_aat failed.";
270 |   A_rt2->stype = 1;
271 | 
272 |   double alpha[2];
273 |   alpha[0] = 1.0;
274 |   double beta[2];
275 |   beta[0] = -1.0;
276 | 
277 |   *Omega = cholmod_l_add(A_rt2, A_rtQ2, alpha, beta, 1, 1, cholmod);
278 |   CHECK(*Omega != nullptr) << "cholmod_l_add failed.";
279 | 
280 |   cholmod_l_free_sparse(&A_rt2, cholmod);
281 |   cholmod_l_free_sparse(&A_rtQ2, cholmod);
282 | }
283 | 
284 | cholmod_dense* reduceRightHandSide(SuiteSparseQR_factorization<double>*
285 |     factor, cholmod_sparse* A_rt, cholmod_sparse* A_rtQ, cholmod_dense* b,
286 |     cholmod_common* cholmod) {
287 |   CHECK_NOTNULL(A_rt);
288 |   CHECK_NOTNULL(b);
289 |   CHECK_NOTNULL(cholmod);
290 | 
291 |   // Handle the special case where the QR part is empty.
292 |   if (factor == nullptr) {
293 |     // NO QR part!
294 |     double one = 1, zero = 0;
295 |     cholmod_dense* b_reduced = cholmod_l_allocate_dense(
296 |         A_rt->nrow, 1, A_rt->nrow, CHOLMOD_REAL, cholmod);
297 |     CHECK(b_reduced != nullptr) << "cholmod_l_allocate_dense failed.";
298 | 
299 |     const int status = cholmod_l_sdmult(A_rt, 0, &one, &zero, b, b_reduced,
300 |                                         cholmod);
301 |     CHECK(status) << "cholmod_l_sdmult failed";
302 |     return b_reduced;
303 |   } else {
304 |     CHECK(factor->QRsym != nullptr) << "Run symbolic factorization first.";
305 |     CHECK(factor->QRnum != nullptr) << "Run QR decomposition first.";
306 |   }
307 |   CHECK_NOTNULL(factor);
308 |   CHECK_NOTNULL(A_rtQ);
309 | 
310 |   cholmod_sparse* b_sparse = cholmod_l_dense_to_sparse(b, 1, cholmod);
311 |   CHECK(b_sparse != nullptr) << "cholmod_l_dense_to_sparse failed.";
312 | 
313 |   cholmod_sparse* A_rtb = cholmod_l_ssmult(A_rt, b_sparse, 0, 1, 1, cholmod);
314 |   CHECK(A_rtb != nullptr) << "cholmod_l_ssmult failed.";
315 | 
316 |   cholmod_sparse* QtbFull = SuiteSparseQR_qmult<double>(SPQR_QTX, factor,
317 |     b_sparse, cholmod);
318 |   CHECK(QtbFull != nullptr) << "SuiteSparseQR_qmult failed.";
319 |   cholmod_l_free_sparse(&b_sparse, cholmod);
320 | 
321 |   cholmod_sparse* Qtb;
322 |   Qtb = rowSubmatrix(QtbFull, 0, factor->QRsym->n - 1, cholmod);
323 |   cholmod_l_free_sparse(&QtbFull, cholmod);
324 | 
325 |   cholmod_sparse* A_rtQQtb = cholmod_l_ssmult(A_rtQ, Qtb, 0, 1, 1, cholmod);
326 |   CHECK(A_rtQQtb != nullptr) << "cholmod_l_ssmult failed.";
327 |   cholmod_l_free_sparse(&Qtb, cholmod);
328 | 
329 |   double alpha[2];
330 |   alpha[0] = 1.0;
331 |   double beta[2];
332 |   beta[0] = -1.0;
333 | 
334 |   cholmod_sparse* b_reduced_sparse = cholmod_l_add(A_rtb, A_rtQQtb, alpha,
335 |     beta, 1, 1, cholmod);
336 |   CHECK(b_reduced_sparse != nullptr) << "cholmod_l_add failed.";
337 |   cholmod_l_free_sparse(&A_rtb, cholmod);
338 |   cholmod_l_free_sparse(&A_rtQQtb, cholmod);
339 | 
340 |   cholmod_dense* b_reduced = cholmod_l_sparse_to_dense(b_reduced_sparse,
341 |     cholmod);
342 |   CHECK(b_reduced != nullptr) << "cholmod_l_sparse_to_dense failed.";
343 |   cholmod_l_free_sparse(&b_reduced_sparse, cholmod);
344 |   return b_reduced;
345 | }
346 | 
347 | void analyzeSVD(const cholmod_sparse* Omega, Eigen::VectorXd& sv,
348 |     Eigen::MatrixXd& U, Eigen::MatrixXd& V) {
349 |   CHECK_NOTNULL(Omega);
350 | 
351 |   Eigen::MatrixXd OmegaDense;
352 |   cholmodSparseToEigenDenseCopy(Omega, OmegaDense);
353 |   const Eigen::JacobiSVD<Eigen::MatrixXd> svd(OmegaDense,
354 |     Eigen::ComputeThinU | Eigen::ComputeThinV);
355 |   U = svd.matrixU();
356 |   V = svd.matrixV();
357 |   sv = svd.singularValues();
358 | }
359 | 
360 | void solveSVD(const cholmod_dense* b, const Eigen::VectorXd& sv, const
361 |     Eigen::MatrixXd& U, const Eigen::MatrixXd& V, std::ptrdiff_t rank,
362 |     Eigen::VectorXd& x) {
363 |   CHECK_NOTNULL(b);
364 |   CHECK_LE(rank, V.cols());
365 |   CHECK_EQ(V.cols(), sv.rows());
366 |   CHECK_EQ(U.rows(), static_cast<std::ptrdiff_t>(b->nrow));
367 | 
368 |   Eigen::Map<const Eigen::VectorXd> b_eigen(
369 |     reinterpret_cast<const double*>(b->x), b->nrow);
370 |   x = V.leftCols(rank) * sv.head(rank).asDiagonal().inverse() *
371 |     U.leftCols(rank).adjoint() * b_eigen;
372 | }
373 | 
374 | cholmod_dense* solveQR(SuiteSparseQR_factorization<double>* factor,
375 |     cholmod_dense* b, cholmod_sparse* A_r, const Eigen::VectorXd& x_r,
376 |     cholmod_common* cholmod) {
377 |   CHECK_NOTNULL(factor);
378 |   CHECK(factor->QRsym != nullptr) << "Run symbolic factorization first.";
379 |   CHECK(factor->QRnum != nullptr) << "Run QR decomposition first.";
380 |   CHECK_NOTNULL(b);
381 |   CHECK_NOTNULL(cholmod);
382 | 
383 |   cholmod_dense* QtbmA_rx_r = nullptr;
384 |   if (A_r != nullptr) {
385 |     CHECK_EQ(x_r.size(), static_cast<int>(A_r->ncol))
386 |       << "Dimension mismatch between x_r and A_r.";
387 | 
388 |     cholmod_dense x_r_cholmod;
389 |     eigenDenseToCholmodDenseView(x_r, &x_r_cholmod);
390 |     cholmod_dense* A_rx_r = cholmod_l_allocate_dense(A_r->nrow, 1, A_r->nrow,
391 |       CHOLMOD_REAL, cholmod);
392 |     CHECK(A_rx_r != nullptr) << "cholmod_l_allocate_dense failed.";
393 | 
394 |     double alpha[2];
395 |     alpha[0] = 1.0;
396 |     double beta[2];
397 |     beta[0] = 0.0;
398 |     const bool success = cholmod_l_sdmult(A_r, 0, alpha, beta, &x_r_cholmod,
399 |                                           A_rx_r, cholmod);
400 |     CHECK(success) << "cholmod_l_sdmult failed.";
401 | 
402 |     Eigen::Map<const Eigen::VectorXd> bEigen(
403 |       reinterpret_cast<const double*>(b->x), b->nrow);
404 |     Eigen::Map<const Eigen::VectorXd> A_rx_rEigen(
405 |       reinterpret_cast<const double*>(A_rx_r->x), A_rx_r->nrow);
406 |     const Eigen::VectorXd bmA_rx_rEigen = bEigen - A_rx_rEigen;
407 |     cholmod_l_free_dense(&A_rx_r, cholmod);
408 |     cholmod_dense bmA_rx_r;
409 |     eigenDenseToCholmodDenseView(bmA_rx_rEigen, &bmA_rx_r);
410 |     QtbmA_rx_r = SuiteSparseQR_qmult<double>(SPQR_QTX, factor, &bmA_rx_r,
411 |                                              cholmod);
412 |   } else {
413 |     QtbmA_rx_r = SuiteSparseQR_qmult<double>(SPQR_QTX, factor, b, cholmod);
414 |   }
415 |   CHECK(QtbmA_rx_r != nullptr) << "SuiteSparseQR_qmult failed.";
416 | 
417 |   cholmod_dense* x_l = SuiteSparseQR_solve<double>(SPQR_RETX_EQUALS_B,
418 |     factor, QtbmA_rx_r, cholmod);
419 |   CHECK(x_l != nullptr) << "SuiteSparseQR_solve failed.";
420 |   cholmod_l_free_dense(&QtbmA_rx_r, cholmod);
421 |   return x_l;
422 | }
423 | 
424 | }  // namespace truncated_svd_solver
425 | 


--------------------------------------------------------------------------------