Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I succeed to solve it by myself. If it will help to any one, heres the code:

Point3f calc3DPointOutOf2DwithYknown(double u, double v, float worldY, double fx, double fy, double cx, double cy, Mat tvec, Mat rotMat)
    Point3f tmpPoint;

    float r1 =<double>(0,0);
    float r2 =<double>(0,1);
    float r3 =<double>(0,2);

    float r4 =<double>(1,0);
    float r5 =<double>(1,1);
    float r6 =<double>(1,2);

    float r7 =<double>(2,0);
    float r8 =<double>(2,1);
    float r9 =<double>(2,2);

    float t1 =<double>(0,0);
    float t2 =<double>(1,0);
    float t3 =<double>(2,0);

    float xt = (u/fx) - (cx/fx);
    float yt = (v/fy) - (cy/fy);

    float K1 = xt*r8*worldY + xt*t3 - r2*worldY - t1;
    float K2 = xt*r9 - r3;
    float K3 = r1 - xt*r7;

    float worldZ = (yt*r7*K1 + yt*K3*r8*worldY + yt*K3*t3 - r4*K1 - K3*r5*worldY - K3*t2)/
                    (r4*K2 + K3*r6 - yt*r7*K2 - yt*K3*r9);

    float worldX = (K1 + worldZ*K2)/K3;

    tmpPoint = Point3f(worldX, worldY, worldZ);

    return tmpPoint;