Develop Reference

Using OpenCV solvePnP for Augmented Reality in OpenGL

I'm trying to build an Augmented Reality application in Android using BoofCV (OpenCV alternative for Java) and OpenGL ES 2.0. I have a marker which I can get the image points of and "world to cam" transformation using BoofCV's solvePnP function. I want to be able to draw the marker in 3D using OpenGL. Here's what I have so far:
On every frame of the camera, I call solvePnP
Se3_F64 worldToCam = MathUtils.worldToCam(__qrWorldPoints, imagePoints);
mGLAssetSurfaceView.setWorldToCam(worldToCam);
This is what I have defined as the world points
static float qrSideLength = 79.365f; // mm
private static final double[][] __qrWorldPoints = {
{qrSideLength * -0.5, qrSideLength * 0.5, 0},
{qrSideLength * -0.5, qrSideLength * -0.5, 0},
{qrSideLength * 0.5, qrSideLength * -0.5, 0},
{qrSideLength * 0.5, qrSideLength * 0.5, 0}
};
I'm feeding it a square that has origin at its center, with a sidelength in millimeters.
I can confirm that the rotation vector and translation vector I'm getting back from solvePnP are reasonable, so I don't know if there's a problem here.
I pass the result from solvePnP into my renderer
public void setWorldToCam(Se3_F64 worldToCam) {
DenseMatrix64F _R = worldToCam.R;
Vector3D_F64 _T = worldToCam.T;
// Concatenating the the rotation and translation vector into
// a View matrix
double[][] __view = {
{_R.get(0, 0), _R.get(0, 1), _R.get(0, 2), _T.getX()},
{_R.get(1, 0), _R.get(1, 1), _R.get(1, 2), _T.getY()},
{_R.get(2, 0), _R.get(2, 1), _R.get(2, 2), _T.getZ()},
{0, 0, 0, 1}
};
DenseMatrix64F _view = new DenseMatrix64F(__view);
// Matrix to convert from BoofCV (OpenCV) coordinate system to OpenGL coordinate system
double[][] __cv_to_gl = {
{1, 0, 0, 0},
{0, -1, 0, 0},
{0, -1, 0, 0},
{0, 0, 0, 1}
};
DenseMatrix64F _cv_to_gl = new DenseMatrix64F(__cv_to_gl);
// Multiply the View Matrix by the BoofCV to OpenGL matrix to apply the coordinate transform
DenseMatrix64F view = new SimpleMatrix(__view).mult(new SimpleMatrix(__cv_to_gl)).getMatrix();
// BoofCV stores matrices in row major order, but OpenGL likes column major order
// I transpose the view matrix and get a flattened list of 16,
// Then I convert them to floating point
double[] viewd = new SimpleMatrix(view).transpose().getMatrix().getData();
for (int i = 0; i < mViewMatrix.length; i++) {
mViewMatrix[i] = (float) viewd[i];
}
}
I'm also using the camera intrinsics I get from camera calibration to feed into the projection matrix of OpenGL
#Override
public void onSurfaceChanged(GL10 gl, int width, int height) {
// this projection matrix is applied to object coordinates
// in the onDrawFrame() method
double fx = MathUtils.fx;
double fy = MathUtils.fy;
float fovy = (float) (2 * Math.atan(0.5 * height / fy) * 180 / Math.PI);
float aspect = (float) ((width * fy) / (height * fx));
// be careful with this, it could explain why you don't see certain objects
float near = 0.1f;
float far = 100.0f;
Matrix.perspectiveM(mProjectionMatrix, 0, fovy, aspect, near, far);
GLES20.glViewport(0, 0, width, height);
}
The square I'm drawing is the one defined in this Google example.
#Override
public void onDrawFrame(GL10 gl) {
// redraw background color
GLES20.glClear(GLES20.GL_COLOR_BUFFER_BIT);
// Set the camera position (View matrix)
// Matrix.setLookAtM(mViewMatrix, 0, 0, 0, -3, 0f, 0f, 0f, 0f, 1.0f, 0.0f);
// Combine the rotation matrix with the projection and camera view
// Note that the mMVPMatrix factor *must be the first* in order
// for matrix multiplication product to be correct
// Calculate the projection and view transformation
Matrix.multiplyMM(mMVPMatrix, 0, mProjectionMatrix, 0, mViewMatrix, 0);
// Draw shape
mSquare.draw(mMVPMatrix);
}
I believe the problem has to do with the fact that this definition of a square in Google's example code doesn't take the real world side length into account. I understand that the OpenGL coordinate system has the corners (-1, 1), (-1, -1), (-1, 1), (1, 1) which doesn't correspond to the millimeter object points I have defined for use in BoofCV, even though they are in the right order.
static float squareCoords[] = {
-0.5f, 0.5f, 0.0f, // top left
-0.5f, -0.5f, 0.0f, // bottom left
0.5f, -0.5f, 0.0f, // bottom right
0.5f, 0.5f, 0.0f }; // top right

Convert OpenGL 3D point to 2D using GLU.glProject

I have an OpenGL scene with a sphere having a radius of 1, and the camera being at the center of the sphere (it's a 360° picture viewer). The user can rotate the sphere by panning.
Now I need to display 2D pins "attached" to some parts of the picture. To do so, I want to convert the 3D coordinates of my pins into 2D screen coordinates, to add the pin image at that screen coordinates.
I'm using GLU.glProject and the following classes from android-apidemo:
MatrixGrabber
MatrixStack
MatrixTrackingGL
I save the projection matrix in the onSurfaceChanged method and the model-view matrix in the onDraw method (after having drawn my sphere). Then I feed GLU.glProject with them when the user rotates the sphere to update the pins position.
When I pan horizontally, the pins pan correctly, but when I pan vertically, the texture pans "faster" than the pin image (like if the pin was closer to the camera than the sphere).
Here are some relevant parts of my code:
public class CustomRenderer implements GLSurfaceView.Renderer {
MatrixGrabber mMatrixGrabber = new MatrixGrabber();
private float[] mModelView = null;
private float[] mProjection = null;
[...]
#Override
public void onSurfaceChanged(GL10 gl, int width, int height) {
// Get the sizes:
float side = Math.max(width, height);
int x = (int) (width - side) / 2;
int y = (int) (height - side) / 2;
// Set the viewport:
gl.glViewport(x, y, (int) side, (int) side);
// Set the perspective:
gl.glMatrixMode(GL10.GL_PROJECTION);
gl.glLoadIdentity();
GLU.gluPerspective(gl, FIELD_OF_VIEW_Y, 1, Z_NEAR, Z_FAR);
// Grab the projection matrix:
mMatrixGrabber.getCurrentProjection(gl);
mProjection = mMatrixGrabber.mProjection;
// Set to MODELVIEW mode:
gl.glMatrixMode(GL10.GL_MODELVIEW);
gl.glLoadIdentity();
}
#Override
public void onDrawFrame(GL10 gl) {
// Load the texture if needed:
if(mTextureToLoad != null) {
mSphere.loadGLTexture(gl, mTextureToLoad);
mTextureToLoad = null;
}
// Clear:
gl.glClearColor(0.5f, 0.5f, 0.5f, 0.0f);
gl.glClear(GL10.GL_COLOR_BUFFER_BIT | GL10.GL_DEPTH_BUFFER_BIT);
gl.glLoadIdentity();
// Rotate the scene:
gl.glRotatef( (1 - mRotationY + 0.25f) * 360, 1, 0, 0); // 0.25 is used to adjust the texture position
gl.glRotatef( (1 - mRotationX + 0.25f) * 360, 0, 1, 0); // 0.25 is used to adjust the texture position
// Draw the sphere:
mSphere.draw(gl);
// Grab the model-view matrix:
mMatrixGrabber.getCurrentModelView(gl);
mModelView = mMatrixGrabber.mModelView;
}
public float[] getScreenCoords(float x, float y, float z) {
if(mModelView == null || mProjection == null) return null;
float[] result = new float[3];
int[] view = new int[] {0, 0, (int) mSurfaceViewSize.getWidth(), (int) mSurfaceViewSize.getHeight()};
GLU.gluProject(x, y, z,
mModelView, 0,
mProjection, 0,
view, 0,
result, 0);
result[1] = mSurfaceViewSize.getHeight() - result[1];
return result;
}
}
I use the result of the getScreenCoords method to display my pins. The y value is wrong.
What am I doing wrong?

Android OpenGL set rotation speed of cube

I have cube that rotates around the center of the coordinates system. But the problem is it rotates very slowly. So in my case how to set the rotation speed?
The following three methods update the mCurrentModelMatrix with the given model transformation. These are stateful accumulative methods.
public void trnslate(float x, float y, float z)
{
float[] tempModelMatrix = new float[16];
Matrix.setIdentityM(tempModelMatrix, 0);
Matrix.translateM(tempModelMatrix,0,x,y,z);
Matrix.multiplyMM(this.mCurrentModelMatrix, 0,
tempModelMatrix, 0, this.mCurrentModelMatrix, 0);
}
public void rotate(float angle, float x, float y, float z)
{
float[] tempModelMatrix = new float[16];
Matrix.setIdentityM(tempModelMatrix, 0);
Matrix.rotateM(tempModelMatrix,0,angle,x,y,z);
Matrix.multiplyMM(this.mCurrentModelMatrix, 0,
tempModelMatrix, 0, this.mCurrentModelMatrix, 0);
}
public void scale(float xFactor, float yFactor, float zFactor)
{
float[] tempModelMatrix = new float[16];
Matrix.setIdentityM(tempModelMatrix, 0);
Matrix.scaleM(tempModelMatrix,0,xFactor,yFactor,zFactor);
Matrix.multiplyMM(this.mCurrentModelMatrix, 0,
tempModelMatrix, 0, this.mCurrentModelMatrix, 0);
}
/*
* Calculaute the final model view matrix
* 1. Order of matrix multiplication is important
* 2. MVPmatrix = proj * view * model;
* 3. Setup the MVP matrix in the vertex shader memory
*/
protected void setupMatrices()
{
float[] tempModelMatrix = new float[16];
Matrix.setIdentityM(tempModelMatrix, 0);
//translate the model combo next
Matrix.multiplyMM(mMVPMatrix, 0, //matrix and offset
mCurrentModelMatrix, 0,
tempModelMatrix, 0);
//translate eye coordinates first
Matrix.multiplyMM(mMVPMatrix, 0,
this.mVMatrix, 0,
mMVPMatrix, 0);
//Project it: screen coordinates
Matrix.multiplyMM(mMVPMatrix, 0,
mProjMatrix, 0,
mMVPMatrix, 0);
//Set the vertex uniform handler representing the MVP matrix
GLES20.glUniformMatrix4fv(muMVPMatrixHandle, //uniform handle
1, //number of uniforms. 1 if it is not an array
false, //transpose: must be false
mMVPMatrix, //client matrix memory pointer
0); //offset
}
draw method
// Drawing operation
#Override
protected void draw(GL10 gl, int positionHandle) {
// Hide the hidden surfaces using these APIs
GLES20.glEnable(GLES20.GL_DEPTH_TEST);
GLES20.glDepthFunc(GLES20.GL_LESS);
// Transfer vertices to the shader
transferVertexPoints(positionHandle);
// Transfer texture points to the shader
transferTexturePoints(getTextureHandle());
// Implement rotation from 0 to 360 degrees
// Stop when asked and restart when the stopFlag
// is set to false.
// Decide what the current angle to apply
// for rotation is.
if (stopFlag == true) {
// stop rotation
curAngle = stoppedAtAngle;
} else {
curAngle += 1.0f;
}
if (curAngle > 360) {
curAngle = 0;
}
// Tell the base class to start their
// matrices to unit matrices.
this.initializeMatrices();
// The order of these model transformations matter
// Each model transformation is specified with
// respect to the last one, and not the very first.
// Center the cube
this.trnslate(0, 0, -1);
// Rotate it around y axis
this.rotate(curAngle, 0, -1, 0);
// Decenter it to where ever you want
this.trnslate(0, -2, 2);
// Go ahead calculate the ModelViewMatrix as
// we are done with ALL of our model transformations
this.setupMatrices();
// Call glDrawArrays to use the vertices and draw
int vertexCount = mTriangleVerticesData.length / 3;
GLES20.glDrawArrays(GLES20.GL_TRIANGLES, // what primitives to use
0, // at what point to start
vertexCount); // Starting there how many points to use
// Check if there are errors
checkGlError("glDrawArrays");
}
Thanks in advance!

You are rotating at 1 degree per frame, so it will take 360 frames to do a complete rotation.
If you want it to rotate in 2 seconds, and you were running at 30 frames per second, you would want to rotate by 6 degrees per frame, by changing this section:
if (stopFlag == true) {
// stop rotation
curAngle = stoppedAtAngle;
} else {
curAngle += 6.0f;
}
if (curAngle > 360) {
curAngle = 0;
}

Android OpenGLES 2.0 Buttons

I am a newbee to android OpenGL i am trying to draw buttons using OpenGL I have added a Gesture Listener for the GLSurface View now i have motionevent when ever the user touches. My question is how can i convert motionevent.getx and motionevent.gety (which are in pixel range
)to window or Object coordinates of the view?

I Found the solution for this question posting in case if some one requires it.
public float[] convertToObjectCoordinates(MotionEvent event) {
float[] worldPos = new float[2];
float[] invertedMatrix, transformMatrix,
normalizedInPoint, outPoint, mProjMatrix, mVMatrix;
invertedMatrix = new float[16];
transformMatrix = new float[16];
mProjMatrix = new float[16];
mProjMatrix = mRenderer.getmProjMatrix();
mVMatrix = new float[16];
//Change the Proj and ModelView matrix according to your model and view matrix or you can use your mvpMatrix directly instead of transform Matrix
Matrix.setLookAtM(mVMatrix, 0, 0, 0, -3, 0, 0, 0f, 0.0f, 1.0f, 0.0f);
normalizedInPoint = new float[4];
outPoint = new float[4];
float y = screenHeight - event.getY();
setHeightAndWidth();
normalizedInPoint[0] = (float) ((event.getX()) * 2.0f / screenWidth - 1.0);
normalizedInPoint[1] = (float) ((y) * 2.0f / screenHeight - 1.0);
normalizedInPoint[2] = - 1.0f;
normalizedInPoint[3] = 1.0f;
Matrix.multiplyMM( transformMatrix, 0, mProjMatrix, 0, mVMatrix, 0);
Matrix.invertM(invertedMatrix, 0, transformMatrix, 0);
Matrix.multiplyMV(outPoint, 0, invertedMatrix, 0, normalizedInPoint, 0);
if (outPoint[3] != 0.0)
{
worldPos[0] = outPoint[0] / outPoint[3];
worldPos[1] = outPoint[1] / outPoint[3];
} else {
Log.e("Error", "Normalised Zero Error");
}
return worldPos;
}
This is remake from the following post for Android OpenGL2.0 :
Android OpenGL ES 2.0 screen coordinates to world coordinates
EROl's Answer
thanks to EROl for his reply.
public void setHeightAndWidth() {
screenHeight = this.getHeight();
screenWidth = this.getWidth();
}
The above method should be written in the GLSurfaceView class so that it gives the exact view's height and width. If your View occupies complete screen you may also use Display metrics to get complete screen width and height.

Android OpenGL ES 2.0 screen coordinates to world coordinates

I'm building an Android application that uses OpenGL ES 2.0 and I've run into a wall. I'm trying to convert screen coordinates (where the user touches) to world coordinates. I've tried reading and playing around with GLU.gluUnProject but I'm either doing it wrong or just don't understand it.
This is my attempt....
public void getWorldFromScreen(float x, float y) {
int viewport[] = { 0, 0, width , height};
float startY = ((float) (height) - y);
float[] near = { 0.0f, 0.0f, 0.0f, 0.0f };
float[] far = { 0.0f, 0.0f, 0.0f, 0.0f };
float[] mv = new float[16];
Matrix.multiplyMM(mv, 0, mViewMatrix, 0, mModelMatrix, 0);
GLU.gluUnProject(x, startY, 0, mv, 0, mProjectionMatrix, 0, viewport, 0, near, 0);
GLU.gluUnProject(x, startY, 1, mv, 0, mProjectionMatrix, 0, viewport, 0, far, 0);
float nearX = near[0] / near[3];
float nearY = near[1] / near[3];
float nearZ = near[2] / near[3];
float farX = far[0] / far[3];
float farY = far[1] / far[3];
float farZ = far[2] / far[3];
}
The numbers I am getting don't seem right, is this the right way to utilize this method? Does it work for OpenGL ES 2.0? Should I make the Model Matrix an identity matrix before these calculations (Matrix.setIdentityM(mModelMatix, 0))?
As a follow up, if this is correct, how do I pick the output Z? Basically, I always know at what distance I want the world coordinates to be at, but the Z parameter in GLU.gluUnProject appears to be some kind of interpolation between the near and far plane. Is it just a linear interpolation?
Thanks in advance

/**
* Calculates the transform from screen coordinate
* system to world coordinate system coordinates
* for a specific point, given a camera position.
*
* #param touch Vec2 point of screen touch, the
actual position on physical screen (ej: 160, 240)
* #param cam camera object with x,y,z of the
camera and screenWidth and screenHeight of
the device.
* #return position in WCS.
*/
public Vec2 GetWorldCoords( Vec2 touch, Camera cam)
{
// Initialize auxiliary variables.
Vec2 worldPos = new Vec2();
// SCREEN height & width (ej: 320 x 480)
float screenW = cam.GetScreenWidth();
float screenH = cam.GetScreenHeight();
// Auxiliary matrix and vectors
// to deal with ogl.
float[] invertedMatrix, transformMatrix,
normalizedInPoint, outPoint;
invertedMatrix = new float[16];
transformMatrix = new float[16];
normalizedInPoint = new float[4];
outPoint = new float[4];
// Invert y coordinate, as android uses
// top-left, and ogl bottom-left.
int oglTouchY = (int) (screenH - touch.Y());
/* Transform the screen point to clip
space in ogl (-1,1) */
normalizedInPoint[0] =
(float) ((touch.X()) * 2.0f / screenW - 1.0);
normalizedInPoint[1] =
(float) ((oglTouchY) * 2.0f / screenH - 1.0);
normalizedInPoint[2] = - 1.0f;
normalizedInPoint[3] = 1.0f;
/* Obtain the transform matrix and
then the inverse. */
Print("Proj", getCurrentProjection(gl));
Print("Model", getCurrentModelView(gl));
Matrix.multiplyMM(
transformMatrix, 0,
getCurrentProjection(gl), 0,
getCurrentModelView(gl), 0);
Matrix.invertM(invertedMatrix, 0,
transformMatrix, 0);
/* Apply the inverse to the point
in clip space */
Matrix.multiplyMV(
outPoint, 0,
invertedMatrix, 0,
normalizedInPoint, 0);
if (outPoint[3] == 0.0)
{
// Avoid /0 error.
Log.e("World coords", "ERROR!");
return worldPos;
}
// Divide by the 3rd component to find
// out the real position.
worldPos.Set(
outPoint[0] / outPoint[3],
outPoint[1] / outPoint[3]);
return worldPos;
}
Algorithm is further explained here.

Hopefully my question (and answer) should help you out:
How to find absolute position of click while zoomed in
It has not only the code but also diagrams and diagrams and diagrams explaining it :) Took me ages to figure it out as well.

IMHO one doesn't need to re-implement this function...
I experimented with Erol's solution and it worked, so thanks a lot for it Erol.
Furthermore, I played with
Matrix.orthoM(mtrxProjection, 0, left, right, bottom, top, near, far);
and it works fine as well in my tiny noob example 2D OpenGL ES 2.0 project:
public void onSurfaceChanged(GL10 unused, int width, int height) {...