Camera Pose with Covariance

[b.emery94]

Hello!

I am using kinect v2 attached to a turtlebot 2 in addition to an IMU and 2D laser scanner. I want to use the ROS wrapped RTAB-Map to provide the 3D pose of the kinect (and by extension the robot) for use in an EKF used for target tracking. I was wondering:

   - Is there a parameter or simple in-built way to publish the pose as a ROS msg, or do I have to extract it from the published TF?
   - Is there a way to get the covariance matrix associated with the camera's pose? I noticed that the rgbd_odometry node seems to publish an odometry message with an associated covariance matrix, although I'm assuming that this does not have the added benefit of loop closure or the graph optimisation that RTAB-Map performs.

What would you recommend on these 2 points?

Thanks in advance for your help!

[matlabbe]

Hi,

You can indeed get the current pose with /TF frames /map -> /camera_link. You can also subscribe to /odom from rgbd_odometry to get the pose in /odom frame and the covariance associated with it. That way you don't have to pool TF to know if there is a new pose. If you want the odometry pose in /map frame, use /TF to get transform "M" between /map and /odom, then multiply odom: Pose = M x Odom.

cheers,
Mathieu

[b.emery94]

Hi Mathieu,

Thanks very much for the response! My understanding is that the graph optimisation and loop closure is performed in the main rtabmap node, whereas the rgbd_odometry node only uses visual features to compute odometry. If I use the covariance from the odometry node, will I be losing any covariance information that may be computed in the main node? I'm not very familiar with graph SLAM so I'm not sure how covariance is dealt with in graph SLAM.

Thanks again!

[matlabbe]

Hi,

odometry covariance (the twist one) is used in the link of the graph. On loop closure, this tells how much a link can be corrected relatively to others (e.g., links with higher covariance will take more errors). However, there is no resulting covariance for the current pose after graph optimization. I opened an issue on github.

cheers

[b.emery94]

Great, thanks very much for your help! I look forward to seeing the changes, and until then I will use the covariance from the rgbd odometry.