From 870065cc80e077c6e91a94a555a15d307cf8a852 Mon Sep 17 00:00:00 2001 From: Kena Alexander Date: Sun, 14 Jun 2015 19:09:17 +0900 Subject: [PATCH 1/2] Create MotionAlgorithms.py --- MotionAlgorithms.py | 304 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 304 insertions(+) create mode 100644 MotionAlgorithms.py diff --git a/MotionAlgorithms.py b/MotionAlgorithms.py new file mode 100644 index 0000000..c817269 --- /dev/null +++ b/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) + + From 7e48d332f1d7ccfc90720843f66450d58dea7903 Mon Sep 17 00:00:00 2001 From: Kena Alexander Date: Sun, 14 Jun 2015 19:10:48 +0900 Subject: [PATCH 2/2] Create Test_MotionAlgorithms.py --- Test_MotionAlgorithms.py | 90 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 Test_MotionAlgorithms.py diff --git a/Test_MotionAlgorithms.py b/Test_MotionAlgorithms.py new file mode 100644 index 0000000..7e131ee --- /dev/null +++ b/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() +