/// <summary>Computes A*inv(H)*A' and stores result in ans</summary>
/// <param name="A">A matrix, mxn</param>
/// <param name="H">H matrix, nxn</param>
/// <param name="ans">answer, mxm</param>
/// <param name="temp">Temporary matrix for the operation, size nxm</param>
/// <param name="refine">Refine linear system solution?</param>
public static floatSymPosDefMatrix ComputeAinvHTranspA(floatMatrix A, floatSymPosDefMatrix H, ref floatSymPosDefMatrix ans, ref floatMatrix temp, bool refine)
{
int m=A.Rows;
int n=A.Cols;
if (ans == null) ans = new floatSymPosDefMatrix(m);
if (H.getN != n) throw new Exception("Matrix sizes not compatible");
if (ans.getN != m) throw new Exception("Answer size not compatible");
H.LinearSolve(A, refine, ref temp);
//Go on to multiplying A*temp
if (CLCalc.CLAcceleration == CLCalc.CLAccelerationType.UsingCL)
{
kernelComputeAinvHAt.Execute(new CLCalc.Program.MemoryObject[] { A.CLValues, A.CLDim, temp.CLValues, ans.CLValues }, (m * (m + 1)) >> 1);
ans.IsCholeskyFactorized = false;
}
else
{
for (int p = 0; p < m; p++)
{
int np = n * p;
for (int q = 0; q <= p; q++)
{
int nq = n * q;
float val = 0;
for (int k = 0; k < n; k++)
{
val += A.Values[k + np] * temp.Values[k + nq];
}
ans.Values[((p * (1 + p)) >> 1) + q] = val;
}
}
}
return ans;
}
/// <summary>Symmetric positive definite product with vector, Msym*v. resp gets constructed if ==null </summary>
private static void MultiplyNoCL(floatSymPosDefMatrix M, floatVector v, ref floatVector resp)
{
for (int i = 0; i < M.getN; i++)
{
float val = 0;
for (int k = 0; k < M.getN; k++)
{
val += M[i, k] * v.Values[k];
}
resp.Values[i] = val;
}
}
/// <summary>Computes transpose(A)*A and transpose(A)*b weighted by W using OpenCL. Lambda is regularization term</summary>
private static floatSymPosDefMatrix AuxLSAtACL(floatMatrix A, floatDiag W, floatVector lambda, ref floatSymPosDefMatrix AtA)
{
if (AtA == null || AtA.CLValues.OriginalVarLength != (A.Cols * (A.Cols + 1)) >> 1)
{
AtA = new floatSymPosDefMatrix(new float[(A.Cols * (A.Cols + 1)) >> 1]);
}
CLCalc.Program.Variable[] args = new CLCalc.Program.Variable[] { A.CLValues, A.CLDim, W.CLValues, AtA.CLValues, lambda.CLValues };
kernelComputeAtWA.Execute(args, AtA.CLValues.OriginalVarLength);
//Just modified values in CL memory, matrix is no longer Cholesky factorized
AtA.IsCholeskyFactorized = false;
return AtA;
}
/// <summary>Computes transpose(A)*A and transpose(A)*b weighted by W</summary>
/// <param name="A">Original matrix</param>
/// <param name="W">Measurement weight vector</param>
/// <param name="lambda">Regularization term</param>
/// <param name="AtA">Answer, A transpose times A</param>
private static floatSymPosDefMatrix AuxLeastSquaresAtAnoCL(floatMatrix A, floatDiag W, floatVector lambda, ref floatSymPosDefMatrix AtA)
{
//A (mxn), AtA (nxn) positive semidef symmetric
int m = A.Rows;
int n = A.Cols;
if (AtA == null) AtA = new floatSymPosDefMatrix(new float[(n * (n + 1)) >> 1]);
if (W != null)
{
for (int i = 0; i < n; i++)
{
for (int j = 0; j <= i; j++)
{
double val = 0;
for (int k = 0; k < m; k++)
{
val += A[k, i] * A[k, j] * W.Values[k];
}
AtA.Values[((i * (i + 1)) >> 1) + j] = (float)val;
}
}
}
else
{
for (int i = 0; i < n; i++)
{
for (int j = 0; j <= i; j++)
{
double val = 0;
for (int k = 0; k < m; k++)
{
val += A[k, i] * A[k, j];
}
AtA.Values[((i * (i + 1)) >> 1) + j] = (float)val;
}
}
}
//regularization term
for (int i = 0; i < n; i++)
{
AtA.Values[((i * (i + 1)) >> 1) + i] += lambda.Values[i];
}
return AtA;
}
/// <summary>Symmetric positive definite product with vector, Msym*v. resp gets constructed if ==null </summary>
public static floatVector SymPosDefSymMatrVecMultiply(floatSymPosDefMatrix M, floatVector v, ref floatVector ans)
{
if (ans == null) ans = new floatVector(new float[v.Length]);
if (ans.Length != v.Length) throw new Exception("ans and v should have the same length");
if (v.Length != M.getN) throw new Exception("Invalid vector length to multiply by this matrix");
if (CLCalc.CLAcceleration != CLCalc.CLAccelerationType.UsingCL)
{
MultiplyNoCL(M, v, ref ans);
return ans;
}
if (!M.IsMatrixInClMemoryUpdated)
{
M.CLValues.WriteToDevice(M.Values);
M.IsMatrixInClMemoryUpdated = true;
}
kernelSymMatrVecMultiply.Execute(new CLCalc.Program.Variable[] { M.CLValues, v.CLValues, ans.CLValues }, M.getN);
return ans;
}
/// <summary>Symmetric positive definite product with matrix transpose, Msym*At. ans gets constructed if ==null </summary>
public static floatMatrix SymPosDefMatrMatrMultiply(floatSymPosDefMatrix M, floatMatrix A, ref floatMatrix ans)
{
if (ans == null) ans = new floatMatrix(new float[A.Cols, M.getN]);
if (CLCalc.CLAcceleration == CLCalc.CLAccelerationType.UsingCL)
{
if (!M.IsMatrixInClMemoryUpdated) M.CLValues.WriteToDevice(M.Values);
kernelSymMatrMatrMultiply.Execute(new CLCalc.Program.MemoryObject[] { M.CLValues, A.CLValues, ans.CLValues }, new int[] { A.Rows, A.Cols });
}
else
{
for (int j = 0; j < A.Cols; j++)
{
for (int i = 0; i < M.getN; i++)
{
float val = 0;
for (int k = 0; k < M.getN; k++)
{
val += M[i, k] * A.Values[k + j * A.Rows];
}
ans.Values[i + j * A.Rows] = val;
}
}
}
return ans;
}
/// <summary>Computes transpose(A)*A using weights W</summary>
/// <param name="A">Original matrix</param>
/// <param name="W">Measurement weight vector</param>
/// <param name="lambda">Regularization term</param>
/// <param name="AtA">Answer, A transpose times A</param>
public static floatSymPosDefMatrix MatrTranspMatrProd(floatMatrix A, floatDiag W, floatVector lambda, ref floatSymPosDefMatrix AtA)
{
if (CLCalc.CLAcceleration == CLCalc.CLAccelerationType.UsingCL)
{
return AuxLSAtACL(A, W, lambda, ref AtA);
}
else
{
return AuxLeastSquaresAtAnoCL(A, W, lambda, ref AtA);
}
}
/// <summary>Computes transpose(A)*A</summary>
/// <param name="A">Original matrix</param>
/// <param name="lambda">Regularization term</param>
/// <param name="AtA">Answer, A transpose times A</param>
public static floatSymPosDefMatrix MatrTranspMatrProd(floatMatrix A, floatVector lambda, ref floatSymPosDefMatrix AtA)
{
return MatrTranspMatrProd(A, null, lambda, ref AtA);
}
/// <summary>Creates vector from M elements sequentially</summary>
/// <param name="symM">Symmetric matrix to use</param>
public floatVector(floatSymPosDefMatrix symM)
{
this.CLValues = symM.CLValues;
this.Values = symM.Values;
//Since I'm probably going to modify the matrix, I want a new Cholesky factorization
//if I ever call a LinearSolve
symM.IsCholeskyFactorized = false;
if (CLCalc.CLAcceleration == CLCalc.CLAccelerationType.UsingCL)
{
CLCoef = new CLCalc.Program.Variable(new float[1]);
}
}
/// <summary>Returns the identity matrix</summary>
/// <param name="n">Matrix dimension nxn</param>
public static floatSymPosDefMatrix Identity(int n)
{
floatSymPosDefMatrix M = new floatSymPosDefMatrix(n);
for (int i = 0; i < n; i++) M[i, i] = 1;
return M;
}
