public static SafeMatrix SolveLinearSystem(SafeMatrix A, SafeMatrix X)
{
double[] a = A.GetElements();
double[] x = X.GetElements();
double[] y = new double[A.ColumnCount * X.ColumnCount];
SolveLinearSystem(a, A.RowCount, A.ColumnCount, x, X.RowCount, X.ColumnCount, y);
SafeMatrix rv = new SafeMatrix(X.RowCount, X.ColumnCount);
rv.SetElements(y);
return rv;
}
/// <summary>
/// Warning: This method is not thread safe!
/// </summary>
public static SafeMatrix SolveLinearSystemFast(SafeMatrix A, SafeMatrix X)
{
if (A.RowCount > 35 * 35)
throw new InvalidOperationException("Not a PSF fitting linear system.");
if (s_NumVariables != A.ColumnCount)
{
LinearSystemFastInitialiseSolution(A.ColumnCount, 35 * 35);
s_NumVariables = A.ColumnCount;
}
double[] a = A.GetElements();
double[] x = X.GetElements();
double[] y = new double[A.ColumnCount * X.ColumnCount];
SolveLinearSystemFast(a, x, A.RowCount, y);
SafeMatrix rv = new SafeMatrix(X.RowCount, X.ColumnCount);
rv.SetElements(y);
return rv;
}
public void Solve()
{
if (m_XValues.Count < 3)
throw new InvalidOperationException("Cannot get a linear fit from less than 3 points.");
SafeMatrix A = new SafeMatrix(m_XValues.Count, 2);
SafeMatrix X = new SafeMatrix(m_XValues.Count, 1);
for (int i = 0; i < m_XValues.Count; i++)
{
A[i, 0] = m_XValues[i];
A[i, 1] = 1;
X[i, 0] = m_YValues[i];
}
SafeMatrix a_T = A.Transpose();
SafeMatrix aa = a_T * A;
SafeMatrix aa_inv = aa.Inverse();
SafeMatrix bx = (aa_inv * a_T) * X;
m_A = bx[0, 0];
m_B = bx[1, 0];
}
// I(x, y) = IBackground + IStarMax * Exp ( -((x - X0)*(x - X0) + (y - Y0)*(y - Y0)) / (r0 * r0))
private void NonLinearFit(uint[,] intensity, bool useNativeMatrix)
{
m_IsSolved = false;
try
{
int full_width = (int)Math.Round(Math.Sqrt(intensity.Length));
m_MatrixSize = full_width;
int half_width = full_width / 2;
m_HalfWidth = half_width;
switch (DataRange)
{
case PSFFittingDataRange.DataRange8Bit:
m_Saturation = Properties.Settings.Default.Saturation8Bit;
break;
case PSFFittingDataRange.DataRange12Bit:
m_Saturation = Properties.Settings.Default.Saturation12Bit;
break;
case PSFFittingDataRange.DataRange14Bit:
m_Saturation = Properties.Settings.Default.Saturation14Bit;
break;
default:
m_Saturation = Properties.Settings.Default.Saturation8Bit;
break;
}
int nonSaturatedPixels = 0;
double IBackground = 0;
double r0 = 4.0;
double IStarMax = 0;
double found_x = half_width;
double found_y = half_width;
for (int iter = NumberIterations; iter > 0; iter--)
{
if (iter == NumberIterations)
{
uint zval = 0;
IBackground = 0.0; /* assume no backgnd intensity at first... */
for (int i = 0; i < full_width; i++)
{
for (int j = 0; j < full_width; j++)
{
if (intensity[i, j] > zval) zval = intensity[i, j];
if (intensity[i, j] < m_Saturation) nonSaturatedPixels++;
}
}
IStarMax = (double)zval - IBackground;
}
double[] dx = new double[full_width];
double[] dy = new double[full_width];
double[] fx = new double[full_width];
double[] fy = new double[full_width];
double r0_squared = r0 * r0;
for (int i = 0; i < full_width; i++)
{
dx[i] = (double)i - found_x;
fx[i] = Math.Exp(-dx[i] * dx[i] / r0_squared);
dy[i] = (double)i - found_y;
fy[i] = Math.Exp(-dy[i] * dy[i] / r0_squared);
}
SafeMatrix A = new SafeMatrix(nonSaturatedPixels, 5);
SafeMatrix X = new SafeMatrix(nonSaturatedPixels, 1);
int index = -1;
for (int i = 0; i < full_width; i++)
{
for (int j = 0; j < full_width; j++)
{
uint zval = intensity[i, j];
if (zval < m_Saturation)
{
index++;
double exp_val = fx[i] * fy[j];
double residual = IBackground + IStarMax * exp_val - (double)zval;
X[index, 0] = -residual;
A[index, 0] = 1.0; /* slope in i0 */
A[index, 1] = exp_val; /* slope in a0 */
A[index, 2] = 2.0 * IStarMax * dx[i] * exp_val / r0_squared;
A[index, 3] = 2.0 * IStarMax * dy[j] * exp_val / r0_squared;
A[index, 4] = 2.0 * IStarMax * (dx[i] * dx[i] + dy[j] * dy[j]) * exp_val / (r0 * r0_squared);
}
}
}
SafeMatrix Y;
if (useNativeMatrix)
{
Y = NativePSFFitting.SolveLinearSystemFast(A, X);
}
else
{
SafeMatrix a_T = A.Transpose();
SafeMatrix aa = a_T * A;
SafeMatrix aa_inv = aa.Inverse();
Y = (aa_inv * a_T) * X;
}
/* we need at least 6 unsaturated pixels to solve 5 params */
if (nonSaturatedPixels > 6) /* Request all pixels to be good! */
{
IBackground += Y[0, 0];
IStarMax += Y[1, 0];
for (int i = 2; i < 4; i++)
{
if (Y[i, 0] > 1.0) Y[i, 0] = 1.0;
if (Y[i, 0] < -1.0) Y[i, 0] = -1.0;
}
found_x += Y[2, 0];
found_y += Y[3, 0];
if (Y[4, 0] > r0 / 10.0) Y[4, 0] = r0 / 10.0;
if (Y[4, 0] < -r0 / 10.0) Y[4, 0] = -r0 / 10.0;
r0 += Y[4, 0];
}
else
{
m_IsSolved = false;
return;
}
}
m_IsSolved = true;
m_IBackground = IBackground;
m_IStarMax = IStarMax;
m_X0 = found_x;
m_Y0 = found_y;
m_R0 = r0;
m_Residuals = new double[full_width, full_width];
for (int x = 0; x < full_width; x++)
for (int y = 0; y < full_width; y++)
{
m_Residuals[x, y] = intensity[x, y] - GetPSFValueInternal(x, y);
}
}
catch (DivideByZeroException)
{ }
}
public SafeMatrix Transpose()
{
SafeMatrix M = new SafeMatrix(m_ColumnCount, m_RowCount);
for (int i = 0; i < m_ColumnCount; i++)
{
for (int j = 0; j < m_RowCount; j++)
{
M.m_Elements[i * M.m_ColumnCount + j] = m_Elements[j * m_ColumnCount + i];
}
}
return M;
}
public SafeMatrix Minor(int row, int col)
{
// THIS IS THE LOW-LEVEL SOLUTION ~ O(n^2)
SafeMatrix buf = new SafeMatrix(m_RowCount - 1, m_ColumnCount - 1);
int r = 0;
int c = 0;
for (int i = 0; i < m_RowCount; i++)
{
if (i != row)
{
for (int j = 0; j < m_ColumnCount; j++)
{
if (j != col)
{
buf.m_Elements[r * buf.m_ColumnCount + c] = m_Elements[i * m_ColumnCount + j];
c++;
}
}
c = 0;
r++;
}
}
return buf;
}
/// <summary>
/// Inverts square matrix as long as det != 0.
/// </summary>
/// <returns>Inverse of matrix.</returns>
public SafeMatrix Inverse()
{
if (!this.IsSquare())
throw new InvalidOperationException("Cannot invert non-square matrix.");
double det = this.Determinant();
if (det == 0)
throw new InvalidOperationException("Cannot invert (nearly) singular matrix.");
SafeMatrix buf = new SafeMatrix(m_ColumnCount, m_ColumnCount);
for (int i = 0; i < m_ColumnCount; i++)
{
for (int j = 0; j < m_ColumnCount; j++)
{
buf.m_Elements[i * buf.m_ColumnCount + j] = (Math.Pow(-1, i + j) * this.Minor(j, i).Determinant()) / det;
}
}
return buf;
}
public SafeMatrix Clone()
{
SafeMatrix A = new SafeMatrix(m_RowCount, m_ColumnCount);
for (int i = 0; i < m_MatrixSize; i++)
A.m_Elements[i] = m_Elements[i];
return A;
}
public static double Dot(SafeMatrix v, int vRow, SafeMatrix w, int wCol)
{
if (v.ColumnCount != w.RowCount)
throw new ArgumentException("Vectors must be of the same length.");
double buf = 0;
for (int i = 0; i < v.ColumnCount; i++)
{
buf += v.m_Elements[vRow * v.m_ColumnCount + i] * w.m_Elements[i * w.m_ColumnCount + wCol];
}
return buf;
}
public static SafeMatrix operator *(double x, SafeMatrix A)
{
SafeMatrix B = new SafeMatrix(A.RowCount, A.ColumnCount);
for (int i = 0; i < A.m_MatrixSize; i++)
{
B.m_Elements[i] = A.m_Elements[i] * x;
}
return B;
}
public static SafeMatrix operator *(SafeMatrix A, SafeMatrix B)
{
if (A.ColumnCount != B.RowCount)
throw new ArgumentException("Inner matrix dimensions must agree.");
SafeMatrix C = new SafeMatrix(A.RowCount, B.ColumnCount);
for (int i = 0; i < A.RowCount; i++)
{
for (int j = 0; j < B.ColumnCount; j++)
{
C.m_Elements[i * C.m_ColumnCount + j] = Dot(A, i, B, j);
}
}
return C;
}
