两个 AR 标记之间的相对平移矩阵

Question

我现在正在尝试编写一个简单的 AR 应用程序，特别是我正在尝试计算两个方形标记之间的翻译，类似于他们在底部的网站上所做的操作： hitl.washington.edu/artoolkit/documentation/tutorialcamera.htm" rel="nofollow noreferrer">Artoolkit

我使用的标记尺寸相同 (100mm x 100mm)，并且我的图像没有相机失真（我目前正在使用测试图像）。

目前我的流程遵循以下步骤：

1. 从图像中检测标记并提取角点。
2. 给定标记角，运行共面 POSIT。
3. 给定 2 个标记变换（A 和 B），计算 Inverse(A) * B
4. 分解以获取标记 B 相对于标记 A 的 x、y 和 z 平移

。问题是我的结果似乎完全不对劲。下面是程序的屏幕：

橙色方块代表角的位置，我已经检查过这些并且它们是正确的。

我已经使用 XNA 覆盖了轴。 我正在使用 AForge.net 进行共面 POSIT。 图像尺寸为 640x480。 共面位置知道标记为 100mm，焦距设置为 640。

我在 c# 中运行执行此操作的确切代码：

    private void computePOSIT()
    {

        matrixes.Clear();
        AForge.Math.Matrix3x3 matrix;
        AForge.Math.Vector3 trans;
        foreach (Marker m in markers)
        {

            AForge.Point[] points = new AForge.Point[4]; 

            for (int i = 0; i < 4; i++)
            {
                points[i] = 
                    new AForge.Point(m.corners[i].X-320 ,240 - m.corners[i].Y);
            }

            posit.EstimatePose(points, out matrix, out trans);

            float yaw, pitch, roll;

            matrix.ExtractYawPitchRoll(out yaw, out pitch, out roll);

            Matrix rotation = 
                Matrix.CreateFromYawPitchRoll(-yaw, -pitch, roll);

            Matrix translation = 
                Matrix.CreateTranslation(new Vector3(trans.X, trans.Y, -trans.Z));

            Matrix complete = rotation * translation;
            List<Matrix> all = new List<Matrix>();
            all.Add(rotation);
            all.Add(translation);
            all.Add(complete);
            matrixes.Add(all);

        }

        Matrix res = Matrix.Invert(matrixes[0][4]) * matrixes[1][5];
        Vector3 scaleR;
        Vector3 translationR;
        Quaternion rotationR;
        res.Decompose(out scaleR, out rotationR, out translationR);

    }

结果矩阵显示：

TranslationR: X:-402.2295 Y:191.7873 Z:-135.3166} 旋转R：{X：0.007288148 Y：-0.4478231 Z：-0.5093549 W：0.734819} scaleR：1,1,1

更新：我已经修复了 POSIT 问题并且轴现在正确对齐，但是平移问题仍然存在。我的数学计算正确吗？