[SLH]

[More attachments in following post]

Here's my progress so far...

The test object should be a blue box the size of the marker, with a red box indicating the centre and then a grid of spheres just to show a bunch of positions in 3D space.

The translation tracking appears to be working (the red square is always in the centre of the marker). But the rotations are wrong.  It looks like I need to add a rotation around one axis or flip an axis. But I have no idea where or how.

Here is my code for the important bits.  Hopefully someone can spot an obvious bug!

View matrix code from the pose provided by openCV. cvRotation is the rodrigues function's output from the rotation. It is 3x3 matrix. cvT is the translation:

Code:

      private SharpDX.Matrix OpenCVToWPFViewMatrix(Mat cvRotation, Mat cvT)
        {
            var srcRot = cvRotation;
            CvInvoke.Invert(srcRot, srcRot, Emgu.CV.CvEnum.DecompMethod.LU);
            var r = Mat3x3ToSharpDXMatrix(srcRot);
            var t = Mat3x1ToSharpDXVector(cvT);
            var viewMatrix = new SharpDX.Matrix(
                0, 0, 0, 0,
                0, 0, 0, 0,
                0, 0, 0, 0,
                0, 0, 0, 0);
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    viewMatrix[i, j] = r[i, j];
                }
                viewMatrix[i, 3] = t[i];
            }
            viewMatrix[3, 3] = 1;
//this bit here is to flip signs etc. 99% sure here is where the error lies
            var correctionMatrix = new SharpDX.Matrix(
                1, 1, 1, 1,
                1, 1, 1, 1,
                1, 1, 1, -1,
                1, 1, 1, 1);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    viewMatrix[i, j] = viewMatrix[i, j] * correctionMatrix[i, j];
                }
            }
            viewMatrix.Transpose();
            return viewMatrix;
        }

projection matrix code, again, likely something is backwards or flipped in here:

Code:

private SharpDX.Matrix OpenCVTOWPFProjectionMatrix(Size imgSize)
        {
            var camMtx = this.CameraCalibration.CameraMatrix;
            var cameraModel = this.CameraCalibration.GetSensorInfo(imgSize);
            var near = 0.00125f;
            var far = 10000;
            var fx = (float)camMtx.at(0, 0);
            var fy = (float)camMtx.at(1, 1);
            var cx = (float)camMtx.at(0, 2);
            var cy = (float)camMtx.at(1, 2);
            var width = (float)imgSize.Width;
            var height = (float)imgSize.Height;
            var f = (float)(0.5d * height * Math.Tan(fy / 2));
            var m = new SharpDX.Matrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); ;
            m[0, 0] = 2.0f * fx / width;
            m[0, 1] = 0.0f;
            m[0, 2] = 0.0f;
            m[0, 3] = 0.0f;
            m[1, 0] = 0.0f;
            m[1, 1] = -2.0f * fy / height;
            m[1, 2] = 0.0f;
            m[1, 3] = 0.0f;
            m[2, 0] = 1.0f - 2.0f * cx / width;
            m[2, 1] = (2.0f * (cy / height)) - 1.0f;
            m[2, 2] = (far + near) / (near - far);
            m[2, 3] = -1.0f;
            m[3, 0] = 0.0f;
            m[3, 1] = 0.0f;
            m[3, 2] = 2.0f * far * near / (near - far);
            m[3, 3] = 0.0f;
            return m;
        }

Code to convert to WPF matrix and back. Too many formats of Matrix in this project - maybe here I am getting something wrong?

Code:

        private SharpDX.Matrix3x3 Mat3x3ToSharpDXMatrix(Mat r)
        {
            SharpDX.Matrix3x3 m = new SharpDX.Matrix3x3();
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    m[i, j] = (float)r.at(j, i);
                }
            }
            return m;
        }
        private SharpDX.Matrix3x3 Mat3x1ToSharpDXVector(Mat r)
        {
            SharpDX.Matrix3x3 m = new SharpDX.Matrix3x3();
            m.M11 = (float)r.at<double>(0);
            m.M12 = (float)r.at<double>(1);
            m.M13 = (float)r.at<double>(2);
            return m;
        }
private Matrix3D SharpToWPF(SharpDX.Matrix src)
        {
            var m = new Matrix3D();
            m.M11 = src.M11;
            m.M12 = src.M12;
            m.M13 = src.M13;
            m.M14 = src.M14;
            m.M21 = src.M21;
            m.M22 = src.M22;
            m.M23 = src.M23;
            m.M24 = src.M24;
            m.M31 = src.M31;
            m.M32 = src.M32;
            m.M33 = src.M33;
            m.M34 = src.M34;
            m.OffsetX = src.M41;
            m.OffsetY = src.M42;
            m.OffsetZ = src.M43;
            m.M44 = src.M44;
            return m;
        }