Receive and display Kinect RGB and depth image from ROS using Matlab, rosjava and matlab_bridge interface

I was looking for a smart way (without having to compile much stuff on my own) on how to communicate with ROS from Matlab. I wanted to receive color and depth images published by a ROS node for the Microsoft Kinect camera and Display the Images on a Matlab GUI.

How-i-fixed-it:
Me and Google worked out together, how to get it done.

I used the ROS-Matlab Bridge created by Tingfan Wu at UCSD, which is based on rosjava. It can be downloaded under: https://code.google.com/p/mplab-ros-pkg/wiki/java_matlab_bridge.
The setup is described on the project page.

Next i did a svn checkout of the repository http://mplab-ros-pkg.googlecode.com/svn/trunk
The trunk has some updated files compared to the zip download dated 2012-10-23.

The project can either be used to run your own ROS master locally or to connect to another ROS master somewhere on the network. The HelloWorld.m example script is the point to start to make yourself familiar with the functionality of the software.

Based on this examples, i created my own script to subscribe to a kinect Images Publisher on my local network. I subsribed to the topics '/camera/rgb/image_color' and '/camera/depth/image_rect'.

I found the right hints on how to convert the sensor_msgs/Image message data to MATLAB Images here: https://alliance.seas.upenn.edu/~meam620/wiki/index.php?n=Roslab.KinectMatlab, written by Ben Cohen. My implementation is a little different to theirs, but it was a good point to start.

Here's my final result:

 

Here's the MATLAB script:

 

%Init rosjava first

jmb_init();

 

%MASTER_URI='http://localhost:11311';

%Set URI of the ROS master

 

 

MASTER_URI='http://i60p23:11311';

 

 

 

%Set the name of your node

 

 

NODE_NAME='Kinect_Listener';

 

 

 

%Init the node

 

 

node=jmb_init_node(NODE_NAME, MASTER_URI);

 

%Setup a logger

 

 

logger=node.getLog();

 

%Subscribe to the RGB and depth messages

 

 

sub_rgb=edu.ucsd.SubscriberAdapter(node,'/camera/rgb/image_color','sensor_msgs/Image');

sub_depth=edu.ucsd.SubscriberAdapter(node,'/camera/depth/image_rect','sensor_msgs/Image');

 

 

 

%Set the read timeout

 

 

timeout=5;

 

%Setup a figure to draw onto

 

 

close all;

 

 

rect = [100, 100, 320, 500]

 

figure('Position',rect);

 

 

 

%Two variables to calculate the fps

 

 

lastTime = 0;

fps = 0;

 

%Get 500 frames and draw the images on the figure

 

 

for frame = 1:500

 

 

 

%Get the RGB image

 

 

msg=sub_rgb.takeMessage(timeout);

 

if isempty(msg)

logger.warn('timeout');

else

%Read the timestamp

 

 

timeStamp = msg.header.stamp.toSeconds;

 

%logger.info(sprintf('I got rgb image %dx%d', msg.width, msg.height));

%Calculate the fps for frames > 0

if(frame == 1)

 

 

lastTime = timeStamp;

 

else

 

 

fps = 1/(timeStamp-lastTime);

lastTime = timeStamp;

 

end

 

 

 

%Convert data type

data = typecast(msg.data,'uint8');

 

 

 

%Re-Order the data into a matlab rgb array. The raw data has the

%format RGBRGBRGB.... beginning with the upper left image pixel

%(0,0). This format has to be converted to matlab format

 

 

img_rgb(:,:,1) = reshape(data(1:3:end),640,480);

img_rgb(:,:,2) = reshape(data(2:3:end),640,480);

img_rgb(:,:,3) = reshape(data(3:3:end),640,480);

 

%Flip the values

 

 

img_rgb = flipdim(img_rgb,2);

 

%Rotate the image 90 degrees

 

 

img_rgb_rot = imrotate(img_rgb,90);

 

%Plot the image

 

 

subplot(2,1,1);

 

imshow(img_rgb_rot); title(sprintf('Kinect: RGB (%d@%2.1f fps)',frame,fps),'FontSize',14,'FontWeight','Bold');

 

 

drawnow;

 

end

 

 

 

%Get the depth image

 

 

msg=sub_depth.takeMessage(timeout);

 

if isempty(msg)

logger.warn('timeout');

else

 

 

 

%Re-Order the data into a matlab rgb array. The raw data has the

%format RGBRGBRGB.... beginning with the upper left image pixel

%(0,0). This format has to be converted to matlab format

img_depth = reshape(typecast(msg.data,'single'), msg.width, msg.height);

 

 

 

%Flip the values

 

 

img_depth = flipdim(img_depth,2);

 

%Rotate the image 90 degrees

 

 

img_depth_rot = imrotate(img_depth,90);

 

%Plot the image

 

 

subplot(2,1,2);

 

imagesc(img_depth_rot./max(img_depth_rot(:))); title(sprintf('Kinect: Depth'),'FontSize',14,'FontWeight','Bold');

 

 

drawnow;

 

end

 

 

 

end

 

 

node.shutdown()

 

 



Some more info and connecting ROS and MATLAB can be found here: http://www.ros.org/wiki/groovy/Planning/Matlab

My System configuration:
Windows 8 Enterprise 64 Bit
Matlab 2013a 64 Bit
guava-13.0.1.jar