Skip to content

Instantly share code, notes, and snippets.

@michael-nischt
Created June 2, 2011 18:37
Show Gist options
  • Save michael-nischt/1004986 to your computer and use it in GitHub Desktop.
Save michael-nischt/1004986 to your computer and use it in GitHub Desktop.
Convert computer vision camera matrices to OpenGL
/*
* Copyright (c) 2011 Michael Nischt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* Neither the name of the project's author nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
* TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
/**
* matrices are column major
*/
final class VisionGL {
public static float[] modelView(float[] R, float[] t) {
float[] mV = new float[4 * 4];
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
set4x4(mV, row, col, get3x3(R, row, col));
}
}
for (int i = 0; i < 3; i++) {
set4x4(mV, 3, i, 0);
set4x4(mV, i, 3, t[i]);
}
set4x4(mV, 3, 3, 1);
return mV;
}
public static float[] projection(float[] K, float nearZ, float farZ, int width, int height) {
float[] K_inverse = inverse3x3(K);
if (K_inverse == null) {
throw new IllegalArgumentException("K is not ivertible");
}
float left, bottom;
{
float[] p_image = {0, 0, 1};
float[] p_camera = transform3x3(K_inverse, p_image);
float s = nearZ / p_camera[2];
left = p_camera[0] * s;
bottom = p_camera[1] * s;
}
float right, top;
{
float[] p_image = {width, height, 1};
float[] p_camera = transform3x3(K_inverse, p_image);
float s = nearZ / p_camera[2];
right = p_camera[0] * s;
top = p_camera[1] * s;
}
float[] P = frustum(left, right, bottom, top, nearZ, farZ);
changeBase(P);
return P;
}
private static void changeBase(float[] P) {
// efficient version of: P * ScaleMatrix(1,-1,-1)
for (int col = 1; col <= 2; col++) {
for (int row = 0; row < 4; row++) {
set4x4(P, row, col, -set4x4(P, row, col));
}
}
}
private static float[] frustum(float left, float right, float bottom, float top, float near, float far) {
// same as glFrustum:
// RH = right: +x, up: +y, front: -z
// (0,0,-zNear) -> -1
// (0,0, -zFar) -> +1
if (near == far || left == right || bottom == top) {
throw new IllegalArgumentException("Not a viewing volume");
}
float width = right - left;
float height = top - bottom;
float depth = far - near;
float near2 = 2 * near;
float a = (right + left) / width;
float b = (top + bottom) / height;
float c = -(far + near) / depth;
float d = -near2 * far / depth;
float[] P = new float[4 * 4];
set4x4(P, 0, 0, near2 / width);
set4x4(P, 1, 0, 0);
set4x4(P, 2, 0, 0);
set4x4(P, 3, 0, 0);
set4x4(P, 0, 1, 0);
set4x4(P, 1, 1, near2 / height);
set4x4(P, 2, 1, 0);
set4x4(P, 3, 1, 0);
set4x4(P, 0, 2, a);
set4x4(P, 1, 2, b);
set4x4(P, 2, 2, c);
set4x4(P, 3, 2, -1);
set4x4(P, 0, 3, 0);
set4x4(P, 1, 3, 0);
set4x4(P, 2, 3, d);
set4x4(P, 3, 3, 0);
return P;
}
private static float det3x3(float[] M) {
return get3x3(M, 0, 0) * get3x3(M, 1, 1) * get3x3(M, 2, 2)
+ get3x3(M, 0, 1) * get3x3(M, 1, 2) * get3x3(M, 2, 0)
+ get3x3(M, 0, 2) * get3x3(M, 1, 0) * get3x3(M, 2, 1)
- get3x3(M, 0, 2) * get3x3(M, 1, 1) * get3x3(M, 2, 0)
- get3x3(M, 0, 1) * get3x3(M, 1, 0) * get3x3(M, 2, 2)
- get3x3(M, 0, 0) * get3x3(M, 1, 2) * get3x3(M, 2, 1);
}
private static float[] inverse3x3(float[] M) {
// | A B C |
//M = | D E F |
// | G H I |
// -1 1 | EI-FH -(BI-HC) BF-EC |
//M = ----- . | -(DI-FG) AI-GC -(AF-DC) |
// det M | DH-GE -(AH-GB) AE-BD |
float EPSILON = 1e-9f;
float det = det3x3(M);
if (Math.abs(det) < EPSILON) {
return null;
}
float[] M_inverse = new float[3 * 3];
set3x3(M_inverse, 0, 0, +(get3x3(M, 1, 1) * get3x3(M, 2, 2) - get3x3(M, 1, 2) * get3x3(M, 2, 1)) / det);
set3x3(M_inverse, 0, 1, -(get3x3(M, 0, 1) * get3x3(M, 2, 2) - get3x3(M, 0, 2) * get3x3(M, 2, 1)) / det);
set3x3(M_inverse, 0, 2, +(get3x3(M, 0, 1) * get3x3(M, 1, 2) - get3x3(M, 0, 2) * get3x3(M, 1, 1)) / det);
set3x3(M_inverse, 1, 0, -(get3x3(M, 1, 0) * get3x3(M, 2, 2) - get3x3(M, 1, 2) * get3x3(M, 2, 1)) / det);
set3x3(M_inverse, 1, 1, +(get3x3(M, 0, 0) * get3x3(M, 2, 2) - get3x3(M, 0, 2) * get3x3(M, 2, 1)) / det);
set3x3(M_inverse, 1, 2, -(get3x3(M, 0, 0) * get3x3(M, 1, 2) - get3x3(M, 0, 2) * get3x3(M, 1, 0)) / det);
set3x3(M_inverse, 2, 1, +(get3x3(M, 1, 0) * get3x3(M, 2, 1) - get3x3(M, 2, 1) * get3x3(M, 1, 1)) / det);
set3x3(M_inverse, 2, 1, -(get3x3(M, 0, 0) * get3x3(M, 2, 1) - get3x3(M, 2, 1) * get3x3(M, 0, 1)) / det);
set3x3(M_inverse, 2, 2, +(get3x3(M, 0, 0) * get3x3(M, 1, 1) - get3x3(M, 1, 0) * get3x3(M, 0, 1)) / det);
return M_inverse;
}
private static float[] transform3x3(float[] A, float[] x) {
float[] y = {
get3x3(A, 0, 0) * x[0] + get3x3(A, 0, 1) * x[1] + get3x3(A, 0, 2) * x[2],
get3x3(A, 1, 0) * x[0] + get3x3(A, 1, 1) * x[1] + get3x3(A, 1, 2) * x[2],
get3x3(A, 2, 0) * x[0] + get3x3(A, 2, 1) * x[1] + get3x3(A, 2, 2) * x[2]
};
return y;
}
private static float get3x3(float[] m, int row, int col) {
return m[3 * col + row];
}
private static float set4x4(float[] m, int row, int col) {
return m[4 * col + row];
}
private static void set3x3(float[] m, int row, int col, float value) {
m[3 * col + row] = value;
}
private static void set4x4(float[] m, int row, int col, float value) {
m[4 * col + row] = value;
}
private VisionGL() { /*
* static class => no instancing
*/ }
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment