Get extrinsic camera parameters from pose estimation

Hi,

I have a camera mounted on the roof of a vehicle and try to transform between the image and the world coordinate system. I made a calbration via cameraCalibrator and have the intrinsics, which do look good. Afterwards i tried to get the camera pose via estimateMonoCameraParameters, which gave me following values:

Estimated camera pose:

pitch: -0.6738

yaw: 3.2368

roll: 0.9376

height: 1.7581

which looks totally fine and plausible. My question is now, how can I get the camera matrices Translation, R respectively A from the estimated pitch, yaw, roll angle and height which i got from estimateMonoCameraParameters? If i use

monoCamera(cameraParams.Intrinsics, Camera.height, 'Yaw', Camera.yaw, 'Pitch', Camera.pitch, 'Roll', Camera.roll)

i dont get any more information. Unfortunately the camera position was moved, but before that i had a set of corresponding image and worldpoints and used

Camera.Extrinsics = estimateExtrinsics(imagePoints, worldPoints, Camera.Model.Intrinsics)

to get the extrinsics. Since i do not have any image and worldpoints for the new position, i can not use the estimateExtrinsics function for the new positioned camera. Additionally I was trying to compute the rotation and translation matrices by myself using

tform = rigidtform3d(angles,translation)

and use this as an extrinsics object, but i think I'm missing some additional transformation from the camera to the vehicle coordinate system and I'm not quite sure about the convention of the euler angles.

Is there maybe an easier way to get these camera matrices?

Any help would be appreciated,

Thanks a lot, Jens

Answer 1

[Matt J]

From the documentation for estimateMonoCameraParameters, it appears that the camera is assumed to lie at a translation of C=[0,0,height] from the world origin. So, the only non-trivial thing seems to be the conversion of yaw,pitch, roll to a rotation matrix. Here's a function that will do that.

function R = ypr2rotm(yaw, pitch, roll)
%YPR2ROTM Convert yaw, pitch, roll angles to a 3x3 rotation matrix
%
%   R = YPR2ROTM(yaw, pitch, roll) returns the rotation matrix corresponding 
%   to the given yaw, pitch, and roll angles (in degrees).
%
%   - yaw   : rotation about Z-axis (degrees)
%   - pitch : rotation about Y-axis (degrees)
%   - roll  : rotation about X-axis (degrees)
%
%   The rotation order is Z (yaw) -> Y (pitch) -> X (roll).

    % Rotation about Z (yaw)
    Rz = [cosd(yaw) -sind(yaw) 0;
          sind(yaw)  cosd(yaw) 0;
          0          0         1];

    % Rotation about Y (pitch)
    Ry = [cosd(pitch)  0  sind(pitch);
          0            1  0;
         -sind(pitch)  0  cosd(pitch)];

    % Rotation about X (roll)
    Rx = [1  0           0;
          0  cosd(roll) -sind(roll);
          0  sind(roll)  cosd(roll)];

    % Combined rotation (Z-Y-X intrinsic order)
    R = Rz * Ry * Rx;
    
end

[Matt J]

Is there maybe an easier way to get these camera matrices?

By a 'camera matrix', I'm assuming you ultimately want the 3x4 matrix given by,

P=K*R'*[eye(3),-C]

where K is the 3x3 intrinsic matrix. The intrinics are known from your calibration, so you should be able to build K readily. Above, I described how to calculate R and C in the world vehicle coordinate system described here.

But when building a camera matrix, your R and C have to be in a world coordinate system where the x,y axes are parallel to the camera x,y axes, and where the z-axis is depth. Based on the diagram in the doc, it appears we have to transform R and C accordingly:

Q=[0 0 1;
  -1 0 0;
  0 -1 0]'; 


R = Q*ypr2rotm(yaw, pitch, roll);

C = Q*[0;0;height];

P = K*R'*[eye(3),-C]

Finally, I will note that since Q is orthogonal, this could also have been written as,

Q=[0 0 1;
  -1 0 0;
  0 -1 0]; %<---note tranpose removed


R = ypr2rotm(yaw, pitch, roll);

C = [0;0;height];

P = K*R'*[Q,-C]

[Jens Gaechter]

Hi Matt,

thanks a lot for your fast and detailed answer! I oriented myself for this task on the cameraProjection matlab function, where the Projection matrix P is computed as P = K * [R T] as documented. By using following matrices from a former calibration from the estimateExtrinsics function:

K =

   1.0e+03 *

    1.3658         0    0.9591
         0    1.3675    0.5472
         0         0    0.0010


T =

    0.0065
    1.7248
    0.0129


R =

    0.0093   -0.9999   -0.0038
   -0.0075    0.0037   -1.0000
    0.9999    0.0093   -0.0075

i get identical results by using cameraProjection or multiplying it manually:

Pmatlab = cameraProjection(Camera.Model.Intrinsics, Camera.Extrinsics)

   1.0e+03 *

    0.9717   -1.3568   -0.0123    0.0212
    0.5369    0.0102   -1.3716    2.3658
    0.0010    0.0000   -0.0000    0.0000
    
P = K * [R T]

   1.0e+03 *

    0.9717   -1.3568   -0.0123    0.0212
    0.5369    0.0102   -1.3716    2.3658
    0.0010    0.0000   -0.0000    0.0000

What i tried now was to reverse engineer T and R by using following values of my camera:

 Camera = 

  struct with fields:

         pitch: 0.4282
           yaw: 0.5329
          roll: 0.2161
        height: 1.7249   

with

angles = [Camera.pitch Camera.yaw -Camera.roll];
tform = rigidtform3d(angles,T);
RcamToVeh = [0 -1 0; 0 0 -1; 1 0 0];
tform.R = tform.R * RcamToVeh;
tform.A(1:3,1:3) = tform.R;
Preverse = K * [tform.R tform.Translation']

Preverse =

   1.0e+03 *

    0.9717   -1.3568   -0.0124    0.0212
    0.5369    0.0102   -1.3716    2.3658
    0.0010    0.0000   -0.0000    0.0000

which leads to the same projection matrix, but i had to change the sign of the roll angle and i performed the transformation from the camera to the vehicle coordinate system in the form of R * RcamToVeh (which is the same matrix as the matrix Q in your suggestion). If i apply your code, i will get:

Q=[0 0 1;
  -1 0 0;
  0 -1 0]'; 
R = Q*ypr2rotm(Camera.yaw, Camera.pitch, Camera.roll);
C = Q*[0;0;Camera.height];
Pmatt1 = K*R'*[eye(3),-C]

Q=[0 0 1;
  -1 0 0;
  0 -1 0]; %<---note tranpose removed
R = ypr2rotm(Camera.yaw, Camera.pitch, Camera.roll);
C = [0;0;Camera.height];
Pmatt2 = K*R'*[Q,-C]
    
Pmatt1 =

   1.0e+03 *

   -0.0092   -0.9488    1.3729   -1.6366
   -1.3654   -0.5523   -0.0086   -0.9527
    0.0000   -0.0010    0.0000   -0.0017


Pmatt2 =

   1.0e+03 *

   -0.0092   -0.9488    1.3729   -1.6366
   -1.3654   -0.5523   -0.0086   -0.9527
    0.0000   -0.0010    0.0000   -0.0017

which do not match, which is not that surprising since your projection matrix seems to have a different convention. I tried to reverse engineer your solution, so what I did was also using the negative roll angle and i had to swap the angles in your ypr2rotm function, then I was able to get the same projection matrix:

% Try reverse engineering Matt's solution
Q=[0 0 1;
  -1 0 0;
  0 -1 0]'; %<---note tranpose removed
% R = ypr2rotm(Camera.yaw, Camera.pitch, Camera.roll);
R = ypr2rotm(-Camera.roll, Camera.yaw, Camera.pitch) * Q
C = T;
Pmattrev = K * [R T]

Pmattrev =

   1.0e+03 *

    0.9717   -1.3568   -0.0124    0.0212
    0.5369    0.0102   -1.3716    2.3658
    0.0010    0.0000   -0.0000    0.0000

So I'm a total newby in this field and I'm sure you know what is going on here, but I think in the rigidtform3d function the computation of the rotation matrix seems to have a different convention than your ypr2rotm function and the transformation Q has to be applied afterwards to achieve the same results. I was hoping that there was an easier way in matlab to get the camera extrinsics R and T to compute the projection matrix as with cameraProjection by specifying yaw, pitch, roll angles and height of the camera, since i was expecting all this troubles by implementing it by myself :D

[Matt J]

I think I had a mistake and it should be as below. Keep in mind that I only have your input data to 4 decimal places, so some discrepancies from what you're getting are to be expected.

K =1000*[1.3658         0    0.9591
         0    1.3675    0.5472
         0         0    0.0010];


yaw = 0.5329; pitch = 0.4282; roll = 0.2161; height = 1.7249;

Q=[0 0 1;
  -1 0 0;
  0 -1 0]';
R = Q*ypr2rotm(yaw, pitch, roll)';
C = [0;0;height];
Pmatt3 = K*R*[eye(3),-C]

Pmatt3 = 3×4

1.0e+03 * 0.9717 -1.3568 -0.0123 0.0212 0.5369 0.0102 -1.3715 2.3658 0.0010 0.0000 -0.0000 0.0000

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

function R = ypr2rotm(yaw, pitch, roll)
%YPR2ROTM Convert yaw, pitch, roll angles to a 3x3 rotation matrix
%
%   R = YPR2ROTM(yaw, pitch, roll) returns the rotation matrix corresponding 
%   to the given yaw, pitch, and roll angles (in degrees).
%
%   - yaw   : rotation about Z-axis (degrees)
%   - pitch : rotation about Y-axis (degrees)
%   - roll  : rotation about X-axis (degrees)
%
%   The rotation order is Z (yaw) -> Y (pitch) -> X (roll).

    % Rotation about Z (yaw)
    Rz = [cosd(yaw) -sind(yaw) 0;
          sind(yaw)  cosd(yaw) 0;
          0          0         1];

    % Rotation about Y (pitch)
    Ry = [cosd(pitch)  0  sind(pitch);
          0            1  0;
         -sind(pitch)  0  cosd(pitch)];

    % Rotation about X (roll)
    Rx = [1  0           0;
          0  cosd(roll) -sind(roll);
          0  sind(roll)  cosd(roll)];

    % Combined rotation (Z-Y-X intrinsic order)
    R = Rz * Ry * Rx;
    
end

[Jens Gaechter]

Thanks a lot Matt!