Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

The formulas look correct for me. The corresponding equations are given below.

The transformation that allows converting a 3D point expressed in the frame 0 to the frame 1 is: Eq1-1

Eq1-2

The transformation that allows converting a 3D point expressed in the frame 1 to the frame 2 is: Eq2

The transformation that allows converting a 3D point expressed in the frame 0 to the frame 2 is then: Eq3 Eq4

Note that as rvec is a Rodriges rotation vector, rodrigues(rvec) gives the corresponding rotation matrix R.