MEAM 620: Robotics Roslab/Kinect Matlab

Kinect & Matlab

Brief Summary

This page describes how you can retrieve RGB and Depth images from your Kinect into Matlab using ROS. Every researcher's dream.

http://www.youtube.com/watch?v=liSFGQIgS74

Prerequisites

Note: If you are in the ROSLab, then no need to worry about installing anything because everything is installed correctly. Just make sure you are configured to use diamondback. If you are - then the IPC-bridge is already configured for you.

ROS Diamondback is installed and set as your ROS_ROOT (tutorial).
The openni_kinect stack is compiled and in your ROS_PACKAGE_PATH. Also, you added your user to the 'video' group on your machine (tutorial).
IPC-Bridge is installed and in your ROS_PACKAGE_PATH (tutorial).
You have matlab :)

How to Use it

Download this example matlab script. Start matlab and open the script.
Download this launch file. Launch the launch file. Check the terminal for any errors. You should not see any errors whatsoever if everything launched correctly. (Note: Launch files do not need to be placed in your ROS_PACKAGE_PATH...You can launch them from anywhere.)
roslaunch kinect-matlab.launch
In matlab, run your script by pressing 'F5'.
You might drop 1 or 2 frames at the start of the program but then you should see a window pop up displaying streaming depth and rgb images.

Example Launch file

Here is an example launch file that brings up the proper Kinect nodes, IPC node, and IPC-Bridge message nodes for the two topics that we are interested in here.

kinect-matlab.launch

<launch>

  <include file="$(find openni_camera)/launch/openni_node.launch" />

   <node pkg="ipc"
    name="central"
    type="central"
    output="screen"
    args="-su">
  </node>

  <node pkg="ipc_sensor_msgs"
    name="kinect_rgb_image_node"
    type="sensor_msgs_Image_publisher"
    output="screen">
    <remap from="~topic" to="/camera/rgb/image_color"/>
    <param name="message" value="rgb"/>
  </node>

  <node pkg="ipc_sensor_msgs"
    name="kinect_depth_image_node"
    type="sensor_msgs_Image_publisher"
    output="screen">
    <remap from="~topic" to="/camera/depth/image_raw"/>
    <param name="message" value="depth"/>
  </node>

</launch>

Example Matlab Script

stream_kinect.m

Note: This script is by no means efficient whatsoever. I am open to all suggestions on how to improve it.


% Benjamin Cohen
% Spring 2011

clear all; clc; close all;

% add the ipc_bridge_matlab binaries to your path
[a, p] = system('rospack find ipc_sensor_msgs');
addpath(strcat(p, '/bin')); 

% 'subscribe' to the RGB & Depth image topics
sid=sensor_msgs_Image('connect','subscriber','kinect_rgb_image_node','rgb');
sid2=sensor_msgs_Image('connect','subscriber','kinect_depth_image_node','depth');

% this make for a nice spot & window size on my monitor
figure('OuterPosition',[1,1,654,1125]);

for i = 1:1000
  tic
  % read a depth image
  depth_msg = sensor_msgs_Image('read',sid2,50000);

  % check if it's empty before we use it
  if(~isempty(depth_msg))

    % convert from signed to unsigned 
    depth_msg.data(depth_msg.data < 0) = depth_msg.data(depth_msg.data < 0) + 256;

    % depth is represented by 16-bit values but we receive 8-bit values
    % we first have to concatenate every adjacent pair of 8-bit numbers
    % beware of proper endianness (I think this is correct)
    c = bitor(bitshift(bitor(uint16(0),uint16(depth_msg.data(2:2:end))),8),uint16(depth_msg.data(1:2:end)));

    % convert the 1D vector into a 2D matrix
    c = reshape(c,640,480);

    % flip it horizontally about the center and then rotate the image
    c = fliplr(imrotate(c,-90));

    % let's see what ya got
    subplot(2,1,1);
    imagesc(c);title('Kinect: Depth','FontSize',14,'FontWeight','Bold');
    axis image;drawnow;
  else
    fprintf('depth is empty\n');
  end

  % receive an RGB image
  rgb_msg = sensor_msgs_Image('read',sid, 50000);

  % always check to see we actually received a message
  if(~isempty(rgb_msg))

    % convert from signed to unsigned
    rgb_msg.data(rgb_msg.data < 0) = rgb_msg.data(rgb_msg.data < 0) + 256;

    % scale it to 0.0<->1.0 to make matlab happy
    a = rgb_msg.data ./ 255;

    % convert 1D R,G,B, vectors into 2D image planes
    % flip them horizontally about the center
    a_r = fliplr(reshape(a(1:3:end),640,480));
    a_g = fliplr(reshape(a(2:3:end),640,480));
    a_b = fliplr(reshape(a(3:3:end),640,480));

    % glue the image planes together to make an RGB image
    a_r(:,:,2) = a_g;
    a_r(:,:,3) = a_b;

    % rotate the image 90 degrees
    a_r = imrotate(a_r,90);

    % let's see what ya got
    subplot(2,1,2);
    imagesc(a_r); title('Kinect: RGB','FontSize',14,'FontWeight','Bold');
    axis image; drawnow;
  else
    fprintf('rgb is empty\n');
  end

  toc
end

Written by Ben Cohen

Find It