private Matrix4x4 GetProjectionMatrix(FlexIO.Matrix intrinsics, float zNear, float zFar)
{
float p_x = (float)intrinsics[0, 2];
float p_y = (float)intrinsics[1, 2];
//the intrinsics are in Kinect coordinates: X - left, Y - up, Z, forward
//we need the coordinates to be: X - right, Y - down, Z - forward
p_x = imageWidth - p_x;
p_y = imageHeight - p_y;
// http://spottrlabs.blogspot.com/2012/07/opencv-and-opengl-not-always-friends.html
// http://opencv.willowgarage.com/wiki/Posit
Matrix4x4 projMat = new Matrix4x4();
projMat[0, 0] = (float)(2.0 * intrinsics[0, 0] / imageWidth);
projMat[1, 1] = (float)(2.0 * intrinsics[1, 1] / imageHeight);
projMat[2, 0] = (float)(-1.0f + 2 * p_x / imageWidth);
projMat[2, 1] = (float)(-1.0f + 2 * p_y / imageHeight);
// Note this changed from previous code
// see here: http://www.songho.ca/opengl/gl_projectionmatrix.html
projMat[2, 2] = -(zFar + zNear) / (zFar - zNear);
projMat[3, 2] = -2.0f * zNear * zFar / (zFar - zNear);
projMat[2, 3] = -1;
// Transpose tp fit Unity's column major matrix (in contrast to vision raw major ones).
projMat = projMat.transpose;
return(projMat);
}
public static FlexIO.Matrix Convert(Matrix4x4 uMat)
{
FlexIO.Matrix vMat = new FlexIO.Matrix(4, 4);
for (int i = 0; i < 16; i++)
{
vMat[i] = (double)(uMat[i]);
}
// Todo: ensure this works inplace
vMat.Transpose(vMat);
return(vMat);
}
public static Matrix4x4 Convert(FlexIO.Matrix vMat)
{
// argumement checking
if (vMat.Rows != 4 || vMat.Cols != 4)
{
throw new System.ArgumentException("vMat not 4x4");
}
Matrix4x4 uMat = new Matrix4x4();
for (int i = 0; i < 16; i++)
{
uMat[i] = (float)(vMat[i]);
}
// Todo: ensure this works inplace
uMat = uMat.transpose;
return(uMat);
}
public static Matrix4x4 Convert_3x3To4x4(FlexIO.Matrix vMat)
{
Matrix4x4 retVal = new Matrix4x4();
retVal[0, 0] = (float)vMat[0, 0];
retVal[0, 1] = (float)vMat[0, 1];
retVal[0, 2] = (float)vMat[0, 2];
retVal[1, 0] = (float)vMat[1, 0];
retVal[1, 1] = (float)vMat[1, 1];
retVal[1, 2] = (float)vMat[1, 2];
retVal[2, 0] = (float)vMat[2, 0];
retVal[2, 1] = (float)vMat[2, 1];
retVal[2, 2] = (float)vMat[2, 2];
retVal = retVal.transpose;
return(retVal);
}
