0
$\begingroup$

Rosanswers logo

I'd like to record some bag files of Kinect data and then run RGBDSLAM on them later. (The computer I'm doing the collection with isn't up to doing the RGBDSLAM itself.)

The problem is that I want to keep the bag files as small as possible, so I want to collect the minimal amount of data necessary.

In particular, that means I would like to record to the bag file the smallest number of topics possible, and preferably only image topics and the like -- PointCloud2 messages are quite large in comparison.

Can someone tell me exactly which topics are necessary for running RGBDSLAM off a bag file, and walk me through how to get RGBDSLAM to use only them? (Also, I'm a little confused about whether I should be using the dynamic_reconfigure reconfigure_gui to turn on OpenNI's depth registration or not...)

I thought I needed only RGB and depth image and possibly camera info topics, but when I tried running RGBDSLAM off of only those, it kept saying it was waiting for various other things. I think maybe I just need to change the parameters in the launch file to tell it that those are the only things I want it to work with, but I can't figure out how to do that properly.

Any help would be greatly appreciated! :)

Edit:

As a specific example of one of the things I've tried, I ran the following:

$ rosbag record /camera/depth_registered/image_rect_raw /camera/rgb/image_rect_color /camera/rgb/camera_info

I then ran the RGBDSLAM GUI and pressed the space key. The status bar displayed:

Processing. Waiting for motion information.

I then started playback of the bag file. Nothing changed.

Edit 2:

I followed Felix's directions, and everything worked swell... except for one problem. The depth images were not registered with the RGB images, so the textures in the resulting map were all incorrect.

So I tried again, this time using openni_launch openni.launch, using the dynamic_reconfigure reconfigure_gui to turn on depth registration (/camera/driver --> depth registration checkbox), and then using the following call to rosbag to record the bag file:

$ rosbag record -o /home/full/path/and/filename_prefix /tf /camera/rgb/image_color /camera/rgb/camera_info /camera/depth_registered/image /camera/depth/camera_info

I then ran the following launch file on this bag file (pressing the spacebar to start the processing once the GUI came up):

<launch>
  <node pkg="rgbdslam" type="rgbdslam" name="rgbdslam" cwd="node" required="false" output="log" > 
    <!-- see rgbslam_sample_config.launch for all available parameters and their default values -->
      <param name="config/topic_image_mono"              value="/camera/rgb/image_color"/>

<param name="config/topic_image_depth"             value="/camera/depth_registered/image"/>
<param name="config/topic_points"                  value=""/> <!--if empty, poincloud will be reconstructed from image and depth -->
<param name="config/camera_info_topic"             value="/camera/rgb/camera_info"/>
<param name="config/bagfile_name"                  value="/home/full/path/and/filename.bag"/>
<param name="config/start_paused"                  value="true"/>

      <param name="config/wide_topic"                    value=""/>;
      <param name="config/wide_cloud_topic"              value=""/>;
      <param name="config/drop_async_frames"             value="true"/> <!-- Check association of depth and visual image, reject if not in sync -->
      <param name="config/feature_detector_type"         value="SURF"/><!-- If SIFTGPU is enabled in CMakeLists.txt, use SURF here -->
      <param name="config/feature_extractor_type"        value="SURF"/><!-- If SIFTGPU is enabled in CMakeLists.txt, use SURF here -->
      <param name="config/matcher_type"                  value="FLANN"/> <!-- FLANN (not avail for ORB features), SIFTGPU (only for SIFTGPU detector)  or BRUTEFORCE-->
      <param name="config/max_keypoints"                 value="2000"/><!-- Extract no more than this many keypoints (not honored by SIFTGPU)-->
      <param name="config/min_keypoints"                 value="1600"/><!-- Extract no less than this many ... -->
      <param name="config/optimizer_skip_step"           value="5"/><!-- optimize every n-th frame -->
      <param name="config/store_pointclouds"             value="true"/> <!-- if, e.g., only trajectory is required, setting this to false saves lots of memory -->
      <param name="config/individual_cloud_out_topic"    value="/rgbdslam/batch_clouds"/>;
      <param name="/config/use_gui" value="true"/>
    <!-- see rgbslam_sample_config.launch for all available parameters and their default values -->

<remap from="/camera/depth/camera_info" to="/camera/depth_registered/camera_info" />

  </node>
</launch>

However, in the RGBDSLAM GUI, the colors are messed up in the images, no color seems to show up on the generated map, and I get the following errors in the output on the commandline:

...
[ERROR] [1332866155.186909382]: Unknown image encoding: bgr8!
[ERROR] [1332866159.956687487]: Unknown image encoding: bgr8!
[ERROR] [1332866164.509685562]: Unknown image encoding: bgr8!
...

Edit 3:

I've got a fix which works for now, but I'd greatly appreciate a better solution.

I edited createXYZRGBPointCloud in src/misc.cpp:

pointcloud_type* createXYZRGBPointCloud (const sensor_msgs::ImageConstPtr& depth_msg, 
                                         const sensor_msgs::ImageConstPtr& rgb_msg1,
                                         const sensor_msgs::CameraInfoConstPtr& cam_info) 
{
  sensor_msgs::ImagePtr rgb_msg(new sensor_msgs::Image);
  rgb_msg->header = rgb_msg1->header;
  rgb_msg->height = rgb_msg1->height;
  rgb_msg->width = rgb_msg1->width;
  rgb_msg->encoding = rgb_msg1->encoding;
  rgb_msg->is_bigendian = rgb_msg1->is_bigendian;
  rgb_msg->step = rgb_msg1->step;
  rgb_msg->data = rgb_msg1->data;

  if (rgb_msg->encoding.compare("bgr8") == 0) {
    rgb_msg->encoding = "rgb8";
    for (int i=0; i<rgb_msg->data.size(); i+=3) {
      char temp_red = rgb_msg->data[i];
      rgb_msg->data[i] = rgb_msg->data[i+2];
      rgb_msg->data[i+2] = temp_red;
    }
  }

  ...

Originally posted by Yo on ROS Answers with karma: 183 on 2012-03-22

Post score: 7

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The best way in general is to record the following:

  1. /tf
  2. /camera/rgb/image_mono OR /camera/rgb/image_color if you want color
  3. /camera/rgb/camera_info
  4. /camera/depth/image
  5. /camera/depth/camera_info

/tf is not strictly required but helpful depending on the use case. The rosbag command is then:

rosbag record -O filename-prefix /tf /camera/rgb/image_color /camera/rgb/camera_info /camera/depth/image /camera/depth/camera_info

I still use the openni_camera package to get the data, that would be:

roslaunch openni_camera openni_node.launch

Have a look at the file launch/rgbdslam.launch. If necessary adapt to your choice of grayscale image: change the first parameter to /camera/rgb/image_mono

Play the bagfile (but stop the openni_node first):

rosbag play filename-prefix-some-date-string.bag

Have a look at the options (rosbag help play). Reducing the playback rate might help unless you have a really fast computer.

Alternatively, you can set the bagfile_name parameter (see the example config launchfile to your bagfile. Use an absolute path. The start_paused parameter could make things more convenient.

Then run rgbdslam from the (adapted) launch file

roslaunch rgbdslam rgbdslam.launch

Originally posted by Felix Endres with karma: 6468 on 2012-03-26

This answer was ACCEPTED on the original site

Post score: 6


Original comments

Comment by Yo on 2012-03-27:
Awesome, thank you, Felix! :) I'm still having some trouble however -- please see Edit 2 in the question. :)

Comment by Felix Endres on 2012-03-27:
Interesting. Are you using openni_launch or openni_camera? That could cause the missing calibration. The rgb thing is maybe a different version of cv_bridge. I am using ROS electric and you? A workaround may be to additionally record "/camera/rgb/points" and set that topic as the topic_points param.

Comment by Yo on 2012-04-04:
I'm using openni_launch, since, as I noted in Edit 2, openni_camera was not providing registered depth images. I'm also using ROS Electric. I'm not using /camera/rgb/points because I want to keep the bag filesize as low as possible.

Comment by Felix Endres on 2012-04-07:
Well, interesting. I haven't got that problem with openni_camera. So openni_launch gives you bgr8 with the rgb8 encoding specified? If you don't care about the image colors in the UI, you could also just switch the channels in misc.cpp line 481-483 and skip the conversion in Edit 3

Comment by pinocchio on 2014-03-26:
Hi @Yo, I encounter the same problem as discribed in your Edit 2. I run kinect XBOX on ROS hydro, and it works well for unregistered image. But there is no output of registered RGBD image. The detailed information is listed here http://answers.ros.org/question/143871/no-register-data-from-kinect/. Could you please tell me how to fix it?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.