NB 1: I find the notation foo2bar inconvenient because itās opposite to how matrices are written and how they compose. I would recommend
^\text{output frame} T_\text{input frame} = ~^\text{bar} T_\text{foo}
or T_bar_foo
because that composes like so:
^\text{baz} T_\text{foo} = ^\text{baz} T_\text{bar} ~\cdot~ ^\text{bar} T_\text{foo}
or:
T_baz_foo = T_baz_bar @ T_bar_foo
# @ being numpy's matrix multiplication operator
and if you need to invert a matrix, you can use np.linalg.inv (example assuming two aruco markers and their poses T_cam_marker, and a relative pose: pose of m1 relative to m2ās frame):
\begin{align*}
^{m_2} T_{m_1} &= ~^{m_2} T_C &\cdot ~ ^C T_{m_1} \\
^{m_2} T_{m_1} &= (^C T_{m_2})^{-1} &\cdot ~ ^C T_{m_1} \\
\end{align*}
(notice how the frames match up like dominos, and you can consider them to ācancelā)
the ways to read this are:
- in the
baz frame: pose of foo
- transformation that moves into
baz frame from foo frame
T or M, or A, all the same.
NB 2: chuck rvec and tvec. they are nearly impossible to calculate with. there is a numerical argument to be made for them but you arenāt trying to write obfuscated code, you are trying to write code you can understand tomorrow, in a week, in a month, in a year.
you need a convenience function that takes (rvec, tvec) and spits out a 4x4 matrix. those are trivial to work with.
def matrix_from_rtvec(rvec, tvec):
(R, jac) = cv.Rodrigues(rvec) # ignore the jacobian
M = np.eye(4)
M[0:3, 0:3] = R
M[0:3, 3] = tvec.squeeze() # 1-D vector, row vector, column vector, whatever
return M
if anything needs rvec and tvec, make another convenience function that decomposes a 4x4 matrix. both operations involve cv.Rodrigues
def rtvec_from_matrix(M):
(rvec, jac) = cv.Rodrigues(M[0:3, 0:3]) # ignore the jacobian
tvec = M[0:3, 3]
assert M[3] == [0,0,0,1], M # sanity check
return (rvec, tvec)
NB 3: dumping some more convenience functions
def vec(vec):
return np.asarray(vec)
def normalize(vec):
return np.asarray(vec) / np.linalg.norm(vec)
def translate(dx=0, dy=0, dz=0):
M = np.eye(4)
M[0:3, 3] = (dx, dy, dz)
return M
def rotate(axis, angle=None):
if angle is None:
rvec = vec(axis)
else:
rvec = normalize(axis) * angle
(R, jac) = cv.Rodrigues(rvec)
M = np.eye(4)
M[0:3, 0:3] = R
return M
def scale(s=1, sx=1, sy=1, sz=1):
M = np.diag([s*sx, s*sy, s*sz, 1])
return M
def apply_to_rowvec(T, data):
(n,k) = data.shape
if k == 4:
pass
elif k == 3:
data = np.hstack([data, np.ones((n,1))])
else:
assert False, k
data = data @ T.T # yes that's weird. one could transpose the data just as well. makes no difference really
# TODO: check that the 4th values are still all 1, or else we have a 4D projective transformation, which is bad in this instance
return data[:,:k]
