Revision history [back]

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 = rotMat.at<double>(0,0);
    float r2 = rotMat.at<double>(0,1);
    float r3 = rotMat.at<double>(0,2);

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

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

    float t1 = tvec.at<double>(0,0);
    float t2 = tvec.at<double>(1,0);
    float t3 = tvec.at<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;
}