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:
The transformation that allows converting a 3D point expressed in the frame 1 to the frame 2 is:
The transformation that allows converting a 3D point expressed in the frame 0 to the frame 2 is then:
Note that as rvec
is a Rodriges rotation vector, rodrigues(rvec)
gives the corresponding rotation matrix R
.