X = cv2.triangulatePoints(P1, P2, x1, x2) • P1: Camera projection from X to x1; x1 = dot(P1,X) • P2: Camera projection from X to x2; x2 = dot(P2,X) • x1: 2xN normalized points • x2: 2xN normalized points
The projection matrices have the same origin. If camera one is set as the world origin, then P1 = eye(4)[:3]. The program crashed when I tried integer point arrays.
Complete example:
# Camera projection matrices
P1 = eye(4)
P2 = array([[ 0.878, -0.01 , 0.479, -1.995],
[ 0.01 , 1. , 0.002, -0.226],
[-0.479, 0.002, 0.878, 0.615],
[ 0. , 0. , 0. , 1. ]])
# Homogeneous arrays
a3xN = array([[ 0.091, 0.167, 0.231, 0.083, 0.154],
[ 0.364, 0.333, 0.308, 0.333, 0.308],
[ 1. , 1. , 1. , 1. , 1. ]])
b3xN = array([[ 0.42 , 0.537, 0.645, 0.431, 0.538],
[ 0.389, 0.375, 0.362, 0.357, 0.345],
[ 1. , 1. , 1. , 1. , 1. ]])
# The cv2 method
X = cv2.triangulatePoints( P1[:3], P2[:3], a3xN[:2], b3xN[:2] )
# Remember to divide out the 4th row. Make it homogeneous
X /= X[3]
# Recover the origin arrays from PX
x1 = dot(P1[:3],X)
x2 = dot(P2[:3],X)
# Again, put in homogeneous form before using them
x1 /= x1[2]
x2 /= x2[2]
print 'X\n', X
print 'x1\n', x1
print 'x2\n', x2
X [[ 1.003 2.008 3.012 1.003 2.011] [ 4.012 4.009 4.017 4.029 4.019] [ 11.019 12.023 13.041 12.089 13.057] [ 1. 1. 1. 1. 1. ]] x1 [[ 0.091 0.167 0.231 0.083 0.154] [ 0.364 0.333 0.308 0.333 0.308] [ 1. 1. 1. 1. 1. ]] x2 [[ 0.42 0.537 0.645 0.431 0.538] [ 0.389 0.375 0.362 0.357 0.345] [ 1. 1. 1. 1. 1. ]]
It would be nice if the cv2 function could accept 4×4 transformation and 3xN homogeneous point matrices and also return a homogeneous array.
def triangulatePoints( P1, P2, x1, x2 ):
X = cv2.triangulatePoints( P1[:3], P2[:3], x1[:2], x2[:2] )
return X/X[3]