I have two frame transformations, one from frame 0 to frame 1, and one from frame 1 to frame 2, and I would like to concatenate these into the transformation from frame 0 to frame 2. I'm computing the pose of a moving camera, and the first transformation (0-1) represent the previous pose of the camera (transformation from its initial pose) and the 1-2 transformation is the newest change of pose, represented by tvec and rvec gotten from solvePnPRansac.
However, as I cannot just try out different inputs in my code and see if the output seems correct, since my system currently consists of lots of noise, I would like to have the math check out before I implement it into my application. But while I try to use the formulas given in the documentation with different rvecs and tvecs, I can't get the outputs (rvec3/tvec3) I want. These are the formulas:
I've tried with the following rvecs/tvecs:
- rvec1: Rotation from frame 1 to frame 0
- tvec1: vector from the origin of frame 0 to the origin of frame 1, given in frame 0 coordinates
- rvec2: Rotation from frame 2 to frame 1
- tvec2: vector from the origin of frame 1 to the origin of frame 2, given in frame 1 coordinates
I want to end up with:
- rvec3: Rotation from frame 0 to frame 2 (or inversed, doesn't matter)
- tvec3: vector from frame 0 to frame 2 given in frame 0 coordinates (or negated)
However, with these vectors, I can't get the formula in the documentation to make sense. The rvec3-formula makes sense, and with rvec1/rvec2 I get rvec3=(rvec2*rvec1)⁻¹ to equal the rotation from frame 2 to frame 0. However, the computation of tvec3 doesn't add up:
The formula says tvec3 = rvec2 * tvec1 + tvec2, but rvec2 * tvec1 doesn't make sense with my vectors. I mean, it says to rotate a vector given in frame 0 coordinates from frame 2 to frame 1. A vector given in frame 0 coordinates need to be multiplied with a rotation matrix representing the rotation from frame 0 to some other frame, but this is not the case here. And it haven't made sense with any other vectors I've tried as well, for that matter.
Someone that could help me with these calculcations? Thanks!