CODEWITHSUNDEEP
pythonnumpymatrix
Golgii
Golgii

Reputation: 61

How to solve an equation with variables in a matrix in Python?

i'm coding in Pyhon, and I'm working on stereo-correlation. I want to resolve this equation : m = K.T.M

m,K,M are know.

where :

M is the homogeneous coordinate of a point in the cartesian coordinate system "world"

M=np.array([X,Y,Z,1])

K is my intrinsic matrix for the left camera

K=np.matrix([ [fx,  0, cx, 0],
              [ 0, fy, cy, 0], 
              [ 0,  0,  1, 0]])

m its the M point views by the left camera

m=np.array([x,y,1])

and T is the transformation to pass to the "world" coordinate system to the left camera coordinate system.

T= np.matrix([[x00, x01, x02, Tx],
              [x10, x11, x12, Ty], 
              [x20, x21, x22, Tz], 
              [0  , 0  , 0  , 1 ]])

So I want to solve this equation to find T but it's impossible to create a matrice without give values to variables.

someone have a solution?

Thanks best regards

Upvotes: 6

Views: 7659

Answers (1)

Dietrich
Dietrich

Reputation: 5531

If you want a generic solution, you can use Sympy, which allows you to work with symbolic expression. In the following code the expression K.T.M = m is reformulated to a standard linear equation HH.xx = mm, where xx is the vector with the unknowns extracted from T:

from IPython.display import display
import sympy as sy

sy.init_printing()  # LaTeX like pretty printing for IPython

# declaring symbolic variables:
x, y, X, Y, Z, fx, fy, cx, cy = sy.symbols("x y X Y Z f_x f_y c_x c_y", real=True)
x00, x01, x02, x10, x11 = sy.symbols("x00, x01, x02, x10, x11", real=True)
x12, x20, x21, x22 = sy.symbols("x12, x20, x21, x22", real=True)
Tx, Ty, Tz = sy.symbols(" T_x T_y T_z", real=True)

# Building matrices and vectors:
M = sy.Matrix([X, Y, Z, 1])
m = sy.Matrix([x, y, 1])
K = sy.Matrix([[fx,  0, cx, 0],
               [0,  fy, cy, 0],
               [0,   0,  0, 1]])
T = sy.Matrix([[x00, x01, x02, Tx],
               [x10, x11, x12, Ty],
               [x20, x21, x22, Tz],
               [0,     0,   0,  1]])

print("KTM = K.T.M = ")
KTM = sy.simplify(K*T*M)
display(KTM)

print("Vector of Unkowns xx.T = ")
xx = sy.Matrix(list(T.atoms(sy.Symbol)))
display(xx.T)
print("For equation HH.xx = mm, HH = ")
HH = KTM[:2, :].jacobian(xx)  # calculate the derivative for each unknown
display(HH)

As @Sven-Marnach already noted, there are not enough equations for a unique solution. Since the last row of the vector KTM and of m is 1, there are only two equations for twelve variables.

If you have multiple pixelsto evaluate, i.e., multiple pairs of (m, M), you can use Numpy's Least Squares Solver to find a solution.

Upvotes: 2

Related Questions