udu.cpp

良好的代码实现

}


RowMatrix::value_type UdUfactor (RowMatrix& UD, const SymMatrix& M)
/*
 * Modified upper triangular Cholesky factor of a
 * Positive definate or Semi-definate Matrix M
 * Wraps UdUfactor for non in place factorisation
 * Output:
 *    UD the UdU' factorisation of M with strict lower triangle zero
 * Return:
 *    see in-place UdUfactor
 */
{
	noalias(UD) = M;
	RowMatrix::value_type rcond = UdUfactor (UD, M.size1());

	Lzero (UD);	// Zero lower triangle ignored by UdUfactor
	return rcond;
}


LTriMatrix::value_type LdLfactor (LTriMatrix& LD, const SymMatrix& M)
/*
 * Modified lower triangular Cholesky factor of a
 * Positive definate or Semi-definate Matrix M
 * Wraps LdLfactor for non in place factorisation
 * Output:
 *    LD the LdL' factorisation of M
 * Return:
 *    see in-place LdLfactor
 */
{
	noalias(LD) = M;
	LTriMatrix::value_type rcond = LdLfactor (LD, M.size1());

	return rcond;
}


UTriMatrix::value_type UCfactor (UTriMatrix& UC, const SymMatrix& M)
/*
 * Upper triangular Cholesky factor of a
 * Positive definate or Semi-definate Matrix M
 * Wraps UCfactor for non in place factorisation
 * Output:
 *    UC the UC*UC' factorisation of M
 * Return:
 *    see in-place UCfactor
 */
{
	noalias(UC) = UpperTri(M);
	UTriMatrix::value_type rcond = UCfactor (UC, UC.size1());

	return rcond;
}



bool UdUinverse (RowMatrix& UD)
/*
 * In-place (destructive) inversion of diagonal and unit upper triangular matrices in UD
 * BE VERY CAREFUL THIS IS NOT THE INVERSE OF UD
 *  Inversion on d and U is seperate: inv(U)*inv(d)*inv(U') = inv(U'dU) NOT EQUAL inv(UdU')
 * Lower triangle of UD is ignored and unmodified
 * Only diagonal part d can be singular (zero elements), inverse is computed of all elements other then singular
 * Reference: A+G p.223
 *
 * Output:
 *    UD: inv(U), inv(d)
 * Return:
 *    singularity (of d), true iff d has a zero element
 */
{
	std::size_t i,j,k;
	const std::size_t n = UD.size1();
	assert (n == UD.size2());

	// Invert U in place
	if (n > 1)
	{
		i = n-2;
		do {
			RowMatrix::Row UDi(UD,i);
			for (j = n-1; j > i; --j)
			{
				RowMatrix::value_type UDij = - UDi[j];
				for (k = i+1; k < j; ++k)
					UDij -= UDi[k] * UD(k,j);
				UDi[j] = UDij;
			}
		} while (i-- > 0);
	}

	// Invert d in place
	bool singular = false;
	for (i = 0; i < n; ++i)
	{
		// Detect singular element
		if (UD(i,i) != 0)
			UD(i,i) = Float(1) / UD(i,i);
		else
			singular = true;
	}

	return singular;
}


bool UTinverse (UTriMatrix& U)
/*
 * In-place (destructive) inversion of upper triangular matrix in U
 *
 * Output:
 *    U: inv(U)
 * Return:
 *    singularity (of U), true iff diagonal of U has a zero element
 */
{
	const std::size_t n = U.size1();
	assert (n == U.size2());

	bool singular = false;
	// Invert U in place
	if (n > 0)
	{
		std::size_t i = n-1;
		do {
			UTriMatrix::Row Ui(U,i);
			UTriMatrix::value_type d = Ui[i];
			if (d == 0)
			{
				singular = true;
				break;
			}
			d = 1/d;
			Ui[i] = d;

			for (std::size_t j = n-1; j > i; --j)
			{
				UTriMatrix::value_type e = 0.;
				for (std::size_t k = i+1; k <= j; ++k)
					e -= Ui[k] * U(k,j);
				Ui[j] = e*d;
			}
		} while (i-- > 0);
	}

	return singular;
}


void UdUrecompose_transpose (RowMatrix& M)
/*
 * In-place recomposition of Symmetric matrix from U'dU factor store in UD format
 *  Generally used for recomposing result of UdUinverse
 * Note definateness of result depends purely on diagonal(M)
 *  i.e. if d is positive definate (>0) then result is positive definate
 * Reference: A+G p.223
 * In place computation uses simple structure of solution due to triangular zero elements
 *  Defn: R = (U' d) row i , C = U column j   -> M(i,j) = R dot C;
 *  However M(i,j) only dependant R(k<=i), C(k<=j) due to zeros
 *  Therefore in place multiple sequences such k < i <= j
 * Input:
 *    M - U'dU factorisation (UD format)
 * Output:
 *    M - U'dU recomposition (symmetric)
 */
{
	std::size_t i,j,k;
	const std::size_t n = M.size1();
	assert (n == M.size2());

	// Recompose M = (U'dU) in place
	if (n > 0)
	{
		i = n-1;
		do {
			RowMatrix::Row Mi(M,i);
			// (U' d) row i of lower triangle from upper trinagle
			for (j = 0; j < i; ++j)
				Mi[j] = M(j,i) * M(j,j);
			// (U' d) U in place
			j = n-1;
			do { // j>=i
				// Compute matrix product (U'd) row i * U col j
				RowMatrix::value_type Mij = Mi[j];
				if (j > i)					// Optimised handling of 1 in U
					Mij *= Mi[i];
				for (k = 0; k < i; ++k)		// Inner loop k < i <=j, only strict triangular elements
					Mij += Mi[k] * M(k,j);		// M(i,k) element of U'd, M(k,j) element of U
				M(j,i) = Mi[j] = Mij;
			} while (j-- > i);
		} while (i-- > 0);
	}
}


void UdUrecompose (RowMatrix& M)
/*
 * In-place recomposition of Symmetric matrix from UdU' factor store in UD format
 *  See UdUrecompose_transpose()
 * Input:
 *    M - UdU' factorisation (UD format)
 * Output:
 *    M - UdU' recomposition (symmetric)
 */
{
	std::size_t i,j,k;
	const std::size_t n = M.size1();
	assert (n == M.size2());

	// Recompose M = (UdU') in place
	for (i = 0; i < n; ++i)
	{
		RowMatrix::Row Mi(M,i);
		// (d U') col i of lower triangle from upper trinagle
		for (j = i+1; j < n; ++j) {
			RowMatrix::Row Mj(M,j);
			Mj[i] = M(i,j) * Mj[j];
		}
		// U (d U') in place
		for (j = 0; j <= i; ++j)	// j<=i
		{
			// Compute matrix product (U'd) row i * U col j
			RowMatrix::value_type Mij = Mi[j];
			if (j > i)					// Optimised handling of 1 in U
				Mij *= Mi[i];
			for (k = i+1; k < n; ++k)		// Inner loop k > i >=j, only strict triangular elements
				Mij += Mi[k] * M(k,j);		// M(i,k) element of U'd, M(k,j) element of U
			M(j,i) = Mi[j] = Mij;
		}
	}
}


void Lzero (RowMatrix& M)
/*
 * Zero strict lower triangle of Matrix
 */
{
	std::size_t i,j;
	const std::size_t n = M.size1();
	assert (n == M.size2());
	for (i = 1; i < n; ++i)
	{
		RowMatrix::Row Ui(M,i);
		for (j = 0; j < i; ++j)
		{
			Ui[j] = 0;
		}
	}
}

void Uzero (RowMatrix& M)
/*
 * Zero strict upper triangle of Matrix
 */
{
	std::size_t i,j;
	const std::size_t n = M.size1();
	assert (n == M.size2());
	for (i = 0; i < n; ++i)
	{
		RowMatrix::Row Li(M,i);
		for (j = i+1; j < n; ++j)
		{
			Li[j] = 0;
		}
	}
}


void UdUfromUCholesky (RowMatrix& U)
/*
 * Convert a normal upper triangular Cholesky factor into
 * a Modified Cholesky factor.
 * Lower triangle of UD is ignored and unmodified
 * Ignores Columns with zero diagonal element
 *  Correct for zero columns i.e. UD is Cholesky factor of a PSD Matrix
 * Note: There is no inverse to this function toCholesky as square losses the sign
 *
 * Input:
 *    U Normal Cholesky factor (Upper triangular)
 * Output:
 *    U Modified Cholesky factor (UD format)
 */
{
	std::size_t i,j;
	const std::size_t n = U.size1();
	assert (n == U.size2());
	for (j = 0; j < n; ++j)
	{
		RowMatrix::value_type sd = U(j,j);
		U(j,j) = sd*sd;
					// Devide columns by square of non zero diagonal
		if (sd != 0)
		{
			for (i = 0; i < j; ++i)
			{
				U(i,j) /= sd;
			}
		}
	}
}

void UdUseperate (RowMatrix& U, Vec& d, const RowMatrix& UD)
/*
 * Extract the seperate U and d parts of the UD factorisation
 * Output:
 *    U and d parts of UD
 */
{
	std::size_t i,j;
	const std::size_t n = UD.size1();
	assert (n == UD.size2());

	for (j = 0; j < n; ++j)
	{
					// Extract d and set diagonal to 1
		d[j] = UD(j,j);
		RowMatrix::Row Uj(U,j);
		Uj[j] = 1;
		for (i = 0; i < j; ++i)
		{
			U(i,j) = UD(i,j);
			// Zero lower triangle of U
			Uj[i] = 0;
		}
	}
}


/*
 * Function built using UdU factorisation
 */

void UdUrecompose (SymMatrix& X, const RowMatrix& M)
{
						// Abuse X as a RowMatrix
	RowMatrix& X_matrix = X.asRowMatrix();
		// assign elements of common top left block of R into L
	std::size_t top = std::min(X_matrix.size1(), M.size1());
	std::size_t left = std::min(X_matrix.size2(), M.size2());
	X_matrix.sub_matrix(0,top, 0,left) .assign (M.sub_matrix(0,top, 0,left));

	UdUrecompose (X_matrix);
}

SymMatrix::value_type UdUinversePDignoreInfinity (SymMatrix& M)
/*
 * inverse of Positive Definate matrix
 * The inverse is able to deal with infinities on the leading diagonal
 * Input:
 *     M is a symmetric matrix
 * Output:
 *     M inverse of M, only updated if return value >0
 * Return:
 *     reciprocal condition number, -1 if negative, 0 if semi-definate (including zero)
 */
{
					// Abuse as a RowMatrix
	RowMatrix& M_matrix = M.asRowMatrix();					// Must use variant1, variant2 cannot deal with infinity
	SymMatrix::value_type orig_rcond = UdUfactor_variant1 (M_matrix, M_matrix.size1());
					// Ignore the normal rcond and recompute ingnoring infinites
	SymMatrix::value_type rcond = rcond_ignore_infinity_internal (diag(M_matrix));
	assert (rcond == orig_rcond || orig_rcond == 0); (void)orig_rcond;

	// Only invert and recompose if PD
	if (rcond > 0) {
		bool singular = UdUinverse (M_matrix);
		assert (!singular); (void)singular;
		UdUrecompose_transpose (M_matrix);
	}
	return rcond;
}

SymMatrix::value_type UdUinversePD (SymMatrix& M)
/*
 * inverse of Positive Definate matrix
 * Input:
 *     M is a symmetric matrix
 * Output:
 *     M inverse of M, only updated if return value >0
 * Return:
 *     reciprocal condition number, -1 if negative, 0 if semi-definate (including zero)
 */
{
					// Abuse as a RowMatrix
	RowMatrix& M_matrix = M.asRowMatrix();
	SymMatrix::value_type rcond = UdUfactor (M_matrix, M_matrix.size1());
	// Only invert and recompose if PD
	if (rcond > 0) {
		bool singular = UdUinverse (M_matrix);
		assert (!singular); (void)singular;
		UdUrecompose_transpose (M_matrix);
	}
	return rcond;
}

SymMatrix::value_type UdUinversePD (SymMatrix& M, SymMatrix::value_type& detM)
/*
 * As above but also computes determinant of original M if M is PSD
 */
{
					// Abuse as a RowMatrix
	RowMatrix& M_matrix = M.asRowMatrix();
	SymMatrix::value_type rcond = UdUfactor (M_matrix, M_matrix.size1());
	// Only invert and recompose if PD
	if (rcond > 0) {
		detM = UdUdet(M_matrix);
		bool singular = UdUinverse (M_matrix);
		assert (!singular); (void)singular;
		UdUrecompose_transpose (M_matrix);
	}
	return rcond;
}


SymMatrix::value_type UdUinversePD (SymMatrix& MI, const SymMatrix& M)
/*
 * inverse of Positive Definate matrix
 * Input:
 *    M is a symmetric matrix
 * Output:
 *    MI inverse of M, only valid if return value >0
 * Return:
 *    reciprocal condition number, -1 if negative, 0 if semi-definate (including zero)
 */
{
	MI = M;
					// Abuse as a RowMatrix
	RowMatrix& MI_matrix = MI.asRowMatrix();
	SymMatrix::value_type rcond = UdUfactor (MI_matrix, MI_matrix.size1());
	// Only invert and recompose if PD
	if (rcond > 0) {
		bool singular = UdUinverse (MI_matrix);
		assert (!singular); (void)singular;
		UdUrecompose_transpose (MI_matrix);
	}
	return rcond;
}

SymMatrix::value_type UdUinversePD (SymMatrix& MI, SymMatrix::value_type& detM, const SymMatrix& M)
/*
 * As above but also computes determinant of original M if M is PSD
 */
{
	MI = M;
					// Abuse as a RowMatrix
	RowMatrix& MI_matrix = MI.asRowMatrix();
	SymMatrix::value_type rcond = UdUfactor (MI_matrix, MI_matrix.size1());
	if (rcond >= 0) {
		detM = UdUdet (MI_matrix);
					// Only invert and recompose if PD
		if (rcond > 0) {
			detM = UdUdet (MI_matrix);
			bool singular = UdUinverse (MI_matrix);
			assert (!singular); (void)singular;
			UdUrecompose_transpose (MI_matrix);
		}
	}
	return rcond;
}

}//namespace