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 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; }