I’d recommend turning everything into 4x4 transformation/pose matrices.
a computer graphics course might be helpful in understanding the math. or see what the tutorials in the opencv documentation can do for you. they’re fairly terse, not an introduction to the math.