icp odometry with t265 as guess

[Mikor]

Hello,

I will start by saying what my end goal is.

Goal.

I want to use a vlp-16 and a t265 in order to make pointclouds. As I understand I want to do something like that . So the first step is to feed the icp_odometry node with the scans and the odometry but I can not understand how exactly I am supposed to do this.

Data to work on.

At this moment the data I am working on are in a bag file and these are the topics it carries.

topics:      /clock                 16974 msgs    : rosgraph_msgs/Clock    
             /econ_img                297 msgs    : econcameras/imName     
             /realsense_publisher   16882 msgs    : nav_msgs/Odometry      
             /rosout                    9 msgs    : rosgraph_msgs/Log       (3 connections)
             /rosout_agg                2 msgs    : rosgraph_msgs/Log      
             /tf_static                 1 msg     : tf2_msgs/TFMessage     
             /velodyne_points        2265 msgs    : sensor_msgs/PointCloud2

The tf_static it only describes the relationship between the lidar and the base_link.

tf/tfMessage

At this example the tf is not static and the structure of the tf tree is more complicated. According to this example, I assume that my tf tree should look like this.



My thinking is the following.
The velodyne will describe the transformation between base_link and the lidar.
The T265 will describe the transformation between the base_link and the T265.
I assume that these 2 transformations will be static.
And then a non-static transformation between the odometry and the base_link will be given by the topics of T265 which are a nav_msgs/Odometry.

I am not very sure if this structure is appropriate and if it is I do not know how to "design" it, any tips here?

Edit 1: I realize after reading the documentation about nav_msgs/Odometry that the information that was recorded by the corresponding topic should be put between the T265 and the base link (the header will be the base_link and the child frame the T265). But still, I am not so sure about this conclusion either.

Edit 2: I think I can use the pose part of the nav_msg to describe the tf between odom and base_link and then between base_link and the vlp will be a tf_static and between the base_link and T265 will be the T265.



I think that this is the way to go but I am really not 100% sure about this and I need to figure out how to actually create something that publishes this tf_tree and put it in a bag.

ICP odometry and guess_frame_id

Based on the demo_hector_mapping.launch file the guess_frame_id takes as input the odom that I assume is taken from the tf_tree, is my assumption right if not how is this supposed to work?

[matlabbe]

Hi,

I don't think you can change T265 TF frames easily to use base frame of the robot (base_link) instead of the t265 frame.

Based on the T265 tree from this post, your final TF tree when using icp_odometry and T265 as guess would look like this:



The base frame would be camera_pose_frame instead of base_link in the examples. You would have to add a static tf between the camera and velodyne. The other static tf would be already published by realsense node.

When recording your bags, record /tf topic too.

EDIT: jsut realized that "Using F2M (RTAB-Map) odometry but with T265 odometry as guess" section can be an example of using t265 as guess for icp_odometry (instead of stereo_odometry).

cheers,
Mathieu

[Mikor]

Hi Mathieu,

thank you for your answer but at this moment I am not using the sensors to experiment with them, I only have a bag of recorded topics. And the topic that came from the T265 contains this information.

header: 
  seq: 997
  stamp: 
    secs: 1572616239
    nsecs: 451276063
  frame_id: "odom"
child_frame_id: "camera_pose_frame"
pose: 
  pose: 
    position: 
      x: -0.0375508069992
      y: 0.00650884723291
      z: -0.0701411291957
    orientation: 
      x: -0.0780994519591
      y: 0.518345415592
      z: 0.0424006991088
      w: 0.85054141283
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

I understood that the pose part of this message should be located in between the camera_odom_frame and camera_pose_frame. Also, I do not know which is the transformation between map and icp_odom, meaning that I do not know which corresponding topic it is. And the tf between the icp_odom and camera_odom_frame corresponds to the /rtabmap/icp_odometry, am I correct?

But my final question is how to create such a tf tree.. I mean should I create a node that publishes it ?

Thank you in advance Mathieu, you always give very good and detailed answers.

[matlabbe]

Hi,

I've shown in my tf image above which node is publishing the corresponding TF. The TF you would have to explicitly publish (e.g., with a static_transform_publisher) is the transform between the camera frame and the velodyne frame.

For TF frames between the camera link and optical frames of T265, they are published by realsense node. If you don't use the T265 images in rtabmap, it won't be a problem though with this bag. You would have however to generate the TF frame of T265 odometry from that topic you shown (that node could be used for that).

rosrun rtabmap_ros odom_msg_to_tf odom:=/realsense_publisher _odom_frame_id:=camera_odom_frame

cheers,
Mathieu