使用 OpenCVsolvePnP 在 OpenGL 中实现增强现实

我正在尝试使用 Android 构建增强现实应用程序BoofCV http://boofcv.org/index.php?title=Main_Page（Java 的 OpenCV 替代品）和 OpenGL ES 2.0。我有一个标记，我可以使用 BoofCV 的solvePnP 函数获取图像点和“世界到凸轮”转换。我希望能够使用 OpenGL 在 3D 中绘制标记。这是我到目前为止所拥有的：

在相机的每一帧上，我调用solvePnP

Se3_F64 worldToCam = MathUtils.worldToCam(__qrWorldPoints, imagePoints);
mGLAssetSurfaceView.setWorldToCam(worldToCam);

这就是我定义的世界点

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}
};

我给它提供一个原点位于中心、边长以毫米为单位的正方形。

我可以确认我从中返回的旋转向量和平移向量solvePnP都是合理的，所以不知道这里有没有问题。

我将solvePnP 的结果传递到渲染器中

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];
    }
}

我还使用从相机校准中获得的相机内在函数来输入 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);

}

我正在绘制的正方形是在此定义的正方形谷歌示例 https://android.googlesource.com/platform/development/+/master/samples/OpenGL/HelloOpenGLES20/src/com/example/android/opengl/Square.java.

@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);
}

我认为问题与谷歌示例代码中正方形的定义没有考虑现实世界的边长这一事实有关。我知道OpenGL坐标系有角点(-1, 1), (-1, -1), (-1, 1), (1, 1)，它与我定义的毫米对象点不对应即使它们的顺序正确，也可以在 BoofCV 中使用。

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