The coordinates of vanishing point about boxcars HOT 1 CLOSED

def getFocal(vp1, vp2, pp):
    return math.sqrt(- np.dot(vp1[0:2]-pp[0:2], vp2[0:2]-pp[0:2]))


def homogenizeAndNormalize(p):
    p = np.array(p)
    p = p.flatten()
    assert len(p) == 2 or len(p) == 3
    if len(p) == 2:
        return np.concatenate((p, [1]))
    else:
        return p/p[2]

"""
Compute camera calibration from two van points and principal point
"""
def computeCameraCalibration(_vp1, _vp2, _pp):
    vp1 = homogenizeAndNormalize(_vp1)
    vp2 = homogenizeAndNormalize(_vp2)
    pp = homogenizeAndNormalize(_pp)
    focal = getFocal(vp1, vp2, pp)
    vp1W = np.concatenate((vp1[0:2], [focal]))    
    vp2W = np.concatenate((vp2[0:2], [focal]))    
    ppW = np.concatenate((pp[0:2], [0])) 
    vp3W = np.cross(vp1W-ppW, vp2W-ppW)
    vp3 = np.concatenate((vp3W[0:2]/vp3W[2]*focal + ppW[0:2], [1]))
    vp3Direction = np.concatenate((vp3[0:2], [focal]))-ppW
    roadPlane = np.concatenate((vp3Direction/np.linalg.norm(vp3Direction), [10]))
    return vp1, vp2, vp3, pp, roadPlane, focal