/// <summary>
///
/// </summary>
/// <param name="p0">First point on route</param>
/// <param name="q0">First point on map</param>
/// <param name="p1">Second point on route</param>
/// <param name="q1">Second point on map</param>
/// <param name="p2">Third point on route</param>
/// <param name="q2">Third point on map</param>
/// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param>
/// <returns></returns>
public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, PointD p2, PointD q2, GeneralMatrix fallbackMatrix)
{
try
{
var m = new GeneralMatrix(3, 3);
m.SetElement(0, 0, p0.X);
m.SetElement(0, 1, p0.Y);
m.SetElement(0, 2, 1.0);
m.SetElement(1, 0, p1.X);
m.SetElement(1, 1, p1.Y);
m.SetElement(1, 2, 1.0);
m.SetElement(2, 0, p2.X);
m.SetElement(2, 1, p2.Y);
m.SetElement(2, 2, 1.0);
var v1 = new GeneralMatrix(3, 1);
v1.SetElement(0, 0, q0.X);
v1.SetElement(1, 0, q1.X);
v1.SetElement(2, 0, q2.X);
var t1 = m.Inverse() * v1;
var v2 = new GeneralMatrix(3, 1);
v2.SetElement(0, 0, q0.Y);
v2.SetElement(1, 0, q1.Y);
v2.SetElement(2, 0, q2.Y);
var t2 = m.Inverse() * v2;
var v3 = new GeneralMatrix(3, 1);
v3.SetElement(0, 0, 1.0);
v3.SetElement(1, 0, 1.0);
v3.SetElement(2, 0, 1.0);
var t3 = m.Inverse() * v3;
var t = new GeneralMatrix(3, 3);
t.SetElement(0, 0, t1.GetElement(0, 0));
t.SetElement(0, 1, t1.GetElement(1, 0));
t.SetElement(0, 2, t1.GetElement(2, 0));
t.SetElement(1, 0, t2.GetElement(0, 0));
t.SetElement(1, 1, t2.GetElement(1, 0));
t.SetElement(1, 2, t2.GetElement(2, 0));
t.SetElement(2, 0, t3.GetElement(0, 0));
t.SetElement(2, 1, t3.GetElement(1, 0));
t.SetElement(2, 2, t3.GetElement(2, 0));
return(t);
}
catch (Exception)
{
return((GeneralMatrix)fallbackMatrix.Clone());
}
}
/// <summary>
/// Solves the increments array (<code>this.da</code>) using alpha and beta.
/// Then updates the <code>this.incrementedParameters</code> array.
/// NOTE: Inverts alpha. Call at least <code>updateAlpha()</code> before calling this.
/// </summary>
protected void SolveIncrements()
{
try
{
//use the GeneralMatrix package to invert alpha
//one could also use
//double[] da = DoubleMatrix.solve(alpha, beta);
GeneralMatrix m = alpha.Inverse();
//set alpha with inverted matrix
alpha.SetMatrix(0, alpha.RowDimension - 1, 0, alpha.ColumnDimension - 1, m);
}
catch (Exception e)
{
Trace.WriteLine(e.StackTrace);
}
for (int i = 0; i < alpha.RowDimension; i++)
{
da[i] = 0;
for (int j = 0; j < alpha.ColumnDimension; j++)
{
da[i] += alpha.GetElement(i, j) * beta[j];
}
}
for (int i = 0; i < parameters.Length; i++)
{
//if (!Double.isNaN(da[i]) && !Double.isInfinite(da[i]))
incrementedParameters[i] = parameters[i] + da[i];
}
}
public void Inverse()
{
GeneralMatrix r = GeneralMatrix.Random(4, 4);
GeneralMatrix iR = r.Inverse();
Assert.IsTrue(GeneralTests.Check(r.Multiply(iR), GeneralMatrix.Identity(4, 4)));
}
/******************* Method Kalman Filter ******************/
// Variable A,B, and H we will asumtion this is a constant value
// Most probably is 1.
private Joint kalmanFilter(GeneralMatrix z, int pos)
{
// Prepare
Joint join = new Joint();
GeneralMatrix r = GeneralMatrix.Identity(3, 3);
r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X
r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z
r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y
R = r;
// Predict
GeneralMatrix Xp = Xk[pos];
GeneralMatrix Pp = P[pos];
// Measurement Update (correction
GeneralMatrix s = Pp + R;
K = Pp * s.Inverse(); // Calculate Kalman Gain
Xk[pos] = Xp + (K * (z - Xp));
GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension);
P[pos] = (I - K) * Pp;
estimationX = Xk[pos].GetElement(0, 0);
estimationY = Xk[pos].GetElement(1, 0);
estimationZ = Xk[pos].GetElement(2, 0);
join.Position.X = (float)estimationX;
join.Position.Y = (float)estimationY;
join.Position.Z = (float)estimationZ;
return(join);
}
public Point GetActualPosition(Point p)
{
GeneralMatrix mat = new GeneralMatrix(3, 1);
mat.SetElement(0, 0, (double)p.X);
mat.SetElement(1, 0, (double)p.Y);
mat.SetElement(2, 0, 1.0);
GeneralMatrix ret = m_transform.Inverse().Multiply(mat);
return(new Point((int)ret.GetElement(0, 0), (int)ret.GetElement(1, 0)));
}
// this is the straight forward implementation of the kabsch algorithm.
// see http://en.wikipedia.org/wiki/Kabsch_algorithm for a detailed explanation.
public void Evaluate(int SpreadMax)
{
int newSpreadMax = SpreadUtils.SpreadMax(FInputQ, FInputP);
FOutput.SliceCount = newSpreadMax;
Matrix4x4 mOut;
if (FInputEnabled[0])
{
for (int slice = 0; slice < newSpreadMax; slice++)
{
// ======================== STEP 1 ========================
// translate both sets so that their centroids coincides with the origin
// of the coordinate system.
double[] meanP = new double[3] {
0.0, 0.0, 0.0
}; // mean of first point set
for (int i = 0; i < FInputP[slice].SliceCount; i++)
{
meanP[0] += FInputP[slice][i].x;
meanP[1] += FInputP[slice][i].y;
meanP[2] += FInputP[slice][i].z;
}
meanP[0] /= FInputP[slice].SliceCount;
meanP[1] /= FInputP[slice].SliceCount;
meanP[2] /= FInputP[slice].SliceCount;
double[][] centroidP = new double[3][] { new double[] { meanP[0] }, new double[] { meanP[1] }, new double[] { meanP[2] } };
GeneralMatrix mCentroidP = new GeneralMatrix(centroidP);
double[][] arrayP = new double[FInputP[slice].SliceCount][];
for (int i = 0; i < FInputP[slice].SliceCount; i++)
{
arrayP[i] = new double[3];
arrayP[i][0] = FInputP[slice][i].x - meanP[0]; // subtract the mean values from the incoming pointset
arrayP[i][1] = FInputP[slice][i].y - meanP[1];
arrayP[i][2] = FInputP[slice][i].z - meanP[2];
}
// this is the matrix of the first pointset translated to the origin of the coordinate system
GeneralMatrix P = new GeneralMatrix(arrayP);
double[] meanQ = new double[3] {
0.0, 0.0, 0.0
}; // mean of second point set
for (int i = 0; i < FInputQ[slice].SliceCount; i++)
{
meanQ[0] += FInputQ[slice][i].x;
meanQ[1] += FInputQ[slice][i].y;
meanQ[2] += FInputQ[slice][i].z;
}
meanQ[0] /= FInputQ[slice].SliceCount;
meanQ[1] /= FInputQ[slice].SliceCount;
meanQ[2] /= FInputQ[slice].SliceCount;
double[][] centroidQ = new double[3][] { new double[] { meanQ[0] }, new double[] { meanQ[1] }, new double[] { meanQ[2] } };
GeneralMatrix mCentroidQ = new GeneralMatrix(centroidQ);
double[][] arrayQ = new double[FInputQ[slice].SliceCount][];
for (int i = 0; i < FInputQ[slice].SliceCount; i++)
{
arrayQ[i] = new double[3];
arrayQ[i][0] = FInputQ[slice][i].x - meanQ[0]; // subtract the mean values from the incoming pointset
arrayQ[i][1] = FInputQ[slice][i].y - meanQ[1];
arrayQ[i][2] = FInputQ[slice][i].z - meanQ[2];
}
// this is the matrix of the second pointset translated to the origin of the coordinate system
GeneralMatrix Q = new GeneralMatrix(arrayQ);
// ======================== STEP2 ========================
// calculate a covariance matrix A and compute the optimal rotation matrix
GeneralMatrix A = P.Transpose() * Q;
SingularValueDecomposition svd = A.SVD();
GeneralMatrix U = svd.GetU();
GeneralMatrix V = svd.GetV();
// calculate determinant for a special reflexion case.
double det = (V * U.Transpose()).Determinant();
double[][] arrayD = new double[3][] { new double[] { 1, 0, 0 },
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 } };
arrayD[2][2] = det < 0 ? -1 : 1; // multiply 3rd column with -1 if determinant is < 0
GeneralMatrix D = new GeneralMatrix(arrayD);
// now we can compute the rotation matrix:
GeneralMatrix R = V * D * U.Transpose();
// ======================== STEP3 ========================
// calculate the translation:
GeneralMatrix T = mCentroidP - R.Inverse() * mCentroidQ;
// ================== OUTPUT TRANSFORM ===================
mOut.m11 = (R.Array)[0][0];
mOut.m12 = (R.Array)[0][1];
mOut.m13 = (R.Array)[0][2];
mOut.m14 = 0;
mOut.m21 = (R.Array)[1][0];
mOut.m22 = (R.Array)[1][1];
mOut.m23 = (R.Array)[1][2];
mOut.m24 = 0;
mOut.m31 = (R.Array)[2][0];
mOut.m32 = (R.Array)[2][1];
mOut.m33 = (R.Array)[2][2];
mOut.m34 = 0;
mOut.m41 = (T.Array)[0][0];
mOut.m42 = (T.Array)[1][0];
mOut.m43 = (T.Array)[2][0];
mOut.m44 = 1;
FOutput[slice] = mOut;
}
}
}
/// <summary>
///
/// </summary>
/// <param name="p0">First point on route, in projected (metric) coordinates relative to projection origin</param>
/// <param name="q0">First point on map, in pixels</param>
/// <param name="p1">Second point on route, in projected (metric) coordinates relative to projection origin</param>
/// <param name="q1">Second point on map, in pixels</param>
/// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param>
/// <param name="useRotation">If true, assumes orthogonal map and calculates scale and rotation. If false, calculates different scale in x and y directions and no rotation.</param>
/// <returns></returns>
public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, GeneralMatrix fallbackMatrix, bool useRotation)
{
try
{
if (useRotation)
{
// note that we need to mirror y pixel value in x axis
double angleDifferece = GetAngleR(p1 - p0, new PointD(q1.X, -q1.Y) - new PointD(q0.X, -q0.Y));
double lengthQ = DistancePointToPoint(q0, q1);
double lengthP = DistancePointToPoint(p0, p1);
double scaleFactor = lengthP == 0 ? 0 : lengthQ / lengthP;
double cos = Math.Cos(angleDifferece);
double sin = Math.Sin(angleDifferece);
// translation to origo in metric space
var a = new GeneralMatrix(3, 3);
a.SetElement(0, 0, 1);
a.SetElement(0, 1, 0);
a.SetElement(0, 2, -p0.X);
a.SetElement(1, 0, 0);
a.SetElement(1, 1, 1);
a.SetElement(1, 2, -p0.Y);
a.SetElement(2, 0, 0);
a.SetElement(2, 1, 0);
a.SetElement(2, 2, 1);
// rotation
var b = new GeneralMatrix(3, 3);
b.SetElement(0, 0, cos);
b.SetElement(0, 1, -sin);
b.SetElement(0, 2, 0);
b.SetElement(1, 0, sin);
b.SetElement(1, 1, cos);
b.SetElement(1, 2, 0);
b.SetElement(2, 0, 0);
b.SetElement(2, 1, 0);
b.SetElement(2, 2, 1);
// scaling, note that we need to mirror y scale around x axis
var c = new GeneralMatrix(3, 3);
c.SetElement(0, 0, scaleFactor);
c.SetElement(0, 1, 0);
c.SetElement(0, 2, 0);
c.SetElement(1, 0, 0);
c.SetElement(1, 1, -scaleFactor);
c.SetElement(1, 2, 0);
c.SetElement(2, 0, 0);
c.SetElement(2, 1, 0);
c.SetElement(2, 2, 1);
// translation from origo to pixel space
var d = new GeneralMatrix(3, 3);
d.SetElement(0, 0, 1);
d.SetElement(0, 1, 0);
d.SetElement(0, 2, q0.X);
d.SetElement(1, 0, 0);
d.SetElement(1, 1, 1);
d.SetElement(1, 2, q0.Y);
d.SetElement(2, 0, 0);
d.SetElement(2, 1, 0);
d.SetElement(2, 2, 1);
return(d * c * b * a);
}
else // useRotation == false
{
var m1 = new GeneralMatrix(2, 2);
m1.SetElement(0, 0, p0.X);
m1.SetElement(0, 1, 1);
m1.SetElement(1, 0, p1.X);
m1.SetElement(1, 1, 1);
var v1 = new GeneralMatrix(2, 1);
v1.SetElement(0, 0, q0.X);
v1.SetElement(1, 0, q1.X);
var t1 = m1.Inverse() * v1;
var m2 = new GeneralMatrix(2, 2);
m2.SetElement(0, 0, p0.Y);
m2.SetElement(0, 1, 1);
m2.SetElement(1, 0, p1.Y);
m2.SetElement(1, 1, 1);
var v2 = new GeneralMatrix(2, 1);
v2.SetElement(0, 0, q0.Y);
v2.SetElement(1, 0, q1.Y);
var t2 = m2.Inverse() * v2;
var t = new GeneralMatrix(3, 3);
t.SetElement(0, 0, t1.GetElement(0, 0));
t.SetElement(0, 1, 0);
t.SetElement(0, 2, t1.GetElement(1, 0));
t.SetElement(1, 0, 0);
t.SetElement(1, 1, t2.GetElement(0, 0));
t.SetElement(1, 2, t2.GetElement(1, 0));
t.SetElement(2, 0, 0);
t.SetElement(2, 1, 0);
t.SetElement(2, 2, 1);
return(t);
}
}
catch (Exception)
{
return((GeneralMatrix)fallbackMatrix.Clone());
}
}
/// <summary>
/// 计算四参数
/// </summary>
/// <param name="st4"></param>
public void CalculateTrans4Param(List<Coords4ST> st4)
{
int count = st4.Count;
double[][] A = new double[count * 2][];
for (int i = 0; i < count * 2; i++)
{
A[i] = new double[4];
}
double[][] B = new double[count * 2][];
for (int i = 0; i < count * 2; i++)
{
B[i] = new double[1];
}
int idx = 0;
for (int i = 0; i < count * 2; i = i + 2)
{
A[i][0] = 1; A[i][1] = 0; A[i][2] = st4[idx].SourceX; A[i][3] = -st4[idx].SourceY;
A[i + 1][0] = 0; A[i + 1][1] = 1; A[i + 1][2] = st4[idx].SourceY; A[i + 1][3] = st4[idx].SourceX;
B[i][0] = st4[idx].TargetX;
B[i + 1][0] = st4[idx].TargetY;
idx = idx + 1;
}
GeneralMatrix matrixA = new GeneralMatrix(A);
GeneralMatrix matrixB = new GeneralMatrix(B);
GeneralMatrix matrixParm = matrixA.Inverse().Multiply(matrixB);
this.dx = matrixParm.GetElement(0, 0);
this.dy = matrixParm.GetElement(1, 0);
this.arf = Math.Atan(matrixParm.GetElement(3, 0) / matrixParm.GetElement(2, 0));
this.k = matrixParm.GetElement(3, 0) / Math.Sin(this.arf);
}
private void calculateCCMInverse()
{
this.ccmInverse = ccm.Inverse();
}
/// <summary>
/// Solves between two point sets
/// </summary>
/// <param name="points">Point set</param>
/// <returns>Affine matrix for each point set</returns>
public Matrix Solve(IReadOnlyList <CameraToCameraPoint> points)
{
if (points == null)
{
throw new ArgumentNullException("points");
}
if (points.Count < 1)
{
throw new ArgumentException("points", "No points provided");
}
double[] meanP = new double[3] {
0.0, 0.0, 0.0
}; // mean of first point set
for (int i = 0; i < points.Count; i++)
{
Vector3 orig = points[i].Origin;
meanP[0] += orig.X;
meanP[1] += orig.Y;
meanP[2] += orig.Z;
}
double invCount = 1.0 / (double)points.Count;
meanP[0] *= invCount;
meanP[1] *= invCount;
meanP[2] *= invCount;
double[][] centroidP = new double[3][] { new double[] { meanP[0] }, new double[] { meanP[1] }, new double[] { meanP[2] } };
GeneralMatrix mCentroidP = new GeneralMatrix(centroidP);
double[][] arrayP = new double[points.Count][];
for (int i = 0; i < points.Count; i++)
{
Vector3 orig = points[i].Origin;
arrayP[i] = new double[3];
arrayP[i][0] = orig.X - meanP[0]; // subtract the mean values from the incoming pointset
arrayP[i][1] = orig.Y - meanP[1];
arrayP[i][2] = orig.Z - meanP[2];
}
// this is the matrix of the first pointset translated to the origin of the coordinate system
GeneralMatrix P = new GeneralMatrix(arrayP);
double[] meanQ = new double[3] {
0.0, 0.0, 0.0
}; // mean of second point set
for (int i = 0; i < points.Count; i++)
{
Vector3 dest = points[i].Destination;
meanQ[0] += dest.X;
meanQ[1] += dest.Y;
meanQ[2] += dest.Z;
}
meanQ[0] *= invCount;
meanQ[1] *= invCount;
meanQ[2] *= invCount;
double[][] centroidQ = new double[3][] { new double[] { meanQ[0] }, new double[] { meanQ[1] }, new double[] { meanQ[2] } };
GeneralMatrix mCentroidQ = new GeneralMatrix(centroidQ);
double[][] arrayQ = new double[points.Count][];
for (int i = 0; i < points.Count; i++)
{
Vector3 dest = points[i].Destination;
arrayQ[i] = new double[3];
arrayQ[i][0] = dest.X - meanQ[0]; // subtract the mean values from the incoming pointset
arrayQ[i][1] = dest.Y - meanQ[1];
arrayQ[i][2] = dest.Z - meanQ[2];
}
// this is the matrix of the second pointset translated to the origin of the coordinate system
GeneralMatrix Q = new GeneralMatrix(arrayQ);
// ======================== STEP2 ========================
// calculate a covariance matrix A and compute the optimal rotation matrix
GeneralMatrix A = P.Transpose() * Q;
SingularValueDecomposition svd = A.SVD();
GeneralMatrix U = svd.GetU();
GeneralMatrix V = svd.GetV();
// calculate determinant for a special reflexion case.
double det = (V * U.Transpose()).Determinant();
double[][] arrayD = new double[3][] { new double[] { 1, 0, 0 },
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 } };
arrayD[2][2] = det < 0 ? -1 : 1; // multiply 3rd column with -1 if determinant is < 0
GeneralMatrix D = new GeneralMatrix(arrayD);
// now we can compute the rotation matrix:
GeneralMatrix R = V * D * U.Transpose();
// ======================== STEP3 ========================
// calculate the translation:
GeneralMatrix T = mCentroidP - R.Inverse() * mCentroidQ;
return(new Matrix((float)(R.Array)[0][0],
(float)(R.Array)[0][1],
(float)(R.Array)[0][2],
0.0f,
(float)(R.Array)[1][0],
(float)(R.Array)[1][1],
(float)(R.Array)[1][2],
0.0f,
(float)(R.Array)[2][0],
(float)(R.Array)[2][1],
(float)(R.Array)[2][2],
0.0f,
(float)(T.Array)[0][0],
(float)(T.Array)[1][0],
(float)(T.Array)[2][0],
1.0f));
}
/// <summary>
/// ֱ���������
/// </summary>
/// <param name="st7"></param>
public void CalculateTrans7Param2(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);
GeneralMatrix matrixParam = matrixA.Inverse() * 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));
}
