rtabmap for kinect v2 on ROS Indigo

[tejaswi]

Hi,
I am trying to implement your code on the kinect v2 (K4Wv2) on ROS Indigo.
I am using libfreenect2 and iai_kinect2 to interface the kinect with ROS.

Could you help me out on configuring/building your code using libfreenect2?
Any pointers in this direction would be helpful.

Thanks.

[matlabbe]

Hello,

On ROS, the built-in options for libfreenect, openni and openni2 are not enabled. Images are taken instead from nodes like openni_launch, openni2_launch or freenect_launch to integrate to existing robot system.

If you want to add libfeenect2 for the standalone version, you can look at the examples in https://github.com/introlab/rtabmap/blob/master/corelib/src/CameraRGBD.cpp. There is one for libfreenect (1), but I think the API may have changed between the two versions.

I have a K2Wv2 too here, but I didn't successfully able to use it on Hydro yet. I wasn't aware of the iai_kinect2 package, I will take a look soon. thx

PS: I've seen your email first, answer in this thread instead if you reply :P

[tejaswi]

thanks. I'll have a look

[tejaswi]

Hi,
I got the Kinect2 to work with the ROS version of rtabmap. Here is what I did.

I installed the ROS version of rtabmap as specified in the installation page.

I am using libfreenect2 and the iai_kinect2 package to integrate the sensor with ROS (indigo).

The rtabmap node expects the topics

/camera/rgb/image_rect_color
/camera/depth_registered/image_raw
/camera/depth_registered/camera_info

as inputs. The equivalent topics published by the kinect2_bridge node of the kinect2_bridge package that is part of iai_kinect2 are the following.

/kinect2_head/rgb_rect/image
/kinect2_head/depth_highres/image
/kinect2_head/depth_highres/camera_info

So, I wrote a launch file for launching the kinect2_bridge node that would remap the iai_kinect2 topics to the required rtabmap topics as follows.

<launch>
  <arg name="publish_frame" default="false" />

  <include file="$(find kinect2_bridge)/launch/include/kinect2_frames.launch">
    <arg name="publish_frame" value="$(arg publish_frame)" />
  </include>
  <node name="kinect2_bridge" pkg="kinect2_bridge" type="kinect2_bridge" respawn="true" output="screen">
      <remap from="/kinect2_head/rgb_rect/image" to="/camera/rgb/image_rect_color"/>
      <remap from="/kinect2_head/depth_highres/image" to="/camera/depth_registered/image_raw"/>
      <remap from="/kinect2_head/depth_highres/camera_info" to="/camera/depth_registered/camera_info"/>
  </node>
</launch>

The other thing that needs to be changed is the "frame_id" of the headers of the kinect2_bridge images.
Rtabmap expects the frame_id s of the input images to be base_link, while the kinect2_bridge outputs them as "head_mount_kinect2_rgb_optical_frame".
For lack of a simpler (lazier) way to implement this modification, I just changed the entry for  K2_TF_RGB_FRAME in kinect2_definitions.h file in the kinect2_bridge folder.

Now you can run

 $ roscore 

 $ roslaunch kinect2_bridge <my_new_launch>.launch 

 $ roslaunch rtabmap rgbd_mapping.launch

 or

 $ roslaunch rtabmap rgbd_mapping_rviz.launch

and everything should work.

Thanks again for pointing me in the right direction.

[matlabbe]

Nice, I'll try this! Thank you for the detailed description.

Cheers,
Mathieu

[matlabbe]

For information, I've finally tested the approach above on a clean Ubuntu 14.04/Indigo installation. I've made a launch file (rgbd_mapping_kinect2.launch) to test the Kinect v2.

Install driver
Follow ALL installation directives of the iai_kinect2 package. Make sure it is calibrated for better mapping results!

Run

$ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch
$ rosrun kinect2_bridge kinect2_bridge  _publish_tf:=true

[A]

Hi

In the rgbd_mapping_kinect2.launch file, rgb/image and depth/image are mapped to rgb_lowres and depth_lowres respectively. Is there an alternative to the lowres images, because providing regular rgb and depth images, gives an error that datatype is 16 or 32 bit.

Also, in L44-45: in the arguments /base_link and /kinect2_link, should one of these correspond to the frame_id published by kinect2_bridge?

[matlabbe]

Hi,

Are you using the binaries? I think there was a fix about the datatype error in the trunk (to include more types of images).
In the launch file, I've put the combinations that you can try:

Prefixes:
           sd: rgb_lowres / depth_lowres
           hd: rgb_rect / depth_highres
           ir: ir_rect / depth_rect

Example HD:
$ roslaunch rgbd_mapping_kinect2.launch rgb_prefix:=rgb_rect depth_prefix:=depth_highres max_depth:=10 inlier_distance:=0.1 local_map:=5000

Note that in HD, the odometry is very slow. There is still some parameters tuning to do for the Kinect2... I should check this soon.

The /kinect2_link is published by kinect2_bridge. By default, rtabmap uses "frame_id=/base_link" so I just added a static_transform instead of setting the frame_id for all nodes to /kinect2_link.

cheers

[A]

Hi

Thanks for your response.
I use the latest source code.
Unfortunately kinect2_bridge doesn't publish depth_lowres or depth_highres in my case.
It only publishes depth (512*424), depth_rect (512*424), depth_scaled (1920*1080). But all of these are of type 16UC1 and hence result in an error.

The frame_id issue was resolved with your clarification.

Best

[matlabbe]

These are the topics sent when kinect2_bridge is up:

$ rostopic list
/kinect2/depth/camera_info
/kinect2/depth/image
/kinect2/depth/image/compressedDepth
/kinect2/depth_highres/camera_info
/kinect2/depth_highres/image
/kinect2/depth_highres/image/compressedDepth
/kinect2/depth_lowres/camera_info
/kinect2/depth_lowres/image
/kinect2/depth_lowres/image/compressedDepth
/kinect2/depth_rect/camera_info
/kinect2/depth_rect/image
/kinect2/depth_rect/image/compressedDepth
/kinect2/ir/camera_info
/kinect2/ir/image
/kinect2/ir/image/compressed
/kinect2/ir_rect/camera_info
/kinect2/ir_rect/image
/kinect2/ir_rect/image/compressed
/kinect2/mono/camera_info
/kinect2/mono/image
/kinect2/mono/image/compressed
/kinect2/mono_lowres/camera_info
/kinect2/mono_lowres/image
/kinect2/mono_lowres/image/compressed
/kinect2/mono_rect/camera_info
/kinect2/mono_rect/image
/kinect2/mono_rect/image/compressed
/kinect2/rgb/camera_info
/kinect2/rgb/image
/kinect2/rgb/image/compressed
/kinect2/rgb_lowres/camera_info
/kinect2/rgb_lowres/image
/kinect2/rgb_lowres/image/compressed
/kinect2/rgb_rect/camera_info
/kinect2/rgb_rect/image
/kinect2/rgb_rect/image/compressed