【Gazebo】Tu​rtleBotのカメ​ラの焦点距離（Foc​al Length）について

https://jp.mathworks.com/help/robotics/examples/read-model-and-simulation-properties-from-gazebo.html を拝見しながら、現在、ターゲット物体のトラッキングのシミュレーションを仮想マシン上のGazeboおよびMATLABを用いて構築中です。

GazeboのTurtleBotのカメラパラメータ（ex. 焦点距離）をMATLAB上で取得したいのですが、どのようにすればよいでしょうか。

【使用ツールボックス】Robotics System Toolbox, Image Processing Toolbox

お手数ですが、ご回答いただけると幸いです。よろしくお願い申し上げます。

[Kei Otsuka]

カメラのパラメータはUDFファイルに記載されているのですが、これをMATLABから読み取りたい、ということでしょうか? (KinectカメラオブジェクトのUDFファイルは以下にあります)

~/.gazebo/models/kinect/model.sdf

[S.S]

Otsuka 様

早速のご回答、ありがとうございます。

はい、仮想マシンDesktop上の"Gazebo Empty"をクリックして立ち上げたときに、 最初に表示されるTurtleBotのカメラのパラメータをMATLABから読み取りたい と考えております。

RoSのドキュメント（http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html）を調べたところ、sensor_msgs/CameraInfo内のKマトリックス内に焦点距離f_x、f_yが記載されているとのことだったので、以下のようにしてMATLABから読み取ろうと考えたのですが、正しいでしょうか。また、この場合、f_x、f_yの単位（Unit）はどうなるのでしょうか。

    % サブスクライバーの作成
        imsub = rossubscriber('/camera/rgb/image_raw');
        imInfo = rossubscriber('/camera/rgb/camera_info'); 

    % 画像の取得 & Figureウィンドウでの表示
      latestImg = readImage(receive(imsub));
      imshow(latestImg)

    % latestImg 取得時のTurtleBotのカメラパラメータ（Kマトリックス）の取得
　　　  CameraInfo = receive(imInfo)
      K = CameraInfo.K

お手数おかけしますが、ご回答のほど、よろしくお願い申し上げます。

cf. RoSドキュメント内の記述（抜粋）

# 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

Answer 1

[Tohru Kikawada]

> RoSのドキュメント（http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html）を調べたところ、sensor_msgs/CameraInfo内のKマトリックス内に焦点距離f_x、f_yが記載されているとのことだったので、以下のようにしてMATLABから読み取ろうと考えたのですが、正しいでしょうか。

はい、正しいです。

> また、この場合、f_x、f_yの単位（Unit）はどうなるのでしょうか。

単位としてはピクセルになります。 実寸(メートル)にしたい場合には画素サイズ(センサーの1ピクセルあたりの実寸値)を掛け算する必要があります。 こちら のサイトに換算方法について記載があるようです。ご参考まで。

ちなみに、Computer Vision System Toolboxをお持ちであれば、歪パラメーターを取得して、歪補正をかけたりできるので便利です。

% 初期化
clear; close all; clc;

% GazeboにROSで接続
rosshutdown;
rosinit('192.168.112.130');

% サブスクライバーの作成
imsub = rossubscriber('/camera/rgb/image_raw');
imInfo = rossubscriber('/camera/rgb/camera_info');

% 画像の取得 & Figureウィンドウでの表示
latestImg = readImage(receive(imsub));
imshow(latestImg)

% latestImg 取得時のTurtleBotのカメラパラメータ（Kマトリックス）の取得
CameraInfo = receive(imInfo);
K = reshape(CameraInfo.K,[3 3]); % 単位はすべてpixel
D = CameraInfo.D(1:3); % ひずみパラメーターはk1からk3まで使用可能

% カメラパラメーターオブジェクトを作成
cameraParams = cameraParameters('IntrinsicMatrix',K,'RadialDistortion',D)

% ひずみ除去 (今回はひずみパラメーターが0のため画像は変化しない)
latestImgUndistorted = undistortImage(latestImg,cameraParams);
figure, imshow(latestImgUndistorted);

実行結果：

K =
  554.2547         0         0
         0  554.2547         0
  320.5000  240.5000    1.0000
D =
     0
     0
     0
cameraParams = 
  cameraParameters のプロパティ:

     Camera Intrinsics
                      IntrinsicMatrix: [3×3 double]
                          FocalLength: [554.2547 554.2547]
                       PrincipalPoint: [320.5000 240.5000]

[S.S]

KItagawa 様

とても丁寧にご回答いただき、ありがとうございました。画素サイズの実寸値の変換も、『Figureウィンドウの作成』（https://jp.mathworks.com/help/matlab/ref/figure.html）　の'Units'の箇所を参考に、無事行うことができました。

また何か質問が出てきた際には、改めて投稿させていただきます。よろしくお願い申し上げます。