Rviz視点変更のコードを読む

はじめに

Rvizの視点変更の動きが良いなと思い、内部の計算を見てみることにしました.
調査対象の視点タイプはOrbitというものでデフォルトで指定されている視点タイプのものです.

調査結果(UI側)

parameter

Oribit視点の持つパラメータについて主要と思われるものを列挙します.

• Distance: 対象までの距離
• Yaw: 視点 横回転 (範囲：0 ~ 6.28)
• Pitch: 視点 縦回転 (範囲：-1.54 ~ 1.54)
• Focal Point：焦点位置

挙動

• 左クリック：回転
• 右クリック + マウス：Z移動
• shift + 左クリック + マウス：X/Y移動

調査結果(コード)

orbit_view_controller.cpp
より

視野の回転

マウスの移動による視野の回転はxの移動量に対して,Yawをyの移動量に対してpitchを増減させることでおこなっています.

00129   else if( dragging_ && event.type == QEvent::MouseMove )
00130   {
00131     diff_x = event.x - event.last_x;
00132     diff_y = event.y - event.last_y;
00133     moved = true;
00134   }
00135 
00136   // regular left-button drag
00137   if( event.left() && !event.shift() )
00138   {
00139     setCursor( Rotate3D );
00140     yaw( diff_x*0.005 );
00141     pitch( -diff_y*0.005 );
00142   }

次は、設定されたyaw,pitch,distanceがどのようにカメラの位置、姿勢に変換されているかを見ていきます.

まず、位置については以下のように計算されます.

00254   float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x;
00255   float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
00256   float z = distance *              sin( pitch ) + focal_point.z;