/// <summary>Find a solution of the equation f(x)=0.</summary>
/// <param name="f">The function to find roots from.</param>
/// <param name="initialGuess">Initial guess of the root.</param>
/// <param name="accuracy">Desired accuracy. The root will be refined until the accuracy or the maximum number of iterations is reached.</param>
/// <param name="maxIterations">Maximum number of iterations. Usually 100.</param>
/// <param name="root">The root that was found, if any. Undefined if the function returns false.</param>
/// <returns>True if a root with the specified accuracy was found, else false.</returns>
public static bool TryFindRoot(Func<double[], double[]> f, double[] initialGuess, double accuracy, int maxIterations, out double[] root)
{
var x = new DenseVector(initialGuess);
double[] y0 = f(initialGuess);
var y = new DenseVector(y0);
double g = y.L2Norm();
Matrix<double> B = CalculateApproximateJacobian(f, initialGuess, y0);
for (int i = 0; i <= maxIterations; i++)
{
var dx = (DenseVector) (-B.LU().Solve(y));
var xnew = x + dx;
var ynew = new DenseVector(f(xnew.Values));
double gnew = ynew.L2Norm();
if (gnew > g)
{
double g2 = g*g;
double scale = g2/(g2 + gnew*gnew);
if (scale == 0.0)
{
scale = 1.0e-4;
}
dx = scale*dx;
xnew = x + dx;
ynew = new DenseVector(f(xnew.Values));
gnew = ynew.L2Norm();
}
if (gnew < accuracy)
{
root = xnew.Values;
return true;
}
// update Jacobian B
DenseVector dF = ynew - y;
Matrix<double> dB = (dF - B.Multiply(dx)).ToColumnMatrix() * dx.Multiply(1.0 / Math.Pow(dx.L2Norm(), 2)).ToRowMatrix();
B = B + dB;
x = xnew;
y = ynew;
g = gnew;
}
root = null;
return false;
}
private string PrepareResults()
{
StringBuilder result = new StringBuilder();
result.Append("State: ");
if(Status == AlgorithmStatus.Finished)
result.Append("Finished");
else if(Status != AlgorithmStatus.Error)
result.Append("Not Finished");
else
result.Append("Error");
result.AppendLine();
result.AppendLine();
if(_linearEstimationDone)
{
if(_inMinimsation)
{
var estimatedParams = _miniAlg.BestResultVector;
CameraMatrix[0, 0] = estimatedParams[0];
CameraMatrix[0, 1] = estimatedParams[1];
CameraMatrix[0, 2] = estimatedParams[2];
CameraMatrix[0, 3] = estimatedParams[3];
CameraMatrix[1, 0] = estimatedParams[4];
CameraMatrix[1, 1] = estimatedParams[5];
CameraMatrix[1, 2] = estimatedParams[6];
CameraMatrix[1, 3] = estimatedParams[7];
CameraMatrix[2, 0] = estimatedParams[8];
CameraMatrix[2, 1] = estimatedParams[9];
CameraMatrix[2, 2] = estimatedParams[10];
CameraMatrix[2, 3] = estimatedParams[11];
}
if(_pointsNormalised)
{
DenormaliseCameraMatrix();
}
result.AppendLine("Camera Matrix: ");
result.Append("|" + CameraMatrix[0, 0].ToString("F3"));
result.Append("; " + CameraMatrix[0, 1].ToString("F3"));
result.Append("; " + CameraMatrix[0, 2].ToString("F3"));
result.Append("; " + CameraMatrix[0, 3].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraMatrix[1, 0].ToString("F3"));
result.Append("; " + CameraMatrix[1, 1].ToString("F3"));
result.Append("; " + CameraMatrix[1, 2].ToString("F3"));
result.Append("; " + CameraMatrix[1, 3].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraMatrix[2, 0].ToString("F3"));
result.Append("; " + CameraMatrix[2, 1].ToString("F3"));
result.Append("; " + CameraMatrix[2, 2].ToString("F3"));
result.Append("; " + CameraMatrix[2, 3].ToString("F3"));
result.AppendLine("|");
DecomposeCameraMatrix();
result.AppendLine();
result.AppendLine("Calibration Matrix: ");
result.Append("|" + CameraInternalMatrix[0, 0].ToString("F3"));
result.Append("; " + CameraInternalMatrix[0, 1].ToString("F3"));
result.Append("; " + CameraInternalMatrix[0, 2].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraInternalMatrix[1, 0].ToString("F3"));
result.Append("; " + CameraInternalMatrix[1, 1].ToString("F3"));
result.Append("; " + CameraInternalMatrix[1, 2].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraInternalMatrix[2, 0].ToString("F3"));
result.Append("; " + CameraInternalMatrix[2, 1].ToString("F3"));
result.Append("; " + CameraInternalMatrix[2, 2].ToString("F3"));
result.AppendLine("|");
result.AppendLine();
result.AppendLine("Rotation Matrix: ");
result.Append("|" + CameraRotationMatrix[0, 0].ToString("F3"));
result.Append("; " + CameraRotationMatrix[0, 1].ToString("F3"));
result.Append("; " + CameraRotationMatrix[0, 2].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraRotationMatrix[1, 0].ToString("F3"));
result.Append("; " + CameraRotationMatrix[1, 1].ToString("F3"));
result.Append("; " + CameraRotationMatrix[1, 2].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraRotationMatrix[2, 0].ToString("F3"));
result.Append("; " + CameraRotationMatrix[2, 1].ToString("F3"));
result.Append("; " + CameraRotationMatrix[2, 2].ToString("F3"));
result.AppendLine("|");
result.AppendLine();
result.AppendLine("Translation Vector: ");
result.Append("|" + CameraTranslation[0].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraTranslation[1].ToString("F3"));
result.AppendLine("|");
result.Append("|" + CameraTranslation[2].ToString("F3"));
result.AppendLine("|");
double error = 0.0;
double relerror = 0.0;
double rerrx = 0.0;
double rerry = 0.0;
for(int p = 0; p < Points.Count; ++p)
{
var cp = Points[p];
Vector<double> ip = new DenseVector(new double[] { cp.ImgX, cp.ImgY, 1.0 });
Vector<double> rp = new DenseVector(new double[] { cp.RealX, cp.RealY, cp.RealZ, 1.0 });
Vector<double> eip = CameraMatrix * rp;
eip.DivideThis(eip[2]);
var d = (ip - eip);
error += d.L2Norm();
ip[2] = 0.0;
relerror += d.L2Norm() / ip.L2Norm();
rerrx += Math.Abs(d[0]) / Math.Abs(ip[0]);
rerry += Math.Abs(d[1]) / Math.Abs(ip[1]);
}
result.AppendLine();
result.AppendLine("Projection error ( d(xi, PXr)^2 ): ");
result.AppendLine("Points count: " + Points.Count.ToString());
result.AppendLine("Total: " + error.ToString("F4"));
result.AppendLine("Mean: " + (error / Points.Count).ToString("F4"));
result.AppendLine("Realtive: " + (relerror).ToString("F4"));
result.AppendLine("Realtive mean: " + (relerror / Points.Count).ToString("F4"));
result.AppendLine("Realtive in X: " + (rerrx).ToString("F4"));
result.AppendLine("Realtive in X mean: " + (rerrx / Points.Count).ToString("F4"));
result.AppendLine("Realtive in Y: " + (rerry).ToString("F4"));
result.AppendLine("Realtive in Y mean: " + (rerry / Points.Count).ToString("F4"));
if(MinimaliseSkew == true)
{
result.AppendLine("SkewMini - base residual: " + _zeroSkewMini.BaseResidiual.ToString("F4"));
result.AppendLine("Skewini - best residual: " + _zeroSkewMini.MinimumResidiual.ToString("F4"));
}
if(LinearOnly == false)
{
result.AppendLine("GeoMini - base residual: " + _miniAlg.BaseResidiual.ToString("F4"));
result.AppendLine("GeoMini - best residual: " + _miniAlg.MinimumResidiual.ToString("F4"));
}
}
else
{
result.AppendLine("Camera not yet computed");
return result.ToString();
}
return result.ToString();
}
