c# – 尝试精确测量2D图像的3D距离

一个例子如下所示：

代码是用C#编写的,我使用的是XNA.我正在使用AForge.net作为CoPlanar POSIT
我为了计算距离而采取的步骤：

1.在屏幕上标记角落.角以2D矢量形式表示,图像中心为(0,0).在Y方向上向上是正向,在X方向上向右是正向.

2.使用AForge.net Co-Planar POSIT算法获取每个标记的姿势：

float focalLength = 640; //Needed for POSIT
    float halfCornerSize = 50; //Represents 1/2 an edge i.e. 50mm
    AVector[] modelPoints = new AVector3[]
    {
         new AVector3( -halfCornerSize,halfCornerSize ),new AVector3(  halfCornerSize,-halfCornerSize ),new AVector3( -halfCornerSize,};
    CoplanarPosit coPosit = new CoplanarPosit(modelPoints,focalLength);
    coPosit.EstimatePose(cornersToEstimate,out marker1Rot,out marker1Trans);

3.转换为XNA旋转/平移矩阵(AForge使用OpenGL矩阵形式)：

float yaw,pitch,roll;
    marker1Rot.ExtractYawPitchRoll(out yaw,out pitch,out roll);

    Matrix xnaRot = Matrix.CreateFromYawPitchRoll(-yaw,-pitch,roll);
    Matrix xnaTranslation = Matrix.CreateTranslation(marker1Trans.X,marker1Trans.Y,-marker1Trans.Z);
    Matrix transform = xnaRot * xnaTranslation;

4.找到角落的3D坐标：

//Model corner points
    cornerModel = new Vector3[]
    {
        new Vector3(halfCornerSize,-halfCornerSize),new Vector3(-halfCornerSize,new Vector3(halfCornerSize,halfCornerSize),halfCornerSize)
    };

    Matrix markerTransform =  Matrix.CreateTranslation(cornerModel[i].X,cornerModel[i].Y,cornerModel[i].Z);
    cornerPositions3d1[i] = (markerTransform * transform).Translation;

    //DEBUG: project corner onto screen - represented by brown dots
    Vector3 t3 = viewPort.Project(markerTransform.Translation,projectionMatrix,viewMatrix,transform);
    cornersProjected1[i].X = t3.X; cornersProjected1[i].Y = t3.Y;

5.查看标记上两个角之间的3D距离,这表示100mm.找到将此3D距离转换为100毫米所需的缩放系数. (我实际得到平均比例因子)：

for (int i = 0; i < 4; i++)
    {
        //Distance scale;
        distanceScale1 += (halfCornerSize * 2) / Vector3.Distance(cornerPositions3d1[i],cornerPositions3d1[(i + 1) % 4]);
    }
    distanceScale1 /= 4;

6.最后,我找到相关角之间的3D距离并乘以比例因子得到距离mm：

for(int i = 0; i < 4; i++)
    {
       distance[i] = Vector3.Distance(cornerPositions3d1[i],cornerPositions3d2[i]) * scalingFactor;
    }

获得的距离永远不会真正正确.我使用了切菜板,因为它可以让我轻松计算距离应该是多少.上面的图像计算出角1(红色到紫色)的距离为147mm(预期的150mm).下图显示188毫米(预计200毫米).

同样令人担忧的是,当测量在同一标记上共享边缘的标记角之间的距离时,获得的3D距离永远不会相同.我注意到的另一件事是棕色点似乎永远不会与彩色圆点完全匹配.彩色点是用作CoPlanar位置输入的坐标.褐色点是通过POSIT计算的标记中心的计算位置.

有谁知道这里可能有什么问题？我正在拔出我的头发试图找出它.代码应该很简单,我不认为我在代码中犯了任何明显的错误.我不擅长数学,所以请指出我的基本数学可能也是错的……