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

目前我正在尝试编写一个简单的AR应用程序，具体来说，我正在尝试计算两个方形标记之间的转换，类似于他们在该网站底部所做的转换：

我使用的标记大小相同（100mm x 100mm），并且我的图像没有相机失真（目前我使用的是测试图像）

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

从图像中检测标记并提取角点

给定标记角，运行共面位置

给定2个标记变换（A和B），计算逆（A）*B

分解以获取标记B相对于标记A的x、y和z平移

问题是我的结果似乎完全不正确。以下是该程序的屏幕：

橙色的正方形代表角的位置，我已经检查了这些，它们是正确的

我已经使用XNA在轴上进行了叠加。 我正在使用forge.net进行共面定位。 图像为640x480。 共面posit知道标记为100mm，焦距设置为640

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

private void computePOSIT（）
{
矩阵；
a forme.Math.Matrix3x3矩阵；
a forge.Math.Vector3 trans；
foreach（标记中的标记m）
{
a原点[]点=新原点[4]；
对于（int i=0；i<4；i++）
{
点[i]=
新的角点（m.corners[i].X-320240-m.corners[i].Y）；
}
位置估计位置（点、外矩阵、外变换）；
浮动偏航、俯仰、横摇；
矩阵提取横摇（外偏航、外俯仰、外横摇）；
矩阵旋转=
矩阵.CreateFromYawPitchRoll（-偏航、俯仰、横摇）；
矩阵转换=
矩阵.CreateTranslation（新向量3（trans.X，trans.Y，-trans.Z））；
矩阵完成=旋转*平移；
List all=新列表（）；
全部。添加（旋转）；
全部。添加（翻译）；
全部。添加（完成）；
矩阵。添加（全部）；
}
矩阵res=矩阵.Invert（矩阵[0][4]）*矩阵[1][5]；
矢量3定标器；
向量3平移；
四元数旋转；
res.Decompose（向外缩放、向外旋转、向外平移）；
}

所得矩阵表明：

平移R:X:-402.2295 Y:191.7873 Z:-135.3166} 旋转率：{X:0.007288148 Y:-0.4478231 Z:-0.5093549 W:0.734819} 定标器：1,1,1

---

更新：我已经修复了位置问题，轴现在正确对齐，但是平移问题仍然存在。我的数学计算出相对平移是否正确？

这似乎是错误的透视校正，导致实际世界中的角点位置无效，从而导致偏移，甚至可能是算法中输入点的轻微方向失真

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

    }