/// <summary>
/// v1をなるべくv2に近づけるような回転行列を求める。戻り値は、残差の二乗和を個数で割ったもの
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="result"></param>
/// <returns></returns>
public static double GetRotationMatrix(Vector3DBase[] v1, Vector3DBase[] v2, ref Matrix3D result, Matrix3D initialRotation = null)
{
if (initialRotation == null)
{
//初期回転行列を求める
initialRotation = new Matrix3D();
}
var euler = Euler.GetEulerAngle(initialRotation);
int length = v1.Length;
double phi = euler.Phi, theta = euler.Theta, psi = euler.Psi;
double phi_New, theta_New, psi_New;
Matrix3D rot = initialRotation, rot_New;
Vector3DBase[,] diff = new Vector3DBase[3, length];
var Alpha = new DMat(3, 3);
var Beta = new DMat(3, 1);
Vector3DBase[] ResidualCurrent = new Vector3DBase[length];
Vector3DBase[] ResidualNew = new Vector3DBase[length];
double ResidualSquareCurrent, ResidualSquareNew = 0;
int count = 0;
//現在の残差を計算
ResidualSquareCurrent = 0;
for (int i = 0; i < length; i++)
{
ResidualCurrent[i] = rot * v1[i] - v2[i];
ResidualSquareCurrent += ResidualCurrent[i] * ResidualCurrent[i];
}
double lambda = 1;
while (lambda < 1000000000 && count < 3000)//lambdaが大きくなりすぎた時か、試行回数が一定以上になった時、止める
{
count++;
double cosPhi = Math.Cos(phi), sinPhi = Math.Sin(phi), cosTheta = Math.Cos(theta), sinTheta = Math.Sin(theta), cosPsi = Math.Cos(psi), sinPsi = Math.Sin(psi);
for (int i = 0; i < length; i++)//偏微分を作る 「回転行列の偏微分」というMathematicaファイルを参照
{
//∂F/∂phi
diff[0, i] = new Vector3DBase(
v1[i].X * (-cosPsi * sinPhi - cosPhi * cosTheta * sinPsi) + v1[i].Y * (cosPhi * cosPsi * cosTheta - sinPhi * sinPsi) + v1[i].Z * cosPhi * sinTheta,
v1[i].Y * (-cosPsi * cosTheta * sinPhi - cosPhi * sinPsi) + v1[i].X * (-cosPhi * cosPsi + cosTheta * sinPhi * sinPsi) - v1[i].Z * sinPhi * sinTheta,
0
);
//∂F/∂theta
diff[1, i] = new Vector3DBase(
v1[i].Z * cosTheta * sinPhi - v1[i].Y * cosPsi * sinPhi * sinTheta + v1[i].X * sinPhi * sinPsi * sinTheta,
v1[i].Z * cosPhi * cosTheta - v1[i].Y * cosPhi * cosPsi * sinTheta + v1[i].X * cosPhi * sinPsi * sinTheta,
-v1[i].Y * cosPsi * cosTheta + v1[i].X * cosTheta * sinPsi - v1[i].Z * sinTheta
);
//∂F/∂psi
diff[2, i] = new Vector3DBase(
v1[i].X * (-cosPsi * cosTheta * sinPhi - cosPhi * sinPsi) + v1[i].Y * (cosPhi * cosPsi - cosTheta * sinPhi * sinPsi),
v1[i].Y * (-cosPsi * sinPhi - cosPhi * cosTheta * sinPsi) + v1[i].X * (-cosPhi * cosPsi * cosTheta + sinPhi * sinPsi),
v1[i].X * cosPsi * sinTheta + v1[i].Y * sinPsi * sinTheta
);
}
//行列Alpha, Betaを作る
for (int k = 0; k < 3; k++)
{
for (int l = 0; l < 3; l++)
{
Alpha[k, l] = 0;
for (int i = 0; i < length; i++)
{
Alpha[k, l] += diff[k, i] * diff[l, i];
}
if (k == l)
{
Alpha[k, l] *= (1 + lambda);
}
}
Beta[k, 0] = 0;
for (int i = 0; i < length; i++)
{
Beta[k, 0] += ResidualCurrent[i] * diff[k, i];
}
}
if (!Alpha.TryInverse(out Matrix alphaInv))
{
return(-1);
}
var delta = alphaInv * Beta;
phi_New = phi - delta[0, 0];
theta_New = theta - delta[1, 0];
psi_New = psi + delta[2, 0];
//あたらしいパラメータでの残差の二乗和を計算
ResidualSquareNew = 0;
rot_New = Euler.SetEulerAngle(phi_New, theta_New, psi_New);
for (int i = 0; i < length; i++)
{
ResidualNew[i] = rot_New * v1[i] - v2[i];
ResidualSquareNew += ResidualNew[i] * ResidualNew[i];
}
if (ResidualSquareCurrent > ResidualSquareNew) //新旧の残差の二乗和を比較
{ //改善したとき
if ((ResidualSquareCurrent - ResidualSquareNew) / ResidualSquareCurrent > 0.0000000001 || count < 15 || lambda > 1)
{ //改善率が0.0000000001 以上 (まだまだ改善の余地がある)、あるいはcountが15以下 (回数が少ない)、あるいはlambdaが1より大きいとき (まだまだ改善の余地がある)
ResidualSquareCurrent = ResidualSquareNew; //残差の二乗和を書き換える
lambda *= 0.4; //lambdaを小さくする
for (int i = 0; i < length; i++)
{
ResidualCurrent[i] = ResidualNew[i]; //残差行列を書き換える
}
phi = phi_New; theta = theta_New; psi = psi_New; //新旧パラメータを書き換える
}
else
{
break;
}
}
else//改善しなかったとき
{
lambda *= 2.5;//lambdaを大きくする
}
}
return(ResidualSquareCurrent);
}
/// <summary>
/// 3次元ベクトル集合 v1をなるべくv2に近づけるような回転行列を求める。戻り値は、残差の二乗和を個数で割ったもの
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="result"></param>
/// <returns></returns>
public static double GetRotationMatrix2(Vector3DBase[] v1, Vector3DBase[] v2, ref Matrix3D result, Matrix3D initialRotation = null)
{
if (initialRotation == null)
{
//初期回転行列を求める
initialRotation = new Matrix3D();
}
var euler = Euler.GetEulerAngle(initialRotation);
int length = v1.Length;
double phi = euler.Phi, theta = euler.Theta, psi = euler.Psi;
double phi_Best = 0, theta_Best = 0, psi_Best = 0;
var rot = initialRotation;
double ResidualSquareCurrent = double.MaxValue, ResidualSquareNew = 0;
int count = 0;
//現在の残差を計算
double step = PI / 180.0 * 1.0;
while (count < 150)//試行回数が一定以上になった時、止める
{
for (int j2 = -1; j2 < 2; j2++)
{
var cosTheta = Cos(theta + step * j2);
var sinTheta = Sin(theta + step * j2);
for (int j3 = -1; j3 < 2; j3++)
{
var cosPsi = Cos(psi + step * j3);
var sinPsi = Sin(psi + step * j3);
double zDevSum = 0;
for (int i = 0; i < length; i++)
{
var zDev = sinPsi * sinTheta * v1[i].X + cosPsi * sinTheta * v1[i].Y + cosTheta * v1[i].Z - v2[i].Z;
zDevSum += zDev * zDev;
}
for (int j1 = -1; j1 < 2; j1++)
{
var cosPhi = Cos(phi + step * j1);
var sinPhi = Sin(phi + step * j1);
ResidualSquareNew = zDevSum;
for (int i = 0; i < length; i++)
{
var xDev = (cosPhi * cosPsi - cosTheta * sinPhi * sinPsi) * v1[i].X + (-cosPhi * sinPsi - cosTheta * sinPhi * cosPsi) * v1[i].Y + (sinTheta * sinPhi) * v1[i].Z - v2[i].X;
var yDev = (sinPhi * cosPsi + cosTheta * cosPhi * sinPsi) * v1[i].X + (-sinPhi * sinPsi + cosTheta * cosPhi * cosPsi) * v1[i].Y + (-sinTheta * cosPhi) * v1[i].Z - v2[i].Y;
ResidualSquareNew += xDev * xDev + yDev * yDev;
}
if (ResidualSquareCurrent > ResidualSquareNew) //新旧の残差の二乗和を比較
{ //改善したとき
ResidualSquareCurrent = ResidualSquareNew; //残差の二乗和を書き換える
phi_Best = phi + step * j1; theta_Best = theta + step * j2; psi_Best = psi + step * j3; //新旧パラメータを書き換える
}
}
}
}
if (phi_Best == phi && psi_Best == psi & theta_Best == theta)
{
step *= 0.7;
}
phi = phi_Best;
psi = psi_Best;
theta = theta_Best;
if (step < 0.01 / 180.0 * Math.PI)
{
break;
}
count++;
}
result = Euler.SetEulerAngle(phi, theta, psi);
return(ResidualSquareCurrent / length);
}
