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]
```