Monthly Archives: August 2012

cv2.triangulatePoints

Posted by Wujie of Dasheshire on 
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]

cv2.correctMatches

Posted by Wujie of Dasheshire on

This function requires two 1xN array of points, how does that work? Maybe 1xNx2 would be more clear. You need to convert the point array to the shape (1,N,2). If your point array looks like mine:

a3xN = \
array([[35, 59, 32, 92, 71, 12,  4,  8, 28, 19],
       [ 9,  4, 36, 80, 85, 47, 60, 83, 38, 68],
       [ 1,  1,  1,  1,  1,  1,  1,  1,  1,  1]])

You need to change it to look like this:

a1xNx2 = \
array([[[35,  9],
        [59,  4],
        [32, 36],
        [92, 80],
        [71, 85],
        [12, 47],
        [ 4, 60],
        [ 8, 83],
        [28, 38],
        [19, 68]]])

Try this:

a1xNx2 = r_[[a3xN[:2].T]]
#or
a1xNx2 = array([a3xN[:2].T])

The output is in the same form. Use output[0].T to convert to a 2xN array. If you don’t need the original a3xN array, then copy the output 2xN into the first two rows of a3xN (a3xN[:2] = output[0].T). Or convert to homogeneous coordinates like this:

outa3xN = append(outa[0].T,ones((1,len(outa[0]))),0)
#or
outa3xN = vstack((outa[0].T,ones(len(outa[0]))))

Complete example:

# A fundamental or essential matrix
F3x3 = array([[ 0.0345, -0.2073, -0.0673],
              [-0.1403, -0.0003,  0.6889],
              [ 0.0604, -0.6725,  0.0349]])
# Homogeneous 2D point arrays
a3xN = array([[35, 59, 32, 92, 71, 12,  4,  8],
              [ 9,  4, 36, 80, 85, 47, 60, 83],
              [ 1,  1,  1,  1,  1,  1,  1,  1]], float)
# Pass an integer array and get an integer array back
b3xN = array([[95, 24, 86, 26,  4, 85, 68, 66],
              [35, 44, 91, 10, 14, 41, 93, 34],
              [ 1,  1,  1,  1,  1,  1,  1,  1]], int)
# The cv2 method
outa, outb = cv2.correctMatches( F3x3, r_[[a3xN[:2].T]], r_[[b3xN[:2].T]] )
# Two ways of making the output homogeneous
outa3xN = append(outa[0].T,ones((1,len(outa[0]))),0)
outb3xN = vstack((outb[0].T,ones(len(outb[0]),int)))
print outa3xN
print outb3xN

[[ 34.37  52.71  12.63  94.56  70.1    5.   -24.92 -14.35]
 [ -1.31 -15.19  -3.31  77.    85.69   0.48  35.01  77.4 ]
 [  1.     1.     1.     1.     1.     1.     1.     1.  ]]
[[ 96  36  88   6  -8  85  49   4]
 [ 33  28  89 -10   7  41 103  44]
 [  1   1   1   1   1   1   1   1]]

Note that if you pass an integer array, then the function returns an int array, so you may need to convert to float first.

Epipole and Epipolar Lines

Posted by Wujie of Dasheshire on

Epipole from essential matrix:
e1c2 is the position of camera 2 in image 1 coordinates. (Plot it in image 1)

def epipoleSVD(M):
    V = cv2.SVDecomp(M)[2]
    return V[-1]/V[-1,-1]
e1c2 = epipoleSVD(E)
e2c1 = epipoleSVD(E.T)

Note: This cv2 method returns three values, [S, U, V], but only V is needed to find the epipole. The epipole is in form [x,y,1].

Epipolar lines from essential matrix:
elines1 (a 3xN array) are drawn on image 1 and will converge at e1c2 (from above). x1 and x2 are normalized 3xN arrays of corresponding points in image 1 and image 2. (A line vector contains the values [a,b,c] in ax+by+c=0)

elines2 = dot(E, x1)
elines1 = dot(E.T, x2) #or dot(x2.T, E).T

To plot these lines, try the following:

def lines2points(lines, epipole):
    xmax = ones(lines.shape[1])*epipole[0]
    a,c = lines[0]/lines[1], lines[2]/lines[1]
    x = array([-xmax,xmax])
    y = array([r_[-a*Lx[0] - c], r_[-a*Lx[1] - c]])
    return x, y
Lx,Ly = lines2points( dot(E, x1), epipoleSVD(E.T) )
plt.plot(Lx, Ly, 'r')

Note: This will overlay epipolar lines derived from image 1 on corresponding points in image 2 that are between +/- epipole x-coordinate.

Essential Matrix

Posted by Wujie of Dasheshire on

From point correspondences:
x1 and x2 are 3xN array of normalized points from images 1 and 2.

E = cv2.findFundamentalMat(x1[:2].T, x2[:2].T)[0]

Note: This cv2 method requires points to be in Nx2 format.
The ending [0] returns the matrix. The 2nd returned value [1] is a mask array of inliers (=1) and outliers (=0)

From transformation/projection matrix:
P is a 3×4 (or 4×4) camera projection matrix for image 2.
(World origin at image 1 camera)

R,t = cv2.decomposeProjectionMatrix(P[:3])[1:3]
skewt = roll(roll(diag(t.flat[:3]/t[3]), 1, 1), -1, 0)
skewt -= skewt.T
E = dot(R, skewt)

Note: This method requires a 3×4 array and returns seven items, [1] and [2] are the rotation matrix and translation vector. Also, the t-vec has length 4 and must be normalized by the last value (t[:3]/t[3])
roll, diag, .flat, dot are all NumPy functions.