1 // This is core/vnl/algo/vnl_cholesky.cxx
2 //:
3 // \file
4 // vnl_cholesky
5 // \author Andrew W. Fitzgibbon, Oxford RRG
6 // Created: 08 Dec 96
7 //
8 //-----------------------------------------------------------------------------
9
10 #include <cmath>
11 #include <cassert>
12 #include <iostream>
13 #include "vnl_cholesky.h"
14 #include <vnl/algo/vnl_netlib.h> // dpofa_(), dposl_(), dpoco_(), dpodi_()
15
16 //: Cholesky decomposition.
17 // Make cholesky decomposition of M optionally computing
18 // the reciprocal condition number. If mode is estimate_condition, the
19 // condition number and an approximate nullspace are estimated, at a cost
20 // of a factor of (1 + 18/n). Here's a table of 1 + 18/n:
21 // \verbatim
22 // n: 3 5 10 50 100 500 1000
23 // slowdown: 7.0 4.6 2.8 1.4 1.18 1.04 1.02
24 // \endverbatim
25
vnl_cholesky(vnl_matrix<double> const & M,Operation mode)26 vnl_cholesky::vnl_cholesky(vnl_matrix<double> const & M, Operation mode):
27 A_(M)
28 {
29 long n = M.columns();
30 assert(n == (int)(M.rows()));
31 num_dims_rank_def_ = -1;
32 if (std::fabs(M(0,n-1) - M(n-1,0)) > 1e-8) {
33 std::cerr << "vnl_cholesky: WARNING: non-symmetric: " << M << std::endl;
34 }
35
36 if (mode != estimate_condition) {
37 // Quick factorization
38 v3p_netlib_dpofa_(A_.data_block(), &n, &n, &num_dims_rank_def_);
39 if (mode == verbose && num_dims_rank_def_ != 0)
40 std::cerr << "vnl_cholesky: " << num_dims_rank_def_ << " dimensions of non-posdeffness\n";
41 }
42 else {
43 vnl_vector<double> nullvec(n);
44 v3p_netlib_dpoco_(A_.data_block(), &n, &n, &rcond_, nullvec.data_block(), &num_dims_rank_def_);
45 if (num_dims_rank_def_ != 0)
46 std::cerr << "vnl_cholesky: rcond=" << rcond_ << " so " << num_dims_rank_def_ << " dimensions of non-posdeffness\n";
47 }
48 }
49
50 //: Solve least squares problem M x = b.
51 // The right-hand-side std::vector x may be b,
52 // which will give a fractional increase in speed.
solve(vnl_vector<double> const & b,vnl_vector<double> * x) const53 void vnl_cholesky::solve(vnl_vector<double> const& b, vnl_vector<double>* x) const
54 {
55 assert(b.size() == A_.columns());
56
57 *x = b;
58 long n = A_.columns();
59 v3p_netlib_dposl_(A_.data_block(), &n, &n, x->data_block());
60 }
61
62 //: Solve least squares problem M x = b.
solve(vnl_vector<double> const & b) const63 vnl_vector<double> vnl_cholesky::solve(vnl_vector<double> const& b) const
64 {
65 assert(b.size() == A_.columns());
66
67 long n = A_.columns();
68 vnl_vector<double> ret = b;
69 v3p_netlib_dposl_(A_.data_block(), &n, &n, ret.data_block());
70 return ret;
71 }
72
73 //: Compute determinant.
determinant() const74 double vnl_cholesky::determinant() const
75 {
76 long n = A_.columns();
77 vnl_matrix<double> I = A_;
78 double det[2];
79 long job = 10;
80 v3p_netlib_dpodi_(I.data_block(), &n, &n, det, &job);
81 return det[0] * std::pow(10.0, det[1]);
82 }
83
84 // : Compute inverse. Not efficient.
inverse() const85 vnl_matrix<double> vnl_cholesky::inverse() const
86 {
87 if (num_dims_rank_def_) {
88 std::cerr << "vnl_cholesky: Calling inverse() on rank-deficient matrix\n";
89 return vnl_matrix<double>();
90 }
91
92 long n = A_.columns();
93 vnl_matrix<double> I = A_;
94 long job = 01;
95 v3p_netlib_dpodi_(I.data_block(), &n, &n, nullptr, &job);
96
97 // Copy lower triangle into upper
98 for (int i = 0; i < n; ++i)
99 for (int j = i+1; j < n; ++j)
100 I(i,j) = I(j,i);
101
102 return I;
103 }
104
105 //: Return lower-triangular factor.
lower_triangle() const106 vnl_matrix<double> vnl_cholesky::lower_triangle() const
107 {
108 unsigned n = A_.columns();
109 vnl_matrix<double> L(n,n);
110 // Zap upper triangle and transpose
111 for (unsigned i = 0; i < n; ++i) {
112 L(i,i) = A_(i,i);
113 for (unsigned j = i+1; j < n; ++j) {
114 L(j,i) = A_(j,i);
115 L(i,j) = 0;
116 }
117 }
118 return L;
119 }
120
121
122 //: Return upper-triangular factor.
upper_triangle() const123 vnl_matrix<double> vnl_cholesky::upper_triangle() const
124 {
125 unsigned n = A_.columns();
126 vnl_matrix<double> U(n,n);
127 // Zap lower triangle and transpose
128 for (unsigned i = 0; i < n; ++i) {
129 U(i,i) = A_(i,i);
130 for (unsigned j = i+1; j < n; ++j) {
131 U(i,j) = A_(j,i);
132 U(j,i) = 0;
133 }
134 }
135 return U;
136 }
137