/// <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); }