c# - Cant place obects properly in Unity (AR OpenCV)

Question

im trying to use aruco and opencv to do some AR experiments with Unity.

Im getting the camera pose and the position of two corners of an aruco marker. Now i want to place two axis on those two corners, but the result is this:

As you can see, the center of both axis are suposed to be in both down corners, but they appear to be very bad positioned. And, as i move the camera, for example, far away from the marker, the positioning is even worse.

I was taking measures and other things to ensure the coordinates are ok, so i was thinking about the problem can be the frustrum matrix, field or view or something like this.

Here is what i do:

piece.transform.position= new Vector3((float)c0cords[0], -(float)c0cords[1],(float) c0cords[2]);

In this line, i place one of the axis in the corner 0 of the marker, i inverted the "y".

And here is how i craft the frustrum matrix:

    bool readMatrixIntrinsic () {
    IntPtr Intrinsic = QTNative.getIntrinsicMatrix(pathCalibration);

    if (Intrinsic != IntPtr.Zero) {
        float[] matrix = new float[9];
        Marshal.Copy (Intrinsic, matrix, 0, 9);
        float fx = matrix[0];
        float fy = matrix[4];
        float cx = matrix[2];
        float cy = matrix[5];

        Debug.LogWarning ("*************** Matrix Intrinsic ***************");
        Debug.Log ("Vector:  fx: " + fx + ", fy: " + fy);
        Debug.Log ("Vector:  cx: " + cx + ", cy: " + cy);

        cameraMain.projectionMatrix = LoadProjectionMatrix (fx, fy, cx, cy);

        return true;
    }
    return false;
}

Here i read the YML file with camera parameters and then i call loadProjectionMatrix to get that matrix:

private Matrix4x4 LoadProjectionMatrix (float fx, float fy, float cx, float cy) {
    // https://github.com/kylemcdonald/ofxCv/blob/88620c51198fc3992fdfb5c0404c37da5855e1e1/libs/ofxCv/src/Calibration.cpp
    //float w = _mainCamera.pixelWidth;
    //float h = _mainCamera.pixelHeight;
    //float nearDist = _mainCamera.nearClipPlane;
    //float farDist = _mainCamera.farClipPlane;

    float w = 1280;
    float h = 720;

    float nearDist = cameraMain.nearClipPlane;
    float farDist = cameraMain.farClipPlane;

    return MakeFrustumMatrix (
        nearDist * (-cx) / fx, nearDist * (w - cx) / fx,
        nearDist * (cy) / fy, nearDist * (cy - h) / fy,
        nearDist, farDist);
}

private Matrix4x4 MakeFrustumMatrix (float left, float right, float bottom, float top, float zNear, float zFar) {
    // https://github.com/openframeworks/openFrameworks/blob/master/libs/openFrameworks/math/ofMatrix4x4.cpp
    // note transpose of ofMatrix4x4 wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
    // NB this has been transposed here from the original openframeworks code

    float A = (right + left) / (right - left);
    float B = (top + bottom) / (top - bottom);
    float C = -(zFar + zNear) / (zFar - zNear);
    float D = -2.0f * zFar * zNear / (zFar - zNear);

    var persp = new Matrix4x4 ();
    persp[0, 0] = 2.0f * zNear / (right - left);
    persp[1, 1] = 2.0f * zNear / (top - bottom);
    persp[2, 0] = A;
    persp[2, 1] = B;
    persp[2, 2] = C;
    persp[2, 3] = -1.0f;
    persp[3, 2] = D;

    var rhsToLhs = new Matrix4x4 ();
    rhsToLhs[0, 0] = 1.0f;
    rhsToLhs[1, 1] = -1.0f; // Flip Y (RHS -> LHS)
    rhsToLhs[2, 2] = 1.0f;
    rhsToLhs[3, 3] = 1.0f;

    return rhsToLhs * persp.transpose; // see comment above
}

In this problem, the marker should be in the same position always, and i just want to move the camera arround it.

Answer 1

等待大神答复