[WIP] Update CameraBaseToOffset

[sktometometo]

This PR is integrated version of #1312 and #1504 and other features.

This PR add features to CameraBaseToOffset.py below.

Transform calculation is thanks to @iory @Naoki-Hiraoka @HiroIshida

Features

• Fix odometry calculation ([WIP] convert t265 odom frame to initial base_link pose #1312)
  • Odometry transfomation calculation in original CameraBaseToOffset is wrong. [WIP] convert t265 odom frame to initial base_link pose #1312 fix that.
  • Orignal: new_odom_matrix = self.initial_matrix * current_camera_to_base * source_odom_matrix
  • yuki-sharks's version: new_odom_matrix = self.initial_matrix * source_odom_matrix * current_camera_to_base
• Fix pose covariance transformation
• Support Twist transformation.
• Support T265 type odometry (odometry origin frame is not $\Sigma_\mathrm{Ovis}$, corrected to be aligned to gravity)
• Fix source code format and variables
  • Remove unused import
  • Follow JSK basic parameter name convention (especially for TF frame id)
• Remove unused rosparams (~rate and ~tf_duration)
• [WIP] Add documentation of CameraBaseToOffset.

TODO

• documentation に transform についての数式・図を追加する(Add t265 odometry transformer #1504 の図を参照)
• covarianceの計算を修正。計算がちょっと多いのでon/offのparamがあるといい？
  • covariance は 決め打ち一定値、変換された値、そのままの3種類をparamで選択できるといい
• docsの導出を修正、多分ちょこちょこ間違っている。とくに vの変換
• odomの原点の座標系としてどのようなものをサポートするかリストアップ&paramで選択できるように
• T265方式のOdometryをサポート(odometry origin が $\Sigma_\mathrm{Ovis}ではなく重力方向に補正されている。
• T265でのワーキングサンプル や他のvisual odomでのワーキングサンプル みたいなものの追加
• 新しいロボットで使用する際の Tutorial の追加

[sktometometo]

座標系の闇に飲まれつつあるので、 https://github.com/sktometometo/jsk_robot/blob/PR/udpate-camera-base-to-offset/jsk_robot_common/jsk_robot_startup/README.md の計算式・導出見てておかしいところ見つけた人がいればコメントもらえると助かります。

[sktometometo]

面倒になったんで暫く塩漬けします