Homework Question 5.3

MotionAlgorithms.py

@@ -0,0 +1,304 @@
+'''
+Created on May 26, 2015
+
+@author: Kena Alexander
+@contact: ID# 2015109826  email: kenaxle@hotmail.com
+
+'''
+
+import numpy as np
+import random
+import cv2
+from numpy import dtype
+
+
+# 8-Point Algorithm
+
+# first lets assume that we have the corresponding points supplied in matrices
+# the matrices will be M:X and M1:X' respectively. 
+# we will use a 8 x 9 matrix to hold A
+
+
+#-------------------------8pt-----------------------------------------
+#fill the A matrix row by row
+def eightPoint(m, m1):
+    a_8 = np.empty([8,9]) 
+    for i in range(0, 8):
+        a_8[i] =[m[i - 1, 0] * m1[i - 1, 0], m[i - 1, 1] * m1[i - 1, 0], m1[i - 1, 0],
+                 m[i - 1, 0] * m1[i - 1, 1], m[i - 1, 1] * m1[i - 1, 1], m1[i - 1, 1],
+                 m[i - 1, 0], m[i - 1, 1], 1] 
+
+    # solve using SVD
+    u , s, v = np.linalg.svd(a_8, full_matrices=True)   
+
+    # get the index of the minimum value in s
+    col = np.where(s == np.min(s))[0][0]
+
+    # get the solution which is the column of v related to min s
+    # reshape the array to a 3x3 matrix
+    f1 = v[:, col].reshape(3, 3)
+
+    # solve again
+    u , s , v = np.linalg.svd(f1)
+
+    # get the minimum value and the column. this should be 0
+    # set it to 0 if it is not (enforce constraint)
+    sSmallest = np.min(s)
+    index = np.where(s == np.min(s))[0][0]
+
+    if (sSmallest != 0):
+        s[index] = 0
+
+    # fundamental matrix is USVTrans
+    fMatrix = np.dot(np.dot(u, np.diag(s, 0)), np.transpose(v))
+    return fMatrix
+
+#---------------------------------------------------------------------
+
+
+#------------------------7pt------------------------------------------  
+# fill the A matrix row by row
+def sevenPoint(m, m1):
+    a_7 = np.empty([7,9])
+    for i in range(0,7):
+        a_7[i] = [m[i-1,0]*m1[i-1,0], m[i-1,1]*m1[i-1,0], m1[i-1,0], 
+                m[i-1,0]*m1[i-1,1], m[i-1,1]*m1[i-1,1], m1[i-1,1], 
+                m[i-1,0], m[i-1,1], 1] 
+
+    #solve using SVD
+    u , s, v = np.linalg.svd(a_7, full_matrices = True)   
+
+    #get the second to last and last columns and reshape to 3x3 matrices
+    ef0 = v[:,7].reshape(3,3)
+    ef1 = v[:,8].reshape(3,3)
+    ef2 = ef1 - ef0
+
+    a1 = ef1[0,0]; a2 = ef2[0,0]
+    b1 = ef1[0,1]; b2 = ef2[0,1]
+    c1 = ef1[0,2]; c2 = ef2[0,2] 
+    d1 = ef1[1,0]; d2 = ef2[1,0]
+    e1 = ef1[1,1]; e2 = ef2[1,1]
+    f1 = ef1[1,2]; f2 = ef2[1,2]
+    g1 = ef1[2,0]; g2 = ef2[2,0]
+    h1 = ef1[2,1]; h2 = ef2[2,1]
+    i1 = ef1[2,2]; i2 = ef2[2,2]
+
+    # determine the coefficients of the polynomial 
+    coef3 = a1*e1*i1 + b1*f1*g1 + c1*d1*h1 - c1*e1*g1 - b1*d1*i1 - a1*f1*h1
+    coef2 = a1*e2*i1 + a2*e1*i1 + a1*e1*i2 + b1*f2*g1 + b2*f1*g1 + b1*f1*g2 + c1*d2*h1 + c2*d1*h1 + c1*d1*h2 - c1*e2*g1 - c2*e1*g1 - c1*e1*g2 - b1*d2*i1 - b2*d1*i1 - b1*d1*i2 - a1*f2*h1 - a2*f1*h1 - a1*f1*h2
+    coef1 = a2*e2*i1 + a1*e2*i2 + a2*e1*i2 + b2*f2*g1 + b1*f2*g2 + b2*f1*g2 + c2*d2*h1 + c1*d2*h2 + c2*d1*h2 - c2*e2*g1 - c1*e2*g2 - c2*e1*g2 - b2*d2*i1 - b1*d2*i2 - b2*d1*i2 - a2*f2*h1 - a1*f2*h2 - a2*f1*h2        
+    coef0 = a2*e2*i2 + b2*f2*g2 + c2*d2*h2 - c2*e2*g2 - b2*d2*i2 - a2*f2*h2
+
+    #solve the polynomial
+    cubicEq = np.polynomial.Polynomial([coef0,coef1,coef2,coef3])
+    roots = cubicEq.roots()
+
+    fMatrix1 = ef1+(roots[0]*ef2)
+    fMatrix2 = ef1+(roots[1]*ef2)
+    fMatrix3 = ef1+(roots[2]*ef2)
+
+    return fMatrix1, fMatrix2, fMatrix3
+
+#--------------------------------------------------------------------- 
+
+#------------------------------P3P------------------------------------
+# assume the camera intrinsics are known fx,fy,cx,cy,s
+# also assume fx = fy and s = 0 
+# also assume we are given 3 + 1 sample points u,v,w,z
+# points are in the form u = [ux,uy]
+
+def p3p(Apts, Bpts, camInt):
+
+    fx = camInt[0,0]; fy = camInt[1,1]
+    cx = camInt[0,2]; cy = camInt[1,2]
+
+    dataPts2D = np.array([Apts[0],
+                          Apts[1],
+                          Apts[2]])
+
+    dataPts3D = np.array([Bpts[0],
+                          Bpts[1],
+                          Bpts[2]])
+
+
+    # select 2D points from the sample points above
+    ux = dataPts2D[0,0]; uy = dataPts2D[0,1]; uz = dataPts2D[0,2];
+    vx = dataPts2D[1,0]; vy = dataPts2D[1,1]; vz = dataPts2D[1,2]
+    wx = dataPts2D[2,0]; wy = dataPts2D[2,1]; wz = dataPts2D[2,2]
+
+    #select 3D points from the sample points
+    ux_3D = dataPts3D[0,0]; uy_3D = dataPts3D[0,1]; uz_3D = dataPts3D[0,2];
+    vx_3D = dataPts3D[1,0]; vy_3D = dataPts3D[1,1]; vz_3D = dataPts3D[1,2];
+    wx_3D = dataPts3D[2,0]; wy_3D = dataPts3D[2,1]; wz_3D = dataPts3D[2,2]; 
+
+    # Calculate the local ray vectors
+    uxPrime = (ux-cx)/fx; vxPrime = (vx-cx)/fx; wxPrime = (wx-cx)/fx; 
+    uyPrime = (uy-cy)/fy; vyPrime = (vy-cy)/fy; wyPrime = (wy-cy)/fy; 
+    uzPrime = uz; vzPrime = vz; wzPrime = wz; 
+
+    # Normalize using L2  
+    nu = np.sqrt(uxPrime**2 + uyPrime**2 + uzPrime**2)
+    nv = np.sqrt(vxPrime**2 + vyPrime**2 + vzPrime**2)
+    nw = np.sqrt(wxPrime**2 + wyPrime**2 + wzPrime**2)  
+
+    uxCap = uxPrime/nu; vxCap = vxPrime/nv; wxCap = wxPrime/nw; 
+    uyCap = uyPrime/nu; vyCap = vyPrime/nv; wyCap = wyPrime/nw; 
+    uzCap = uzPrime/nu; vzCap = vzPrime/nv; wzCap = wzPrime/nw; 
+
+    #Calculate the Object distances
+    Rab2 = ((vx_3D-ux_3D)*(vx_3D-ux_3D) + (vy_3D-uy_3D)*(vy_3D-uy_3D) + (vz_3D-uz_3D)*(vz_3D-uz_3D))
+    Rbc2 = ((wx_3D-vx_3D)*(wx_3D-vx_3D) + (wy_3D-vy_3D)*(wy_3D-vy_3D) + (wz_3D-vz_3D)*(wz_3D-vz_3D))
+    Rac2 = ((wx_3D-ux_3D)*(wx_3D-ux_3D) + (wy_3D-uy_3D)*(wy_3D-uy_3D) + (wz_3D-uz_3D)*(wz_3D-uz_3D))
+
+    #calculate the angles
+    cosAb = (uxCap*vxCap + uyCap*vyCap + uzCap*vzCap)
+    cosBc = (uxCap*wxCap + uyCap*wyCap + uzCap*wzCap)
+    cosAc = (vxCap*wxCap + vyCap*wyCap + vzCap*wzCap)
+
+    K1 = (Rbc2)/(Rac2)
+    K2 = (Rbc2)/(Rab2)
+
+    # calculate the coefficients for the Quartic equation
+
+    p = cosAb; q = cosBc; r = cosAc
+
+    a4 = (K1*K2 - K1 - K2)**2 - 4*K1*K2*(q**2)
+    a3 = 4*(K1*K2 - K1 - K2)*K2*(1-K1)*p + 4*K1*q*((K1*K2 + K2 - K1)*r + 2*K2*p*q)
+    a2 = (2*K2*(1-K1)*p)**2 + 2*(K1*K2 + K1 - K2)*(K1*K2 - K1 - K2) + 4*K1*((K1-K2)*(q**2) + (1-K2)*K1*(r**2) - 2*K2*(1+K1)*p*r*q)
+    a1 = 4*(K1*K2 + K1 - K2)*K2*(1-K1)*p + 4*K1*((K1*K2 - K1 + K2)*r*q + 2*K1*K2*p*(r**2))
+    a0 = (K1*K2 + K1 - K2)**2 - 4*(K1**2)*K2*(r**2)
+
+    # get the roots of the equation
+    xRtemp = np.polynomial.Polynomial([a0,a1,a2,a3,a4]).roots()
+    xRoots = np.real(xRtemp[np.isreal(xRtemp)])
+    yRoots = np.zeros(4)
+    numRoots = xRoots.shape[0]
+
+    # the solutions for x and y are stored in xRoots and yRoots respectively 
+    # calculate the solutions for PA, PB and PC
+    # there will be 4 solutions: store in an array
+
+
+    PA = np.zeros(numRoots); PB = np.zeros(numRoots); PC = np.zeros(numRoots)
+    A = np.zeros([numRoots,3]); B = np.zeros([numRoots,3]); C = np.zeros([numRoots,3]) #use a 2D array to hold the x, y and z coords of the points A, B and C
+
+    #need to use only positive real roots
+    # we obtain 4 values for a and b
+
+    for i in range(0,numRoots):
+        PA[i] = np.sqrt(Rab2) / (np.sqrt((xRoots[i]**2) - 2*xRoots[i]*p + 1))
+        PB[i] = PA[i]*xRoots[i]
+
+        m = (1-K1); mPr = 1
+        p1 = 2*(K1*r - xRoots[i]*q); pPr = 2*(-xRoots[i]*q)
+        q1 = ((xRoots[i]**2) - K1); qPr = (((xRoots[i]**2)*(1-K2))+(2*xRoots[i]*K2*p)-K2)
+
+        yRoots[i] = (pPr*q1 - p1*qPr) / (m*qPr - mPr*q1)
+
+        PC[i] = PA[i]*yRoots[i]
+
+        #calculate the 3D coordinate of the image locations
+        A[i,0] = uxCap*np.absolute(PA[i]); A[i,1] = uyCap*np.absolute(PA[i]); A[i,2] = uzCap*np.absolute(PA[i]);
+        B[i,0] = vxCap*np.absolute(PB[i]); B[i,1] = vyCap*np.absolute(PB[i]); B[i,2] = vzCap*np.absolute(PB[i]);
+        C[i,0] = wxCap*np.absolute(PC[i]); C[i,1] = wyCap*np.absolute(PC[i]); C[i,2] = wzCap*np.absolute(PC[i]);
+
+
+    # Calculate the Rt matrices using method given here http://nghiaho.com/?page_id=671
+    # this is required to translate from center origin. 
+    # u,v,w are points in the image dataset
+    # A,B,C are points in the calculated 3D dataset 
+
+    #store the results of the R and T matrices in an array
+    RTMatrices = np.empty([numRoots,2],dtype=object)
+
+    APoints = dataPts3D
+
+    centroidA = np.mean(APoints,axis=0)
+
+    N = APoints.shape[0]
+    APrime = APoints - np.tile(centroidA, (N,1))
+
+    for i in range(0,numRoots):
+        BPoints = np.array([A[i],
+                            B[i],
+                            C[i]])
+
+        centroidB = np.mean(BPoints,axis=0)
+
+        BPrime = BPoints - np.tile(centroidB, (N,1))         
+
+        H = np.transpose(APrime)*BPrime
+
+        U , S , V = np.linalg.svd(H)    
+
+        #rMatrix = np.dot(np.transpose(V),np.transpose(U))
+        rMatrix = V*np.transpose(U)
+
+        # special case
+        if np.linalg.det(rMatrix) < 0:
+            V[:,2] *= -1 
+            rMatrix = V*np.transpose(U)
+
+        tMatrix = np.dot(-rMatrix,np.transpose(centroidA)) + np.transpose(centroidB)
+
+        #store the 4 results
+        RTMatrices[i,0] = rMatrix; RTMatrices[i,1] = tMatrix
+
+    return RTMatrices , APoints, BPoints        
+
+#---------------------------------------------------------------------------------------------
+
+
+#----------------------------------p3p Ransac-------------------------------------------------
+# code is incomplete
+
+def p3pRansac(AllPts_A, AllPts_B, camInt, error, threshold, N):
+
+
+    mInliers = AllPts_A
+
+    ErrMatrix = np.full((3,3),error)
+    bestNum = 0
+    bestErr = 10000
+
+    for i in range(0,N):
+        # need unique random numbers
+        #randPts = np.zeros(3)
+        randPts = random.sample(range(AllPts_A.shape[0]),3)
+        # maybe inliers
+        Apts = np.array([[AllPts_A[randPts[0],0],AllPts_A[randPts[0],1],1],
+                         [AllPts_A[randPts[1],0],AllPts_A[randPts[1],1],1],
+                         [AllPts_A[randPts[2],0],AllPts_A[randPts[2],1],1]])
+
+        Bpts = np.array([AllPts_B[randPts[0]],
+                         AllPts_B[randPts[1]],
+                         AllPts_B[randPts[2]]])
+
+        #maybe model
+        RTMatrices, Apts1, Bpts1 = p3p(Apts, Bpts, camInt)
+
+        for i in range(0,RTMatrices.shape[0]):
+            R = RTMatrices[i,0] 
+            T = RTMatrices[i,1]
+
+            rVec = np.empty(3)
+            cv2.Rodrigues(R,rVec)
+
+            BTest = np.dot(R,np.transpose(Bpts1)) + np.tile(T,(3,1))
+            BTest2 = np.transpose(BTest)
+
+            absErr = np.abs(BTest2 - Bpts)
+            #if this is within the threshold then increment for each point
+
+
+
+        #we chould have inliers
+
+    #after finding our inliers
+
+
+
+
+#p3pRansac(AllPts_A, AllPts_B, camInt, 1.5, 100, 1000)
+
+

Test_MotionAlgorithms.py

@@ -0,0 +1,90 @@
+'''
+Created on Jun 12, 2015
+
+@author: kena
+@contact: ID# 2015109826  email: kenaxle@hotmail.com
+'''
+import random
+import numpy as np
+from MotionAlgorithms import eightPoint, sevenPoint, p3p
+
+m = np.array([[79,57],[75,214],[74,374],[181,111],[182,216],[179,322],[334,16],[334,166],[335,322],[433,67],[434,168],[435,321]])
+m1 = np.array([[175,82],[173,227],[174,374],[258,124],[258,225],[257,327],[396,10],[397,166],[397,328],[497,52],[499,162],[500,329]])
+
+camInt = np.array([[1913.71011, 0.00000,    1311.03556],
+                   [0.00000,    1909.60756, 953.81594],
+                   [0.00000,    0.00000,    1.00000]])
+
+AllPts_A = np.array([[481,  831],
+                     [520,  504],
+                     [1114, 828],
+                     [1106, 507]])
+
+AllPts_B = np.array([[0.11, 1.15, 0], 
+                     [0.11, 1.37, 0], 
+                     [0.40, 1.15, 0],
+                     [0.40, 1.37, 0]])
+
+def test_8pts():
+    randPts = random.sample(range(m.shape[0]),8)
+    lPts = np.empty([8,2])
+    rPts = np.empty([8,2])
+    for i in range(0,8):
+        lPts[i] = m[randPts[i]]
+        rPts[i] = m1[randPts[i]]
+
+    print "-------------------8pt F Matrix-------------------------"
+    print eightPoint(lPts, rPts)
+    print "--------------------------------------------------------"
+    print '\n'
+
+
+
+def test_7pts():
+    randPts = random.sample(range(m.shape[0]),7)
+    lPts = np.empty([7,2])
+    rPts = np.empty([7,2])
+    for i in range(0,7):
+        lPts[i] = m[randPts[i]]
+        rPts[i] = m1[randPts[i]]
+
+    rM1, rM2, rM3 = sevenPoint(lPts, rPts)
+
+    print "-------------------7pt Solutions------------------------"
+    print "-------------------7pt F Matrix 1-----------------------"
+    print rM1
+    print "-------------------7pt F Matrix 2-----------------------"
+    print rM2
+    print "-------------------7pt F Matrix 3-----------------------"
+    print rM3
+    print "--------------------------------------------------------"
+    print '\n'
+
+def test_p3p():
+    randPts = random.sample(range(AllPts_A.shape[0]),3)
+
+    Apts = np.array([[AllPts_A[randPts[0],0],AllPts_A[randPts[0],1],1],
+                     [AllPts_A[randPts[1],0],AllPts_A[randPts[1],1],1],
+                     [AllPts_A[randPts[2],0],AllPts_A[randPts[2],1],1]])
+
+    Bpts = np.array([AllPts_B[randPts[0]],
+                     AllPts_B[randPts[1]],
+                     AllPts_B[randPts[2]]])
+
+    RTMatrices, Apts1, Bpts1 = p3p(Apts, Bpts, camInt)
+
+    print "-------------------P3P Solutions------------------------"
+
+    for i in range(0,RTMatrices.shape[0]):
+        print "---------------------R Matrix---------------------------"
+        print RTMatrices[i,0]
+        print "---------------------T Vector---------------------------"
+        print RTMatrices[i,1]
+        print "--------------------------------------------------------"
+        print '\n'
+
+
+test_8pts()
+test_7pts()
+test_p3p()
+