public static Matrix4 ToKinectMatrix4(this MatOfFloat pose)
{
var m1 = new float[4, 4];
pose.GetArray(0, 0, m1);
var matrix = new Matrix4();
matrix.M11 = m1[0, 0];
matrix.M12 = m1[0, 1];
matrix.M13 = m1[0, 2];
matrix.M14 = m1[0, 3];
matrix.M21 = m1[1, 0];
matrix.M22 = m1[1, 1];
matrix.M23 = m1[1, 2];
matrix.M24 = m1[1, 3];
matrix.M31 = m1[2, 0];
matrix.M32 = m1[2, 1];
matrix.M33 = m1[2, 2];
matrix.M34 = m1[2, 3];
matrix.M41 = m1[3, 0];
matrix.M42 = m1[3, 1];
matrix.M43 = m1[3, 2];
matrix.M44 = m1[3, 3];
return(matrix);
}
public static double[,] To2DArray(this MatOfFloat pose)
{
var m1 = new float[4, 4];
var m2 = new double[4, 4];
pose.GetArray(0, 0, m1);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
m2[i, j] = m1[i, j];
}
}
return(m2);
}
public static Point3f TransformPoint3f(this MatOfFloat pose, Point3f point)
{
if (pose.Cols != 4 || pose.Rows != 4)
{
throw new ArgumentException("Pose must be a 4 x 4 matrix");
}
var m1 = new float[4, 4];
pose.GetArray(0, 0, m1);
var v1 = new float[4] {
point.X, point.Y, point.Z, 1
};
var homogenous = new Vec4f(
(m1[0, 0] * v1[0] + m1[0, 1] * v1[1] + m1[0, 2] * v1[2] + m1[0, 3] * v1[3]),
(m1[1, 0] * v1[0] + m1[1, 1] * v1[1] + m1[1, 2] * v1[2] + m1[1, 3] * v1[3]),
(m1[2, 0] * v1[0] + m1[2, 1] * v1[1] + m1[2, 2] * v1[2] + m1[2, 3] * v1[3]),
(m1[3, 0] * v1[0] + m1[3, 1] * v1[1] + m1[3, 2] * v1[2] + m1[3, 3] * v1[3])
);
return(new Point3f(homogenous.Item0, homogenous.Item1, homogenous.Item2));
}
