public void QRDecomposition()
{
GeneralMatrix A = new GeneralMatrix(columnwise, 4);
QRDecomposition QR = A.QRD();
GeneralMatrix R = QR.R;
Assert.IsTrue(GeneralTests.Check(A, QR.Q.Multiply(R)));
}
/// <summary>
/// Using linear least squares algorithm described at http://en.wikipedia.org/wiki/Linear_least_squares
/// </summary>
/// <returns></returns>
public Transformation CalculateAverageTransformation()
{
var averageProjectionOrigin = new LongLat();
foreach (var session in this)
{
averageProjectionOrigin += session.ProjectionOrigin / Count;
}
if (Count == 0)
{
return(null);
}
var n = 4;
var XtX = new GeneralMatrix(n, n);
var Xty = new GeneralMatrix(n, 1);
var numberOfUnknowns = 0;
foreach (var session in this)
{
var m = session.Handles.Length;
if (m < 2)
{
continue;
}
numberOfUnknowns += m;
var startDistance = session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Route.FirstPL).Value;
for (var i = 0; i < m; i++)
{
var longLat = session.Route.GetLocationFromParameterizedLocation(session.Handles[i].ParameterizedLocation);
var p = longLat.Project(averageProjectionOrigin); // projected point on earth (metres)
var q = session.Handles[i].Location; // point on map image (pixels)
var endDistance = (i != m - 1)
? (session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Handles[i].ParameterizedLocation).Value +
session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Handles[i + 1].ParameterizedLocation).Value) / 2
: session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Route.LastPL).Value;
var w = endDistance - startDistance; // weight
startDistance = endDistance;
XtX.SetElement(0, 0, XtX.GetElement(0, 0) + w * (p.X * p.X + p.Y * p.Y));
XtX.SetElement(0, 2, XtX.GetElement(0, 2) + w * p.X);
XtX.SetElement(0, 3, XtX.GetElement(0, 3) - w * p.Y);
XtX.SetElement(1, 1, XtX.GetElement(1, 1) + w * (p.X * p.X + p.Y * p.Y));
XtX.SetElement(1, 2, XtX.GetElement(1, 2) + w * p.Y);
XtX.SetElement(1, 3, XtX.GetElement(1, 3) + w * p.X);
XtX.SetElement(2, 0, XtX.GetElement(2, 0) + w * p.X);
XtX.SetElement(2, 1, XtX.GetElement(2, 1) + w * p.Y);
XtX.SetElement(2, 2, XtX.GetElement(2, 2) + w);
XtX.SetElement(3, 0, XtX.GetElement(3, 0) - w * p.Y);
XtX.SetElement(3, 1, XtX.GetElement(3, 1) + w * p.X);
XtX.SetElement(3, 3, XtX.GetElement(3, 3) + w);
Xty.SetElement(0, 0, Xty.GetElement(0, 0) + w * (q.X * p.X - q.Y * p.Y));
Xty.SetElement(1, 0, Xty.GetElement(1, 0) + w * (q.X * p.Y + q.Y * p.X));
Xty.SetElement(2, 0, Xty.GetElement(2, 0) + w * q.X);
Xty.SetElement(3, 0, Xty.GetElement(3, 0) + w * q.Y);
}
}
var T = new GeneralMatrix(3, 3);
if (numberOfUnknowns == 0)
{
T = this[0].InitialTransformationMatrix;
}
else if (numberOfUnknowns == 1)
{
T = this[0].Handles[0].TransformationMatrix;
}
else
{
var B = XtX.QRD().Solve(Xty);
T.SetElement(0, 0, B.GetElement(0, 0));
T.SetElement(0, 1, B.GetElement(1, 0));
T.SetElement(0, 2, B.GetElement(2, 0));
T.SetElement(1, 0, B.GetElement(1, 0));
T.SetElement(1, 1, -B.GetElement(0, 0));
T.SetElement(1, 2, B.GetElement(3, 0));
T.SetElement(2, 0, 0);
T.SetElement(2, 1, 0);
T.SetElement(2, 2, 1);
}
return(new Transformation(T, averageProjectionOrigin));
}
/// <summary>
/// QR�ֽⷨ
/// </summary>
/// <param name="st7"></param>
public void CalculateTrans7Param3(List<Coords7ST> st7)
{
double[][] A = new double[0][];
double[][] B = new double[0][];
InitMatrixAB(ref A, ref B, st7);
GeneralMatrix matrixA = new GeneralMatrix(A);
GeneralMatrix matrixB = new GeneralMatrix(B);
QRDecomposition qrDecp = matrixA.QRD();
GeneralMatrix q = qrDecp.Q;
GeneralMatrix r = qrDecp.R;
GeneralMatrix matrixParam = r.Inverse().Multiply(q.Transpose()).Multiply(matrixB);
this.Set4Param(matrixParam.GetElement(0, 0), matrixParam.GetElement(1, 0), matrixParam.GetElement(2, 0), matrixParam.GetElement(6, 0));
this.SetRotationParam(matrixParam.GetElement(3, 0), matrixParam.GetElement(4, 0), matrixParam.GetElement(5, 0));
}
