/// <summary>
/// 利用7参数求新坐标系的坐标
/// </summary>
/// <param name="aPtSource">点的源坐标系的坐标</param>
/// <param name="sep">已经知道的7参数</param>
/// <param name="aPtTo">输出: 点的新坐标系的坐标</param>
public static XYZCoordinate Para7Convert(XYZCoordinate aPtSource, SevenPara sep)
{
double k = sep.scale;
double a2 = k * sep.Ax;
double a3 = k * sep.Ay;
double a4 = k * sep.Az;
XYZCoordinate ToCoordinate = new XYZCoordinate();
ToCoordinate.X = sep.deltaX + k * aPtSource.X - a3 * aPtSource.Z + a4 * aPtSource.Y;
ToCoordinate.Y = sep.deltaY + k * aPtSource.Y + a2 * aPtSource.Z - a4 * aPtSource.X;
ToCoordinate.Z = sep.deltaZ + k * aPtSource.Z - a2 * aPtSource.Y + a3 * aPtSource.X;
return(ToCoordinate);
}
/// <summary>
/// 根据3个或者3个以上的点的两套坐标系的坐标计算7参数(最小二乘法) 适用于小角度转换 bursa模型
/// </summary>
/// <param name="aPtSource">已知点的源坐标系的坐标</param>
/// <param name="aPtTo">已知点的新坐标系的坐标</param>
/// <param name="sep">输出: 7参数</param>
public static SevenPara Calc7Para(List <XYZCoordinate> aPtSource, List <XYZCoordinate> aPtTo, out double rms)
{
#region 给A B 矩阵赋值
double[,] arrA = new double[aPtSource.Count * 3, 7]; // 如果是4个已知点， 12 * 7矩阵 A*X=B中的矩阵A
for (int i = 0; i < arrA.GetLength(0); i++)
{
if (i % 3 == 0)
{
arrA[i, 0] = 1;
arrA[i, 1] = 0;
arrA[i, 2] = 0;
arrA[i, 3] = aPtSource[i / 3].X;
arrA[i, 4] = 0;
arrA[i, 5] = -aPtSource[i / 3].Z;
arrA[i, 6] = aPtSource[i / 3].Y;
}
else if (i % 3 == 1)
{
arrA[i, 0] = 0;
arrA[i, 1] = 1;
arrA[i, 2] = 0;
arrA[i, 3] = aPtSource[i / 3].Y;
arrA[i, 4] = aPtSource[i / 3].Z;
arrA[i, 5] = 0;
arrA[i, 6] = -aPtSource[i / 3].X;
}
else if (i % 3 == 2)
{
arrA[i, 0] = 0;
arrA[i, 1] = 0;
arrA[i, 2] = 1;
arrA[i, 3] = aPtSource[i / 3].Z;
arrA[i, 4] = -aPtSource[i / 3].Y;
arrA[i, 5] = aPtSource[i / 3].X;
arrA[i, 6] = 0;
}
}
double[,] arrB = new double[aPtSource.Count * 3, 1]; // A * X = B 中的矩阵B, 如果有4个点，就是 12*1矩阵
for (int i = 0; i <= arrB.GetLength(0) - 1; i++)
{
if (i % 3 == 0)
{
arrB[i, 0] = aPtTo[i / 3].X;
}
else if (i % 3 == 1)
{
arrB[i, 0] = aPtTo[i / 3].Y;
}
else if (i % 3 == 2)
{
arrB[i, 0] = aPtTo[i / 3].Z;
}
}
#endregion
Matrix mtrA = new Matrix(arrA); // A矩阵
Matrix mtrAT = Matrix.Transpose(mtrA); // A的转置
Matrix mtrB = new Matrix(arrB); // B矩阵
Matrix mtrATmulA = Matrix.Multiply(mtrAT, mtrA); // A的转置×A
// 求(A的转置×A)的逆矩阵
mtrATmulA = Matrix.Inverse(mtrATmulA);
// A的转置 × B
Matrix mtrATmulB = Matrix.Multiply(mtrAT, mtrB); // A的转置 * B
// 结果
Matrix mtrResult = Matrix.Multiply(mtrATmulA, mtrATmulB);
SevenPara sep = new SevenPara();
sep.deltaX = mtrResult[0, 0];
sep.deltaY = mtrResult[1, 0];
sep.deltaZ = mtrResult[2, 0];
sep.scale = mtrResult[3, 0];
sep.Ax = mtrResult[4, 0] / sep.scale;
sep.Ay = mtrResult[5, 0] / sep.scale;
sep.Az = mtrResult[6, 0] / sep.scale;
//计算均方根误差rms
List <XYZCoordinate> pTo = new List <XYZCoordinate>();
for (int i = 0; i < aPtSource.Count; i++)
{
pTo.Add(Para7Convert(aPtSource[i], sep));
}
rms = 0;
for (int i = 0; i < aPtTo.Count; i++)
{
double deltaX = aPtTo[i].X - pTo[i].X;
double deltaY = aPtTo[i].Y - pTo[i].Y;
double oneSigma = Math.Sqrt((Math.Pow(deltaX, 2) + Math.Pow(deltaY, 2)) / 2);
rms += oneSigma;
}
rms = rms / aPtTo.Count;
return(sep);
}
/// <summary>
/// 12参数最小二乘解大角度7参数
/// </summary>
/// <param name="cameraManager"></param>
/// <param name="XcAndXwPairs"></param>
/// <param name="X_ins"></param>
/// <returns></returns>
public static SevenPara CalBigAngle7Paras(List <XYZCoordinate> aPtSource, List <XYZCoordinate> aPtTo)
{
Matrix A = new Matrix(3 * aPtSource.Count, 12);
Matrix B = new Matrix(3 * aPtSource.Count, 1);
for (int i = 0; i < 3 * aPtSource.Count; i++)
{
if (i % 3 == 0)
{
A[i, 0] = aPtSource[i / 3].X;
A[i, 1] = aPtSource[i / 3].Y;
A[i, 2] = aPtSource[i / 3].Z;
A[i, 3] = 0;
A[i, 4] = 0;
A[i, 5] = 0;
A[i, 6] = 0;
A[i, 7] = 0;
A[i, 8] = 0;
A[i, 9] = 1;
A[i, 10] = 0;
A[i, 11] = 0;
}
else if (i % 3 == 1)
{
A[i, 0] = 0;
A[i, 1] = 0;
A[i, 2] = 0;
A[i, 3] = aPtSource[i / 3].X;
A[i, 4] = aPtSource[i / 3].Y;
A[i, 5] = aPtSource[i / 3].Z;
A[i, 6] = 0;
A[i, 7] = 0;
A[i, 8] = 0;
A[i, 9] = 0;
A[i, 10] = 1;
A[i, 11] = 0;
}
else if (i % 3 == 2)
{
A[i, 0] = 0;
A[i, 1] = 0;
A[i, 2] = 0;
A[i, 3] = 0;
A[i, 4] = 0;
A[i, 5] = 0;
A[i, 6] = aPtSource[i / 3].X;
A[i, 7] = aPtSource[i / 3].Y;
A[i, 8] = aPtSource[i / 3].Z;
A[i, 9] = 0;
A[i, 10] = 0;
A[i, 11] = 1;
}
}
for (int i = 0; i < 3 * aPtTo.Count; i++)
{
if (i % 3 == 0)
{
B[i, 0] = aPtTo[i / 3].X;
}
else if (i % 3 == 1)
{
B[i, 0] = aPtTo[i / 3].Y;
}
else if (i % 3 == 2)
{
B[i, 0] = aPtTo[i / 3].Z;
}
}
Matrix R12 = Matrix.Inverse(Matrix.Transpose(A) * A) * (Matrix.Transpose(A) * B);
//根据R_w_ins的矩阵元素可解出各个姿态角
double s = Math.Sqrt(Math.Pow(R12[0, 0], 2) + Math.Pow(R12[3, 0], 2) + Math.Pow(R12[6, 0], 2));
double cosRoll = Math.Sqrt(Math.Pow(R12[0, 0], 2) + Math.Pow(R12[3, 0], 2)) / s;
double sinRoll = -R12[6, 0] / s;
double roll = (sinRoll >= 0) ? (Math.Acos(cosRoll)) : (-Math.Acos(cosRoll));
double sinPitch = R12[7, 0] / Math.Cos(roll) / s;
double cosPitch = R12[8, 0] / Math.Cos(roll) / s;
double pitch = (sinPitch >= 0) ? (Math.Acos(cosPitch)) : (-Math.Acos(cosPitch));
double sinYaw = -R12[3, 0] / Math.Cos(roll) / s;
double cosYaw = R12[0, 0] / Math.Cos(roll) / s;
double yaw = (sinYaw >= 0) ? (Math.Acos(cosYaw)) : (-Math.Acos(cosYaw));
yaw = (yaw >= 0) ? yaw : (-yaw);
roll = roll * 180 / Math.PI;
pitch = pitch * 180 / Math.PI;
yaw = yaw * 180 / Math.PI;
SevenPara sevPara = new SevenPara();
sevPara.deltaX = R12[9, 0];
sevPara.deltaY = R12[10, 0];
sevPara.deltaZ = R12[11, 0];
sevPara.Ax = pitch;
sevPara.Ay = roll;
sevPara.Az = yaw;
sevPara.scale = s;
return(sevPara);
}
