cameraの内部パラメータから計算した水平画角が設定と違う気がする
ishiguroJSK opened this issue · 3 comments
あまり急ぎではないですが，choreonoidからROSに吐き出しているカメラ画像の内部パラメータがおかしい気がしています．
rtmros_choreonoid/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrlの中では画角が90°と設定されていますが，
DEF HEAD_LEFT_CAMERA VisionSensor {
name "LEFT_CAMERA"
translation 0.1005 0.035 0.05425
rotation 0.57735026919 -0.57735026919 -0.57735026919 2.094395102393
type "COLOR_DEPTH"
frontClipDistance 0.05
width 640
height 480
frameRate 15
fieldOfView 1.5708
sensorId 0
}
一方rostopic echo /multisense_local/left/camera_infoでは
---
header:
seq: 429
stamp:
secs: 29
nsecs: 1000000
frame_id: left_camera_optical_frame
height: 480
width: 640
distortion_model: plumb_bob
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [240.0, 0.0, 319.5, 0.0, 240.0, 239.5, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [240.0, 0.0, 319.5, 0.0, 0.0, 240.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
---
となっており，640x480でfx=fy=240なので(fx=fyなのもすでに怪しい)，
水直画角は theta_y / 2 = arctan( (高h/2) / fy ) によるとtheta_y = 90°になるのですが，
ROSに出ているfxを信じると，水平画角 theta_x / 2 = arctan( (幅w/2) / fx ) がtheta_x = 106°になってしまっています．
RTM側で吐き出されているカメラパラメータを読めないかと
rtprint localhost:15005/"JAXON_RED(Robot)0.rtc":HEAD_LEFT_CAMERA
などやろうと思いましたが，
rtprint: Warning: failed to import module Img
rtprint: Type not found: TimedCameraImage
となって表示されませんでした．
他に辿るためのコマンドや心当たりのあるソースファイル等ありますでしょうか？
プログラムの問題というより使い方の問題な気がしてきました．
multisenseの実機のスペックを確認すると「画角80°x45°」のように表記しているので，
問題(?)なのはwrlで画角が1個しか指定されていないことかなとも思ったのですが，
「choreonoidでは垂直画角のみを指定する」という方針だと思えばそれで納得できます．
それにそもそもfx=fyとなるのはほとんどのカメラがそうなので問題なかったです．
http://choreonoid.org/ja/manuals/1.5/handling-models/modelfile/modelfile-openhrp.html
ここに、OpenHRPのモデルファイルのVisionSensorの定義が書いてあります。
定義としては、視野角度と書いてあって詳しい説明はないですが、choreonoidでは垂直画角として扱っています。（ここはソース確認するしかないところ）
それで、以前は正しくなかったので、以下で正しく出るようにした。
s-nakaoka/choreonoid#31
だけど、ステレオの場合はカメラ間距離が必要なので、
パラメータをうわ書くようにセットしている。
https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_choreonoid_tutorials/launch/jaxon_red_vision_connect.launch#L27-L52
パラメータの意味としては、
カメラのレンズは基本円形なので軸対称になるので、fはひとつです。
fx,fyのパラメータの意味は、x方向とy方向で焦点距離が異なるということになって、通常のレンズだとおかしなことになります。
ですが、カメラパラメータのfx,fyはピクセルあたりの焦点なので、ピクセルの大きさがx,yで異なると違ってくるときもあろうかと思います。
キャリブしてみると、fx=fyとなるのはほとんどのカメラがそうということになります。
ステレオの確認方法
ROS_NAMESPACE=/multisense_local rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_rect right/image_raw:=right/image_rect
/multisensr_local/points2にステレオ処理の結果が出ます。
以下のように、ステレオの結果とlaserの結果（こちらは正しいとする）が、まあまあ
一致するので（ステレオはそもそも特徴がないとこれぐらいのばらつきはあたりまえ）
パラメータはほぼ正しいかと思います。
以下で、赤の点がレーザー、黄色の点がステレオです。
コメントありがとうございます．
launch内でも明示的に上書いていたのですね．
今後の参考になります．