'''

2013/12/06, kameda[at]iit.tsukuba.ac.jp

R0,R1,TT = cv2.recoverPose(E)

seems to return normalized TT (such that |TT| = 1.0). Why?

'''

import math

import numpy as np

import cv2

import pickle

# Suppose T = [11,22,33] and Rx = 45 degree (Ry = Rz = 0)

targetT    = np.array([11,22,33], np.float64)

targetR, j = cv2.Rodrigues(np.array([math.radians(45),0,0], np.float64))

print "--- answer is ---"

print targetT

print targetR

print 

# skew-symmetric matrix

skewT = np.roll(np.roll(np.diag(targetT.flatten()), 1, 1), -1, 0)

skewT = skewT - skewT.T

# E = skewT targetR

quizE = np.dot(skewT, targetR)

f = open('z_quizE.param', 'w')

pickle.dump(quizE, f)

f.close()

# recoverPose() actually just calls decomposeEssentialMat() to obtain 2x2 candidates

# candidates are : R0,T  R0,-T  R1,T  R1,-T

tryR0, tryR1, tryT = cv2.decomposeEssentialMat(quizE)

print "--- cv2.decomposeEssentialMat() tells:"

print "R0"

print tryR0

print "R1 ... right choice for [11,22,33],Rx=45[deg]"

print tryR1

print "T ... why normalized?"

print tryT

print "|tryT| = ", cv2.norm(tryT, cv2.NORM_L2)

print

# Then estimate T by E targetR^-1 (E targetR^-1 = skewT targetR targetR^-1 = skewT)

# As R1 is rotation (orthogonal) matrix, R^-1 = R.T

yetTmat = np.dot(quizE, tryR1.T)

yetT = [yetTmat[2,1], yetTmat[0,2], yetTmat[1,0]]

print "T by hack ... same as answerT"

print yetT

#