Precise Intrinsic Camera Calibration - for ROS camera_info message

Question

I use for intrinsic camera calibration the CALDE Tool (http://www.dlr.de/rm/en/desktopdefault.aspx/tabid-3925/) which is really powerfull and I would advise everybody to use this for precise intrinsic camera calibration.

Calde gives me in the end a file that looks like this:

% CAMERA # 1
% 

% Image size:
imagesize_1 = [ 640 480 ]

% Focal length:
fc_1 = [ 537.417 537.311 ]

% Principal point:
cc_1 = [ 314.329 239.206 ]

% Skew (please note Skew = Gamma/ScaleX):
alpha_c_1 = [ 0.00168813 ]

% Distortion (radial, decentering, and thin-prism, if any)
% (only kc_ is present in Bouguets toolbox, in that order)
kc_1 = [ 0.0450068 -0.144093 0.00000 0.00000 0.00000 ]
radial_1 = [ 0.0450068 -0.144093 0.00000 ]
decentering_1 = [ 0.00000 0.00000 0.00000 ]
thinprism_1 = [ 0.00000 0.00000 0.00000 ]

% TCP_T_CAMERA:
TCP_T_CAMERA1 = [ ...
 1.00000 0.00000 0.00000 0.00000 ;
 0.00000 1.00000 0.00000 0.00000 ;
 0.00000 0.00000 1.00000 0.00000 ]


% MAINCAMERA_T_OBJECT:

MAINCAMERA_T_OBJECT(1:3,1:4,1) = [ ...
 0.973021 0.118707 -0.197836 -41.0266 ;
 -0.0730198 0.971849 0.224004 -11.4107 ;
 0.218857 -0.203514 0.954297 662.759 ]

MAINCAMERA_T_OBJECT(1:3,1:4,2) = [ ...
 0.999266 0.0369076 0.0102493 76.4807 ;
 -0.0381896 0.980636 0.192078 116.143 ;
 -0.00296168 -0.192328 0.981326 1084.49 ]

MAINCAMERA_T_OBJECT(1:3,1:4,3) = [ ...
 0.992568 -0.0105678 -0.121230 106.536 ;
 0.0316905 0.984295 0.173663 -159.832 ;
 0.117491 -0.176215 0.977315 1087.52 ]

MAINCAMERA_T_OBJECT(1:3,1:4,4) = [ ...
 0.877250 -0.0191499 0.479652 166.351 ;
 -0.0501447 0.990082 0.131240 -29.7432 ;
 -0.477408 -0.139182 0.867589 988.947 ]

MAINCAMERA_T_OBJECT(1:3,1:4,5) = [ ...
 0.527366 -0.00319134 0.849632 144.484 ;
 -0.124694 0.988874 0.0811117 -24.5847 ;
 -0.840438 -0.148719 0.521101 969.772 ]

MAINCAMERA_T_OBJECT(1:3,1:4,6) = [ ...
 0.891724 0.0522552 -0.449552 94.2428 ;
 0.0213817 0.987339 0.157179 -0.458213 ;
 0.452074 -0.149773 0.879317 1082.62 ]

MAINCAMERA_T_OBJECT(1:3,1:4,7) = [ ...
 0.693097 0.0722656 -0.717212 76.8902 ;
 0.0699173 0.983531 0.166666 -17.6251 ;
 0.717445 -0.165661 0.676631 1050.90 ]

MAINCAMERA_T_OBJECT(1:3,1:4,8) = [ ...
 0.985416 0.0898682 -0.144496 -31.7523 ;
 -0.0664750 0.984994 0.159272 140.445 ;
 0.156641 -0.147344 0.976603 1565.78 ]

MAINCAMERA_T_OBJECT(1:3,1:4,9) = [ ...
 0.992972 0.0389569 -0.111755 -211.173 ;
 -0.0133414 0.975099 0.221369 228.341 ;
 0.117596 -0.218323 0.968765 1974.26 ]

MAINCAMERA_T_OBJECT(1:3,1:4,10) = [ ...
 0.997039 0.0730305 -0.0240702 -26.9394 ;
 -0.0675975 0.981645 0.178339 -45.1356 ;
 0.0366525 -0.176183 0.983675 706.637 ]

MAINCAMERA_T_OBJECT(1:3,1:4,11) = [ ...
 0.998929 0.0272116 -0.0374343 -5.70587 ;
 -0.0215852 0.989451 0.143250 -2.35603 ;
 0.0409375 -0.142289 0.988978 647.193 ]

MAINCAMERA_T_OBJECT(1:3,1:4,12) = [ ...
 0.987303 0.121919 -0.101829 -177.281 ;
 0.00962968 0.593922 0.804465 91.9028 ;
 0.158558 -0.795231 0.585206 1072.55 ]

Now I need this data stored in a ros camera_info message that looks like this: See here: http://docs.ros.org/kinetic/api/sensor_msgs/html/msg/CameraInfo.html

The distortion model used. Supported models are listed in
sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
simple model of radial and tangential distortion - is sufficient.
string distortion_model

The distortion parameters, size depending on the distortion model.
For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
float64[] D

Intrinsic camera matrix for the raw (distorted) images.
     [fx  0 cx]
 K = [ 0 fy cy]
     [ 0  0  1]
Projects 3D points in the camera coordinate frame to 2D pixel
coordinates using the focal lengths (fx, fy) and principal point
(cx, cy).
float64[9]  K # 3x3 row-major matrix

Rectification matrix (stereo cameras only)
A rotation matrix aligning the camera coordinate system to the ideal
stereo image plane so that epipolar lines in both stereo images are
parallel.
float64[9]  R # 3x3 row-major matrix

Projection/camera matrix
    [fx'  0  cx' Tx]
 P = [ 0  fy' cy' Ty]
    [ 0   0   1   0]
 By convention, this matrix specifies the intrinsic (camera) matrix
  of the processed (rectified) image. That is, the left 3x3 portion
  is the normal camera intrinsic matrix for the rectified image.
 It projects 3D points in the camera coordinate frame to 2D pixel
  coordinates using the focal lengths (fx', fy') and principal point
 (cx', cy') - these may differ from the values in K.
 For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
  also have R = the identity and P[1:3,1:3] = K.
 For a stereo pair, the fourth column [Tx Ty 0]' is related to the
  position of the optical center of the second camera in the first
  camera's frame. We assume Tz = 0 so both cameras are in the same
  stereo image plane. The first camera always has Tx = Ty = 0. For
  the right (second) camera of a horizontal stereo pair, Ty = 0 and
  Tx = -fx' * B, where B is the baseline between the cameras.
 Given a 3D point [X Y Z]', the projection (x, y) of the point onto
 the rectified image is given by:
  [u v w]' = P * [X Y Z 1]'
         x = u / w
         y = v / w
  This holds for both images of a stereo pair.
float64[12] P # 3x4 row-major matrix

What I do not know is how to obtain the P Matrix from the data of calde?

Answer 1

It seems you have a single camera. In P matrix:

fx: first value in fc_1.

fy: second value in fc_1.

cx and cy: values in cc_1 in same order as f.

and if you have a single camera, documentation says Tx and Ty are 0.

About distortion parameters, In ROS documentation distortion parameters with k are radial distortion parameters and t are tangential distortion parameters (plumb bob is combination of radial and tangential distortion). Since your CALDE tool only calculated first two radial distortion parameters, you can use those in same order.