void Cholesky (const DenseMatrix & a,
DenseMatrix & l, Vector & d)
{
// Factors A = L D L^T
double x;
int i, j, k;
int n = a.Height();
// (*testout) << "a = " << a << endl;
l = a;
for (i = 1; i <= n; i++)
{
for (j = i; j <= n; j++)
{
x = l.Get(i, j);
for (k = 1; k < i; k++)
x -= l.Get(i, k) * l.Get(j, k) * d.Get(k);
if (i == j)
{
d.Elem(i) = x;
}
else
{
l.Elem(j, i) = x / d.Get(k);
}
}
}
for (i = 1; i <= n; i++)
{
l.Elem(i, i) = 1;
for (j = i+1; j <= n; j++)
l.Elem(i, j) = 0;
}
/*
// Multiply:
(*testout) << "multiplied factors: " << endl;
for (i = 1; i <= n; i++)
for (j = 1; j <= n; j++)
{
x = 0;
for (k = 1; k <= n; k++)
x += l.Get(i, k) * l.Get(j, k) * d.Get(k);
(*testout) << x << " ";
}
(*testout) << endl;
*/
}
void OptimalActiveSetW(DenseMatrix<T>& W, // m x 2
const DenseMatrix<T>& HHt, // 2 x 2
const DenseMatrix<T>& AHt) // m x 2
{
// Remove negative entries in each of the m rows of matrix W, but
// do so in a manner that minimizes the overall objective function.
// Each row can be considered in isolation.
//
// The problem for each row i of W, w = W(i,:), is as follows:
//
// min_{w>=0} |wH - a|^2
//
// This is minimized when w'* = Ha'/(hh'), or w* = aH'/(hh').
// Expressed in terms of the individual elements, this becomes:
//
// w*[0] = ah'[0] / (h[0]h'[0])
// w*[1] = ah'[1] / (h[1]h'[1])
// If both elements of w* are nonnegative, this is the optimal solution.
// If any elements are negative, the Rank2 algorithm is used to adjust
// their values.
int height = W.Height();
const T hht_00 = HHt.Get(0,0);
const T hht_11 = HHt.Get(1,1);
const T inv_hht_00 = T(1.0) / hht_00;
const T inv_hht_11 = T(1.0) / hht_11;
const T sqrt_hht_00 = sqrt(hht_00);
const T sqrt_hht_11 = sqrt(hht_11);
for (int i=0; i<height; ++i)
{
T v1 = AHt.Get(i, 0) * inv_hht_00;
T v2 = AHt.Get(i, 1) * inv_hht_11;
T vv1 = v1 * sqrt_hht_00;
T vv2 = v2 * sqrt_hht_11;
if (vv1 >= vv2)
v2 = T(0);
else
v1 = T(0);
if ( (W.Get(i, 0) <= T(0)) || (W.Get(i, 1) <= T(0)))
{
W.Set(i, 0, v1);
W.Set(i, 1, v2);
}
}
}
void OptimalActiveSetH(DenseMatrix<T>& H, // 2 x n
const DenseMatrix<T>& WtW, // 2 x 2
const DenseMatrix<T>& WtA) // 2 x n
{
// Remove negative entries in each of the n columns of matrix H, but
// do so in a manner that minimizes the overall objective function.
// Each column can be considered in isolation.
//
// The problem for each column i of H, h = H(:,i), is as follows:
//
// min_{h>=0} |Wh - a|^2
//
// This is minimized when h* = w'a / (w'w). Expressed in terms of the
// individual elements, this becomes:
//
// h*[0] = w[0]'a / (w[0]'w[0])
// h*[1] = w[1]'a / (w[1]'w[1])
// If both elements of h* are nonnegative, this is the optimal solution.
// If any elements are negative, the Rank2 algorithm is used to adjust
// their values.
int width = H.Width();
const T wtw_00 = WtW.Get(0,0);
const T wtw_11 = WtW.Get(1,1);
const T inv_wtw_00 = T(1.0) / wtw_00;
const T inv_wtw_11 = T(1.0) / wtw_11;
const T sqrt_wtw_00 = sqrt(wtw_00);
const T sqrt_wtw_11 = sqrt(wtw_11);
for (int i=0; i<width; ++i)
{
T v1 = WtA.Get(0,i) * inv_wtw_00;
T v2 = WtA.Get(1,i) * inv_wtw_11;
T vv1 = v1 * sqrt_wtw_00;
T vv2 = v2 * sqrt_wtw_11;
if (vv1 >= vv2)
v2 = T(0);
else
v1 = T(0);
if ( (H.Get(0,i) <= T(0)) || (H.Get(1,i) <= T(0)))
{
H.Set(0, i, v1);
H.Set(1, i, v2);
}
}
}
void MakeDiagonallyDominant(DenseMatrix<T>& M)
{
// Make the diagonal element larger than the row sum, to ensure that
// the matrix is nonsingular. All entries in the matrix are nonnegative,
// so no absolute values are needed.
for (int r=0; r<M.Height(); ++r)
{
T row_sum = 0.0;
for (int c=0; c<M.Width(); ++c)
row_sum += M.Get(r, c);
M.Set(r, r, row_sum + T(1));
}
}
bool HasNaNs(const DenseMatrix<T>& M)
{
// returns true if any matrix element is NaN
int height = M.Height();
int width = M.Width();
for (int c=0; c<width; ++c)
{
for (int r=0; r<height; ++r)
{
T elt = M.Get(r, c);
if (std::isnan(elt))
return true;
}
}
return false;
}
bool SystemSolveH(DenseMatrix<T>& X, // H 2 x n
DenseMatrix<T>& A, // WtW 2 x 2
DenseMatrix<T>& B) // WtA 2 x n
{
// This function solves the system A*X = B for X. The columns of X and B
// are treated as separate subproblems. Each subproblem can be expressed
// as follows, for column i of X and B (the ' means transpose):
//
// A * [x0 x1]' = [b0 b1]'
//
// The solution proceeds by transforming matrix A to upper triangular form
// via a fast Givens rotation. The resulting triangular system for the
// two unknowns in each column of X is then solved by backsubstitution.
//
// The fast Givens method relies on the fact that matrix A can be
// expressed as the product of a diagonal matrix D and another matrix Y,
// and that this form is preserved under the rotation. If J is the
// Givens rotation matrix, then:
//
// AX = B
// JAX = JB J is the Givens rotation matrix
// (JA)X = JB letting A = D1Y1
// (JD1Y1)X = JB
// (D2Y2)X = JB
//
// The matrix D1 is the identity matrix, and the matrix Y1 is the
// original matrix A. The matrix J has the form [c -s; c s] in Matlab
// notation. NOTE: with this convention, the tangent has a minus sign:
//
// t = -A(1, 0) / A(0, 0)
//
// Matrix A has the form [a b; c d], and after the rotation it is
// transformed to [a2 b2; 0 d2].
//
// There are two forms for the D2 and Y2 matrices, depending on whether
// cosines or sines are 'factored out'. If |A00| >= |A01|, then the
// upper left element is factored out and the 'cosine' formulation is
// used. If |A00| < |A01|, then the upper right element is factored
// out and the 'sine' formulation is used.
//
// A general reference for this code is the paper 'Fast Plane Rotations
// with Dynamic Scaling', by A. Anda and H. Park, SIAM Journal on Matrix
// Analysis and Applications, Vol 15, no. 1, pp. 162-174, Jan. 1994.
int n = B.Width();
T abs_A00 = std::abs(A.Get(0, 0));
T abs_A01 = std::abs(A.Get(0, 1));
const T epsilon = std::numeric_limits<T>::epsilon();
if ( (abs_A00 < epsilon) && (abs_A01 < epsilon))
{
std::cerr << "SystemSolveH: singular matrix" << std::endl;
return false;
}
T a2, b2, d2, e2, f2, inv_a2, inv_d2, x_1;
if (abs_A00 >= abs_A01)
{
// use 'cosine' formulation; t is the tangent
T t = -A.Get(1,0) / A.Get(0,0);
a2 = A.Get(0,0) - t*A.Get(1,0);
b2 = A.Get(0,1) - t*A.Get(1,1);
d2 = A.Get(1,1) + t*A.Get(0,1);
// precompute 1/a2 and 1/d2 to avoid repeated division
inv_a2 = T(1.0) / a2;
inv_d2 = T(1.0) / d2;
// a2 is guaranteed to be positive
if (std::abs(d2/a2) < epsilon)
return false;
// solve the upper triangular systems by backsubstitution
for (int i=0; i<n; ++i)
{
e2 = B.Get(0,i) - t*B.Get(1,i);
f2 = B.Get(1,i) + t*B.Get(0,i);
x_1 = f2 * inv_d2;
X.Set(1, i, x_1);
X.Set(0, i, (e2 - b2*x_1)*inv_a2);
}
}
else
{
// use 'sine' formulation; ct is the cotangent
T ct = -A.Get(0,0) / A.Get(1,0);
a2 = -A.Get(1,0) + ct*A.Get(0,0);
b2 = -A.Get(1,1) + ct*A.Get(0,1);
d2 = A.Get(0,1) + ct*A.Get(1,1);
// precompute 1/a2 and 1/d2 to avoid repeated division
inv_a2 = T(1.0) / a2;
inv_d2 = T(1.0) / d2;
// a2 is guaranteed to be positive
if (std::abs(d2/a2) < epsilon)
return false;
// solve the upper triangular systems by backsubstitution
for (int i=0; i<n; ++i)
{
e2 = -B.Get(1,i) + ct*B.Get(0,i);
f2 = B.Get(0,i) + ct*B.Get(1,i);
x_1 = f2 * inv_d2;
X.Set(1, i, x_1);
X.Set(0, i, (e2 - b2*x_1) * inv_a2);
}
}
return true;
}
bool SystemSolveW(DenseMatrix<T>& X, // m x 2
DenseMatrix<T>& A, // 2 x 2
DenseMatrix<T>& B) // m x 2
{
// Solve XA = B; use the code from the previous solver, but transpose
// everything and change t to -t.
const T eps = std::numeric_limits<double>::epsilon();
int m = B.Height();
if (std::abs(A.Get(0,0)) < eps && std::abs(A.Get(0,1)) < eps)
{
std::cerr << "SystemSolveW: singular matrix" << std::endl;
return false;
}
T a2, b2, d2, e2, f2, x_1;
T inv_a2, inv_d2;
if (std::abs(A.Get(0,0)) >= std::abs(A.Get(0, 1)))
{
// use 'cosine' formulation; t is the tangent
T t = A.Get(0, 1) / A.Get(0,0);
a2 = A.Get(0,0) + t*A.Get(0,1);
b2 = A.Get(1,0) + t*A.Get(1,1);
d2 = A.Get(1,1) - t*A.Get(1,0);
// precompute 1/a2 and 1/d2 to avoid repeated division
inv_a2 = T(1.0) / a2;
inv_d2 = T(1.0) / d2;
// a2 is guaranteed to be positive
if (std::abs(d2/a2) < eps)
return false;
// solve the upper triangular systems by backsubstitution
for (int i=0; i<m; ++i)
{
e2 = B.Get(i,0) + t*B.Get(i,1);
f2 = B.Get(i,1) - t*B.Get(i,0);
x_1 = f2 * inv_d2;
X.Set(i, 1, x_1);
X.Set(i, 0, (e2 - b2*x_1)*inv_a2);
}
}
else
{
// use 'sine' formulation; ct is the cotangent
T ct = A.Get(0,0) / A.Get(0,1);
a2 = -A.Get(0,1) - ct*A.Get(0,0);
b2 = -A.Get(1,1) - ct*A.Get(1,0);
d2 = A.Get(1,0) - ct*A.Get(1,1);
// precompute 1/a2 and 1/d2 to avoid repeated division
inv_a2 = T(1.0) / a2;
inv_d2 = T(1.0) / d2;
// a2 is guaranteed to be positive
if (std::abs(d2/a2) < eps)
return false;
// solve the upper triangular systems by backsubstitution
for (int i=0; i<m; ++i)
{
e2 = -B.Get(i,1) - ct*B.Get(i,0);
f2 = B.Get(i,0) - ct*B.Get(i,1);
x_1 = f2 * inv_d2;
X.Set(i, 1, x_1);
X.Set(i, 0, (e2 - b2*x_1) * inv_a2);
}
}
return true;
}