Introduction

This project is an attempt to implement a 3D visualization of simultaneous localization and mapping (SLAM) based on raw sensor data provided by the Player robotics framework.

This project is useful for both research and application purposes. Visualization of SLAM in research can help robotists better debug their robots. It can also provide much more information for remote robot operation where the robot can be controlled based not only on vision from the cameras, but also on 3D terrain mapped using robot sensors. Furthermore, by overlaying camera frames as texture onto the built 3D terrain, a precise model of the environment physically and visually can be built and saved.

Approach
The project uses player/stage as the robotics interface because player/stage offers a fantastic hardware abstraction layer for robotics programming for various robot hardware platforms. One can also easily build drivers for player so that their customized robots can be interfaced through player as well. SLAM Visualization for Player means the visualizer will work with any robot that provides the player interface with zero configuration.

The project is built using C++ on Qt4 which is a cross platform rich user interface library that provides a wrapper for OpenGL. Using Qt, the visualizer is able to utilize some of the powerful tools like threading, windowing and the signal/slot mechanism.

Technical Details
The structure of the program is fairly simple. It consists of the following components:

• Map: an object that contains occupancy grid map as well as a 3D texture map for image data overlay
• Robot: an object that uses player client to gather information from the player server, and plots the laser data and camera frame onto the map
• GLWidget: is a GUI that displays the robot and the map using OpenGL

When mapping laser data, probabilistic mapping technique is used. Basically, it updates each occupancy grid cell when new evidence about an obstacle or open space is gathered. So the probability of an obstacle increases or decreases as the robot continuously scan the area. The mapping of laser data also uses a probability model for each laser beam such that it simulates the sensor error in the laser range finder and try to minimize it using the probabilistic mapping technique.

When mapping camera frame, 3D texture were built to hold pixel level data. Some trigonometry calculation were performed to project the frame onto the 3D model. It works like a reverse ray tracer where the pixel color on the image in 2D determines the pixel color in corresponding 3D texture space.

The map has the ability to save into files and load from files. Such abilities give the visualizer the mean to playback recording or continue exploration based on previous recordings. Map is automatically saved when the visualizer window is closed. Though Ctrl+C on the command line will terminate the program with data loss.

Finally, the GLWidget provides complete control of the camera in the 3D scene. There are two modes for the GUI. When started up, the mode defaults to robot following where the camera will following the robot’s movement. Using double click, one can toggle the robot following mode. When it is off, one can move around the mapped environment freely. Holding the left mouse button and drag will move the x, y coordinate that the OpenGL camera is looking at. Holding the right mouse button and drag will adjust the yaw and pitch of the OpenGL camera. Scrolling using the mouse wheel will zoom in and out of the scene.

Results
The resulting visualizer program works amazingly. One can watch the 3D probabilistic map being built as the color of the portion indicate its certainty. As the robot travels on the map, red is 100% free space while blue shows 100% obstacle.

From an overhead view

From a robot view

From a bird’s eye view

If a camera is present, not only does the visualizer plots the physically map of the environment, it also overlay camera frame data onto the correct physically location, providing a complete 3D model of the environment.

3rd floor of building #9, the electrical engineering department

4th floor of the new extension of building #9, the senior design center and the robotics club

A closer look at the 4th floor of the building #9 new extension.

3rd floor of the new extension of building #9, the computer engineering department

The screenshots above were taken from playback recording. The recordings were made using a real robot called AMOS III. These recordings were provided at http://drop.io/ghszhux. More screenshots can be found at http://picasaweb.google.com/ziyan.info/ZiyanSBlog.

Future Enhancements
There are a number of things that I plan to work on when I have spare time:

• Multi-robot: supports multiple robot working on the same map. This is fairly easy on the mapping side. On the display side, I need to find a way for user to specify which robot the OpenGL camera should follow
• Multi-camera: supports multiple camera per robot. This is also in fact very easy. Instead of calling mapFrame once every cycle, just call it twice with different camera geometry and frame data.
• Sensor location configuration: provide a way to adjust the sensor location relative to the robot center. Right now, camera geometry is hard coded.
• Visualize Path Planning: using player’s planner interface, I should be able to also plot planned path for the robot on the GUI. Then again, this is tricky for multi-robot.
• Visualize other debugging information: the visualizer should be extended to include raw camera viewer, positioning data, system health data, etc. These could just be text displays.
• Image Distortion Problem: At the moment, we average over a number of frames to get mapping of textures. This methods blurs the walls because of motion blurs and asynchronized sensor data. I need to find a better way to deal with these problems.
• Improve reverse ray tracing: the way camera frames are being mapped onto the 3D texture does not work perfectly. Some improvement may be made around that area.

Source
This project is available as open source freeware under GPL. The source code is maintained at http://svn.ziyan.info/svn/default/trunk/slam

BROWSE / IN TIMELINE
« Mobile Robot Programming Project
» Starting at Microsoft

BROWSE / IN SAME CATEGORY English
« Mobile Robot Programming Project
» Starting at Microsoft