robot_localization全然わからん。

robot_localizationを使って、roombaのwheel odometryと、realsense T265(VSLAMカメラ)によるVisual odometryを統合しようとしたら見事にハマったのでメモ。

http://docs.ros.org/melodic/api/robot_localization/html/index.html


T265のカメラがはき出すodometry座標系を利用して、その上にmapを置く構成の絶対座標を使っています。T265はroombaの端っこに載っているので、camera_pose_frameの下にbase_linkがずれた位置に存在しているというstatic TFを書いています。

後述のにように、このズレがたぶん色々な元凶な気ももします。
ということでTFとか楽勝なツヨツヨな方にチェックしていただければなぁ～。

暗黙のbase_link（本命）

Odometryのchild_frame情報をここで捨てている。
すなわち、Odometryのchild_framが暗黙のうちにbase_linkであることを仮定しているが、
VSLAMカメラは機体の端っこについているのでその分ずれる。
一応、座標変換ノードを入れれば対処可能?
https://github.com/cra-ros-pkg/robot_localization/blob/melodic-devel/src/ros_filter.cpp#L1690-L1701

差分とったら、元がなんであれframe_idはbase_linkだよね？？（ついで）

differentialモードの時の話。
https://github.com/cra-ros-pkg/robot_localization/blob/melodic-devel/src/ros_filter.cpp#L2763

child_fram==base_linkだとして話すると、前回のodometoryPoseからの差分( poseTmp.setData(prevMeasurement.inverseTimes(poseTmp)) )の
frame_idはbase_linkだよね。そして、「twistPtr->header.frame_id = baseLinkFrameId_;」という風にたしかにbase_linkを設定している。
しかし、なぜか追加の回転「poseTmp.mult(targetFrameTrans, poseTmp)」をかけている
このへんが謎。