public Matrix3D GetStrain(Symmetry symmetry, Matrix3D rotation, Matrix3D stress)
{
Matrix3D m1 = rotation.Transpose() * stress * rotation;
Matrix3D m2 = GetStrain(symmetry, m1);
Matrix3D m = rotation * m2 * rotation.Transpose();
return(m);
}
/// <summary>
/// 楕円の中心群とそれらの半径から、真の中心のオフセット位置(offset)と傾き(tau, phi)を返す
/// </summary>
/// <param name="CenterPt"></param>
/// <param name="Peaks"></param>
/// <param name="PixelSize"></param>
/// <param name="CameraLength"></param>
/// <param name="offset"></param>
/// <param name="tau"></param>
/// <param name="phi"></param>
public static void GetTiltAndOffset(PointD[] EllipseCenter, double[] Radius, double CameraLength, ref PointD offset,
ref double tau, ref double phi)
{
//まずCenterPtの回帰直線を求める
double phi1, A;
phi1 = A = 0;
Statistics.LineFitting(EllipseCenter, ref phi1, ref A);
double CosPhi1 = Math.Cos(phi1);
double SinPhi1 = Math.Sin(phi1);
bool xMode = Math.Abs(CosPhi1) > 1 / Math.Sqrt(2);
//この直線上の点B(x,y)と、各CenterPtの距離Riとしたとき
//δ^2= ( Ri - Cameralength * Tan(2θ)^2 *Sin(ψ) / pixelSize )^2
//が最小になるような点Bとψをさがす
double[] TheoriticalRperSinPsi = new double[EllipseCenter.Length];
double startTau1 = -Math.PI / 180;
double stepTau1 = Math.PI / 9000;
double endTau1 = Math.PI / 180;
double tau1, bestTau1;
tau1 = 0;
double startCenter = -5;
double stepCenter = 0.1;
double endCenter = 5;
double Center, BestCenter;
double BestY, BestX, X, Y;
double residual, bestResidual;
bestResidual = double.PositiveInfinity;
BestX = BestY = bestTau1 = BestCenter = 0;
//double temp;
for (int n = 0; n < 40; n++)
{
for (Center = startCenter; Center <= endCenter; Center += stepCenter)
{
if (xMode)
{
X = Center;
Y = (X * SinPhi1 - A) / CosPhi1;
}
else
{
Y = Center;
X = (Y * CosPhi1 + A) / SinPhi1;
}
for (tau1 = startTau1; tau1 <= endTau1; tau1 += stepTau1)
{
residual = 0;
for (int i = 0; i < EllipseCenter.Length; i++)
{
//現在のTau,X,Yから予想される半径Rの円の中心位置は
double IdealX, IdealY;
double TwoTheta = Math.Atan2(Radius[i], CameraLength);
//Rはtauが正のとき正、負のとき負
//double R = CameraLength * 2 * Math.Sin(TwoTheta) * Math.Sin(TwoTheta) * Math.Sin(tau1) / (Math.Cos(2 * TwoTheta) + Math.Cos(2 * tau1));
//1/2 CL tan2q { sinj [tan(2q+j) -tan(2q-j)] }
double R = 0.5 * CameraLength * Math.Sin(TwoTheta) * (1 / Math.Cos(TwoTheta + tau1) - 1 / Math.Cos(TwoTheta - tau1));
if (tau1 > 0)
{
R = Math.Abs(R);
}
else
{
R = -Math.Abs(R);
}
IdealX = R * CosPhi1;
IdealY = R * SinPhi1;
if (xMode && tau1 * IdealX <= 0)//横方向に広がっていて tau1 > 0 かつ idealX <0 あるいは tau1 < 0 かつ idealX > 0 のときは
{
IdealX = -IdealX;
IdealY = -IdealY;
}
if (!xMode && tau1 * IdealY <= 0)//縦方向に広がっていて tau1 > 0 かつ idealY <0 あるいは tau1 < 0 かつ idealY > 0 のときは
{
IdealX = -IdealX;
IdealY = -IdealY;
}
//ここまでで、Tau すなわち Rが正のときは楕円の中心もX,Yのいずれかの方向に正に振れる事になる
residual += (X + IdealX - EllipseCenter[i].X) * (X + IdealX - EllipseCenter[i].X) * 1000
+ (Y + IdealY - EllipseCenter[i].Y) * (Y + IdealY - EllipseCenter[i].Y) * 1000;
}
if (residual < bestResidual)
{
bestResidual = residual;
BestX = X;
BestY = Y;
BestCenter = Center;
bestTau1 = tau1;
}
}
}
startCenter = BestCenter - 2.4 * stepCenter;
endCenter = BestCenter + (2.4 * stepCenter);
stepCenter *= 0.8;
startTau1 = bestTau1 - 2.4 * stepTau1;
endTau1 = bestTau1 + 2.4 * stepTau1;
stepTau1 *= 0.8;
}//最適化終了
offset = new PointD(BestX, BestY);
//無条件に270°引いて
phi1 -= 9 * Math.PI / 2;
//xModeが真かつtauが正のとき. このとき回転軸は-135°〜-45°になるべき
if (xMode && bestTau1 >= 0)
{
while (phi1 < -Math.PI * 3 / 4 - 0.01)//-135°〜-45°の範囲じゃないときは
{
phi1 += Math.PI;
}
}
//xModeが真かつtauが負のとき. このとき回転軸は45°〜135°になるべき
if (xMode && bestTau1 < 0)
{
while (phi1 < Math.PI / 4 - 0.01)//45°以下のときは
{
phi1 += Math.PI;
}
}
//xModeが偽かつtauが正のとき. このとき回転軸は-45°〜45°になるべき
if (!xMode && bestTau1 >= 0)
{
while (phi1 < -Math.PI / 4 - 0.01)//-45°〜45°の範囲じゃないときは
{
phi1 += Math.PI;
}
}
//xModeが偽かつtauが負のとき.このとき回転軸は135°〜215°になるべき
if (!xMode && bestTau1 < 0)
{
while (phi1 < Math.PI * 3 / 4 - 0.01)//135°〜215°の範囲じゃないときは
{
phi1 += Math.PI;
}
}
bestTau1 = Math.Abs(bestTau1);
double CosPhi = Math.Cos(phi);
double SinPhi = Math.Sin(phi);
double CosTau = Math.Cos(tau);
double SinTau = Math.Sin(tau);
CosPhi1 = Math.Cos(phi1);
SinPhi1 = Math.Sin(phi1);
double CosTau1 = Math.Cos(bestTau1);
double SinTau1 = Math.Sin(bestTau1);
var M = new Matrix3D(CosPhi, SinPhi, 0, -SinPhi, CosPhi, 0, 0, 0, 1);
var P = new Matrix3D(1, 0, 0, 0, CosTau, SinTau, 0, -SinTau, CosTau);
var M1 = new Matrix3D(CosPhi1, SinPhi1, 0, -SinPhi1, CosPhi1, 0, 0, 0, 1);
var P1 = new Matrix3D(1, 0, 0, 0, CosTau1, SinTau1, 0, -SinTau1, CosTau1);
var Q = M1 * P1 * M1.Transpose() * M * P * M.Transpose();
tau = Math.Acos(Q.E33);
if (Math.Sin(tau) == 0)
{
phi = 0;
}
else
{
phi = Math.Atan2(-Q.E31, Q.E32);
}
bestResidual = double.PositiveInfinity;
double startPhi = phi - 0.1;
double endPhi = phi + 0.1;
double stepPhi = 0.01;
double startTau = tau - 0.01;
double endTau = tau + 0.01;
double stepTau = 0.001;
double bestPhi = phi;
double bestTau = tau;
double temp;
for (int n = 0; n < 30; n++)
{
for (phi = startPhi; phi <= endPhi; phi += stepPhi)
{
for (tau = startTau; tau <= endTau; tau += stepTau)
{
CosPhi = Math.Cos(phi);
SinPhi = Math.Sin(phi);
CosTau = Math.Cos(tau);
SinTau = Math.Sin(tau);
residual = 0;
temp = Q.E11 - (CosPhi * CosPhi + CosTau * SinPhi * SinPhi);
residual += temp * temp;
temp = Q.E12 - (CosPhi * SinPhi - CosTau * CosPhi * SinPhi);
residual += temp * temp;
temp = Q.E13 - (SinPhi * SinTau);
residual += temp * temp;
temp = Q.E21 - (CosPhi * SinPhi - CosPhi * CosTau * SinPhi);
residual += temp * temp;
temp = Q.E22 - (CosPhi * CosPhi * CosTau + SinPhi * SinPhi);
residual += temp * temp;
temp = Q.E23 - (-CosPhi * SinTau);
residual += temp * temp;
temp = Q.E31 - (-SinPhi * SinTau);
residual += temp * temp;
temp = Q.E32 - (CosPhi * SinTau);
residual += temp * temp;
temp = Q.E33 - (CosTau);
residual += temp * temp;
if (bestResidual > residual)
{
bestPhi = phi;
bestTau = tau;
bestResidual = residual;
}
}
}
startPhi = bestPhi - stepPhi;
endPhi = bestPhi + stepPhi;
stepPhi *= 0.4;
startTau = bestTau - stepTau;
endTau = bestTau + stepTau;
stepTau *= 0.4;
}
phi = bestPhi;
tau = bestTau;
}
