/// <summary>
/// 射影行列をキャリブレーション行列と回転行列とカメラの同次座標系での位置ベクトルに分解する
/// </summary>
/// <param name="projMatr">入力の3x4の射影行列 P</param>
/// <param name="calibMatr">出力の3x3の内部キャリブレーション行列 K</param>
/// <param name="rotMatr">出力の3x3の外部回転行列 R</param>
/// <param name="posVect">出力の4x1の同次座標系での外部位置ベクトル C</param>
/// <param name="rotMatrX">オプション出力の3x3のx軸周りでの回転行列</param>
/// <param name="rotMatrY">オプション出力の3x3のy軸周りでの回転行列</param>
/// <param name="rotMatrZ">オプション出力の3x3のz軸周りでの回転行列</param>
/// <param name="eulerAngles">オプション出力の回転のオイラー角</param>
#else
/// <summary>
/// Computes projection matrix decomposition
/// </summary>
/// <param name="projMatr">The 3x4 input projection matrix P</param>
/// <param name="calibMatr">The output 3x3 internal calibration matrix K</param>
/// <param name="rotMatr">The output 3x3 external rotation matrix R</param>
/// <param name="posVect">The output 4x1 external homogenious position vector C</param>
/// <param name="rotMatrX">Optional 3x3 rotation matrix around x-axis</param>
/// <param name="rotMatrY">Optional 3x3 rotation matrix around y-axis</param>
/// <param name="rotMatrZ">Optional 3x3 rotation matrix around z-axis</param>
/// <param name="eulerAngles">Optional 3 points containing the three Euler angles of rotation</param>
#endif
public static void DecomposeProjectionMatrix(CvMat projMatr, CvMat calibMatr, CvMat rotMatr, CvMat posVect,
CvMat rotMatrX, CvMat rotMatrY, CvMat rotMatrZ, out CvPoint3D64f eulerAngles)
{
if (projMatr == null)
{
throw new ArgumentNullException(nameof(projMatr));
}
if (calibMatr == null)
{
throw new ArgumentNullException(nameof(calibMatr));
}
if (rotMatr == null)
{
throw new ArgumentNullException(nameof(rotMatr));
}
if (posVect == null)
{
throw new ArgumentNullException(nameof(posVect));
}
IntPtr rotMatrXPtr = (rotMatrX == null) ? IntPtr.Zero : rotMatrX.CvPtr;
IntPtr rotMatrYPtr = (rotMatrY == null) ? IntPtr.Zero : rotMatrY.CvPtr;
IntPtr rotMatrZPtr = (rotMatrZ == null) ? IntPtr.Zero : rotMatrZ.CvPtr;
eulerAngles = new CvPoint3D64f();
NativeMethods.cvDecomposeProjectionMatrix(projMatr.CvPtr, calibMatr.CvPtr, rotMatr.CvPtr, posVect.CvPtr,
rotMatrXPtr, rotMatrYPtr, rotMatrZPtr, ref eulerAngles);
KeepAlive(projMatr, calibMatr, rotMatr, posVect, rotMatrX, rotMatrY, rotMatrZ);
}
/// <summary>
/// ベクトルの外積
/// </summary>
/// <param name="vl"></param>
/// <param name="vr"></param>
/// <returns></returns>
private CvPoint3D64f CrossProduct(CvPoint3D64f vl, CvPoint3D64f vr)
{
CvPoint3D64f ret = new CvPoint3D64f
{
X = (vl.Y * vr.Z) - (vl.Z * vr.Y),
Y = (vl.Z * vr.X) - (vl.X * vr.Z),
Z = (vl.X * vr.Y) - (vl.Y * vr.X)
};
return(ret);
}
/// <summary>
/// 指定した点と直線の距離を返す
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
#else
/// <summary>
/// Returns the distance between this line and the specified point
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
#endif
public double Distance(double x, double y, double z)
{
CvPoint3D64f p = new CvPoint3D64f(x, y, z);
CvPoint3D64f a = new CvPoint3D64f(X1, Y1, Z1);
CvPoint3D64f b = new CvPoint3D64f(X1 + Vx, Y1 + Vy, Z1 + Vz);
CvPoint3D64f ab = new CvPoint3D64f {
X = b.X - a.X, Y = b.Y - a.Y, Z = b.Z - a.Z
};
CvPoint3D64f ap = new CvPoint3D64f {
X = p.X - a.X, Y = p.Y - a.Y, Z = p.Z - a.Z
};
// AB, APを外積 -> 平行四辺形Dの面積
double d = VectorLength(CrossProduct(ab, ap));
// AB間の距離
double l = VertexDistance(a, b);
// 平行四辺形の高さ(垂線)
double h = d / l;
return(h);
}
/// <summary>
/// 2点間(2ベクトル)の距離
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
private double VertexDistance(CvPoint3D64f p1, CvPoint3D64f p2)
{
return(Math.Sqrt((p2.X - p1.X) * (p2.X - p1.X) +
(p2.Y - p1.Y) * (p2.Y - p1.Y) +
(p2.Z - p1.Z) * (p2.Z - p1.Z)));
}
/// <summary>
/// ベクトルの長さ(原点からの距離)
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
private double VectorLength(CvPoint3D64f v)
{
return(Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z));
}
/// <summary>
/// 指定した点と直線の距離を返す
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
/// Returns the distance between this line and the specified point
/// </summary>
/// <param name="point"></param>
#endif
public double Distance(CvPoint3D64f point)
{
return(Distance(point.X, point.Y, point.Z));
}
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#endif
public CvPoint3D64f PerpendicularFoot(CvPoint3D64f point)
{
return(PerpendicularFoot(point.X, point.Y, point.Z));
}