Pose estimation of 2nd camera of a calibrated stereo rig, given 1st camera pose

2
votes

Using opencv I've calibrated a pair of cameras in stereo. This results in a Rotation and Translation between the two cameras.

I take an image using the 1st camera (left) and use solvePnp to calculate the pose (rvec and tvec) of the left camera relative to a part.

My question is how to I calculate the pose of the right camera using the pose of the left camera, and the given R & T values from the stereo calibration?

Thanks.

1
opencv3dcomputer-vision

1 Answers

3
votes

The question is a bit unclear, but I think that you would like to find the pose of the right camera with respect to the part. If this is the case, the easiest way to proceed is as follows:

  • Let Q_pl be the pose of the left camera with respect to the part. This is represented by a 4x4 matrix whose first 3 rows and columns are the rotation matrix R_pl of the part w.r.t. the left camera (your rvec), the bottom row is [0, 0, 0, 1], and top 3 rows of of the 4 column are the component of the translation vector t_pl from the camera to the part (your tvec). To clarify, you write Q_pl by writing, ordinately, the component in camera frame of the x, y and z axis unit vectors of the part (as seen from the camera) as column vectors, followed by the vector from the camera to the part, and then write [0, 0, 0, 1] as the fourth row.
  • Likewise, let Q_lr be the pose (obtained from the calibration) of the left camera with respect to the right one. You write this 4x4 matrix laying the components of the x, y, z unit vectors of the left camera (as see from the right one) as column vectors, followed by the translation vector from the right camera to the left one, and then write [0, 0, 0 1] as the fourth row.

  • The pose of the part w.r.t. the right camera is then simply the product of the two matrices:

    Q_pr = Q_pl * Q_lr

Note that the order of the factors matters. This expression simply says that to go from the right camera to the part, you can first go from the right camera to the left one, and then from there to the part. Again, the rotation part of this transformation is in the top 3 rows and 3 columns of Q_pr, and the translation vector from the right camera to the part is in the 4th column.

Note that your calibration procedure may have given you Q_rl, rather than Q_lr. If this is the case, you just invert it. The inverse is very easy to compute: 

R_lr = transpose(R_rl)
t_lr = -(R_lr * t_rl)

Likewise, you'll invert Q_lp if that's what your solvePnP produced, rather than Q_pl.