pycv/Test_MotionAlgorithms.py at 7e48d332f1d7ccfc90720843f66450d58dea7903 · jwlim/pycv · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
'''
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()