recoverPose_debug00.py

script to check the function behavior - Yoshinari Kameda, 2013-12-06 06:24 pm

Download (1.3 kB)

 
1
2
'''
3
2013/12/06, kameda[at]iit.tsukuba.ac.jp
4

5
R0,R1,TT = cv2.recoverPose(E)
6
seems to return normalized TT (such that |TT| = 1.0). Why?
7
'''
8
9
import math
10
import numpy as np
11
import cv2
12
import pickle
13
14
# Suppose T = [11,22,33] and Rx = 45 degree (Ry = Rz = 0)
15
targetT    = np.array([11,22,33], np.float64)
16
targetR, j = cv2.Rodrigues(np.array([math.radians(45),0,0], np.float64))
17
print "--- answer is ---"
18
print targetT
19
print targetR
20
print 
21
22
# skew-symmetric matrix
23
skewT = np.roll(np.roll(np.diag(targetT.flatten()), 1, 1), -1, 0)
24
skewT = skewT - skewT.T
25
26
# E = skewT targetR
27
quizE = np.dot(skewT, targetR)
28
f = open('z_quizE.param', 'w')
29
pickle.dump(quizE, f)
30
f.close()
31
32
# recoverPose() actually just calls decomposeEssentialMat() to obtain 2x2 candidates
33
# candidates are : R0,T  R0,-T  R1,T  R1,-T
34
tryR0, tryR1, tryT = cv2.decomposeEssentialMat(quizE)
35
36
print "--- cv2.decomposeEssentialMat() tells:"
37
print "R0"
38
print tryR0
39
print "R1 ... right choice for [11,22,33],Rx=45[deg]"
40
print tryR1
41
print "T ... why normalized?"
42
print tryT
43
print "|tryT| = ", cv2.norm(tryT, cv2.NORM_L2)
44
print
45
46
# Then estimate T by E targetR^-1 (E targetR^-1 = skewT targetR targetR^-1 = skewT)
47
# As R1 is rotation (orthogonal) matrix, R^-1 = R.T
48
yetTmat = np.dot(quizE, tryR1.T)
49
yetT = [yetTmat[2,1], yetTmat[0,2], yetTmat[1,0]]
50
print "T by hack ... same as answerT"
51
print yetT
52
53
54
55
#
56