Underwater Stereo Vision and vSLAM Support for the Stingray Platform in Parallel with Code Base Porting Julian Fessard, Nathan Manning, Trevor Lang, Chad Morse, Maximilian Wachtveitl, Qihua (Norman) Wu, Associate Professor Ryan Kastner University of California San Diego, San Diego, California, USA

Abstract Autonomous mobile robotics is a rapidly growing field that encompasses a broad variety of applications ranging from planetary expedition to underwater hull inspection. Yet despite these vast differences in environment and function all autonomous systems share one very fundamental challenge- Simultaneous Localization and Mapping (SLAM). That is to say, when introduced into an unknown environment all truly autonomous systems must be able to map that space and reliably localize themselves within it without human assistance. To that end we propose augmenting the navigation capabilities of the existing underwater Stingray platform with visual SLAM techniques achieved through stereo cameras running on the ROS visualization stack.

1. Introduction The origins of SLAM began over 20 years ago at an IEEE conference as informal discussions between numerous researchers. Over the following 10 years those concepts evolved into the widespread formal approach that it is today. Unlike the work that preceded it, SLAM solves the fundamental concepts of localization and mapping by joining them into a single estimation problem, and uses the correlation of cues to its advantage rather than trying to minimize it. Although SLAM methods have been widely vetted both theoretically and conceptually the practical aspects of their implementation still challenge researchers today. Everything from processor speeds and power budgets to types of sensors and their resolution come into play when implementing SLAM. Trends in both processors and sensors have eased the burden of SLAM implementation with increased capabilities and smaller form factors at lower costs, but unfortunately this has not benefited all autonomous vehicles equally. For example Micro Aerial Vehicles (MAVs) with their limited payloads and highly dynamic movements benefit greatly from smaller, faster processing and sensing hardware. Autonomous Underwater Vehicles (AUVs) on the other hand don’t typically suffer the same payload restrictions and instead are limited greatly by the inability to use GPS and other RF/IR sensing for navigation- something that hasn’t changed over the past 20 years and won’t likely change in the next 20.

Unlike MAV designers that are able to take advantage of GPS or inexpensive commercial off-the-shelf (COTS) laser rangefinders and stereo vision suites, such as the Microsoft Kinect, AUV designers are frequently forced to rely on expensive Sonar and Doppler Velocity Logs (DVLs) or inaccurate inertial measurement units (IMUs) for navigation cues. Overcoming these obstacles can cause AUVs to be large, expensive, and difficult to design, and has led researchers to explore and adopt new techniques for navigational cuing most notably vSLAM. This technique which uses visual cues from single or stereo paired cameras has several advantages over traditional cuing methods. It is typically inexpensive, small, and readily available, making it especially well suited to autonomous development platforms. Even though vSLAM has been shown to work well in numerous autonomous robotics systems, including MITs quadcopter[4] and the AQUA underwater robot [6], most of its use has been largely ad hoc or closed source making it difficult to incorporate into new designs and thereby slowing development. As we found out through our own personal experience, time is easily wasted building vSLAM support from the ground up or modifying existing code to match legacy systems. Frameworks such as ROS (Robot Operating System) seek to combat these drawbacks by creating a standardized framework built on common languages, but unfortunately at the time this paper was written ROS had only minimal support for underwater systems and none for underwater vSLAM. Our goal is to add this support to by porting existing the Stingray code into ROS and augmenting it with inexpensive COTS cameras running in ROS's vSLAM and rviz stacks. The next section of this paper provides a description of Stingray at both the hardware and software level. Section 3 provides the methods used to incorporate vSLAM into Stingray project. This is followed by our observations and experiences in section 4. We finish with a brief summary of related work in section 5, and our conclusions in section 6. Lastly, since this is a project based paper for class, you can find a task listing for each of the team members in Appendix A detailing what each of us has done to get the project in its current state.

2. Description Due to the complications involved with underwater

prototyping we chose to develop on a desktop platform prior to deploying on the Stingray. This allowed us to speed up development and focus solely on software development rather than hardware issues surrounding underwater environments. However, what we provide below is the description of the Stingray hardware itself since this will be the final platform that all of our code runs on. Stingray Hardware: • Controls Computer- Kontron ETX-DC with Intel Atom N270 • Vision Computer- Kontron ETXexpress-AI with Intel i7 • Digital Compass- OS5000-USD • Cameras- Allied Vision Prosilica GC750 (x3) two front-facing, one down-facing • Batteries- Thunder Power RC G4 Pro Power 45C • Hydrophones- Aquarian H1a • Depth Sensor- SSI part P51-15-S-B-I36-4.5V-R Stingray Software: • OS- Ubuntu 10.04 • ROS- Diamondback open source framework with prosilica drivers, vSLAM stack, and rviz stack • Stingray- Navigation, Vision, Propulsion all in different phases of transfer into ROS • SampleViewer- Prosilica camera settings software In summary the Stingray is an inexpensive, highly modular design, that can be easily developed and deployed by minimal teams. More information can be found at the project website: http://www.sdibotics.org/.

3. Method Our efforts began as a modest calibration and integration effort. It was believed based on the information available on the ROS wiki that we were in possession of all the necessary software and hardware needed to calibrate our stereo camera setup and provide the proper data to ROS's vSLAM stack in order to process stereo vision. However, after familiarizing ourselves with ROS and Stingray we quickly realized that camera calibration as implemented by ROS was not working as expected for our setup. Despite its ability to find and track the checkered design of our calibration board, ROS's calibration scripts were resulting in epipolar geometry that was outside of any acceptable range. Initially we thought this was because we were using too small of a calibration pattern. So larger patterns were printed and calibration was repeated. Although this allowed the calibration algorithms to find and track the pattern more easily and therefore report that calibration was completing more quickly, it still was not sufficient enough to produce reasonable epipolar geometry and therefore valid calibrations. In fact the calibration tool

itself has a slider in the user interface that is supposed to scale the processed images from the cameras. When zoomed out, the curved borders of a rectified image should appear, but instead what we observed was that the slider had no effect and the output image scale at all. Instead it slid a fixed window across what appeared to be an overly rectified image. Even though the ROS calibration software appears to have an obvious bug (or at the very least doesn't support the hardware as advertised) we were unable to find or monitor any logging or debugging messages during calibration that provided any useful information. Since the software wasn't reporting any failures we attempted running the entire vSLAM stack after each calibration. Only then did we see vague system logs describing a lack of synchronization between frames from the left camera, right camera, and disparity (which is produced from the left and right streams by the ROS stereo processing node). We took this to mean that the two stereo cameras needed to be more tightly synchronized and began adapting the hardware setup to support this requirement. We wired each of our two cameras into a master/slave configuration, and used the capture logic of the master as an output to trigger a capture logic of the slave. This was done so that no external signaling is needed to operate the cameras synchronously. We implemented triggering logic between cameras by attaching the SyncOut2 output of the master to the SyncIn2 input of the slave using pigtailed cabling. Here SyncOut2 and SyncIn2 are just terms used in the Prosilica users manual to refer to low noise triggering I/O. Finally, the cameras needed to be configured to produce the SyncOut2 on the master for each capture logic transition, and set the slave to capture on each transition seen on its SyncIn2. This was accomplished using the SampleViewer setup software provided by Prosilica, but we quickly found that ROS only supports streaming and polling modes of operation. Worse, ROS was overwriting some of the settings set by SampleViewer, breaking the correct synchronization configuration. At this point the initial integration and calibration effort greatly expanded to include ROS system development and testing to support synchronized video. As it turns out many of the functions of the Prosilica cameras are unimplemented in ROS. There are numerous “TODO” comments on many of the functions we are interested in, and so we are forced to implement them ourselves. Given the size, complexity, and lack of documentation of ROS this has been exceptionally challenging. In addition we needed to add the ability to dynamically change each of the additional settings we implemented because ROS overwrites them on each execution. At this point we have added support for fixed frame rate

video and full master/slave capture logic triggering, and with the addition of these features have been able to produce viable camera calibrations to the best of our knowledge. If camera calibration is performed while the cameras are run using master/slave capture logic triggering the same checkerboard pattern is easily tracked and produces acceptable epipolar geometry results. In addition to fixed frame rates and synchronous triggering, we disable automatic exposure and automatic gain to improve the frame rate stability and image consistency on both cameras. We have experimentally found good values for exposure that maintain desired frame rates while allowing reasonable image brightness, and are currently trying to experiment with video pixel formatting (Bayer8 to RGB16, etc) to see if it has any influence on the results. With these step taken, the scaling slider in the calibration user interface that was previously broken now scales the images to show the bowed edges resulting from image rectification as seen in Figure 1 below.

or feedback from the users or creators.

4. Observations Although we were not able to reproduce the same results the creators of ROS have been able to achieve stereo vision using high end Prosilica cameras running on the released ROS code. This leaves us wondering if synchronization is in fact required at all. Either way it was the only way we were able to produce valid calibrations. Once we discover why ROS fails to produce disparity maps from the video streams we are providing, we have reason to believe that the remainder of the integration effort should go smoothly. We have verified that, given valid data (provided by willow garage, the creators of ROS), the vSLAM stack correctly produces localization and mapping as well as point clouds from visual cues.

5. Related Work In 2007 researchers from the PeRL lab at the University of Michigan developed an underwater testbed and outlined their work in [3]. This testbed was created to assist them in 3 major areas of research, “(1) real-time visual simultaneous localization and mapping (SLAM); (2) cooperative multi-vehicle navigation; and (3) perceptiondriven control”. Although the paper discusses each these research areas we are only interested in the first as the others to not pertain to our work.

Figure 1: Viable Configuration Using Synchronized Capture Logic Despite these advances in camera calibration we are still unable to produce the disparity maps needed for stereo vision, and do not know at this point why ROS's stereo processing software is failing, as everything seems to be correct and working well. Once we verify that our calibrations can produce viable disparity and therefore stereo vision, we plan to order a print of our calibration pattern on a dibond substrate in order to improve calibration efficiency and reliability. This will have the added benefit of allowing underwater calibration where the current board would be destroyed by water. Despite our best efforts at seeking guidance from the ROS community and forums, we have yet to receive any support

Their paper also discusses the “Two broad categories of underwater navigation methods” (absolute positioning using long-baseline method and relative positioning using dead-reckoning) and gives a good summary of how to pair dead-reckoning (using a DVL, Doppler Velocity Log, in their platform) with landmark localization to produce fast, precise, navigation with (most importantly) bounded error. The authors extend this concept to include creating an “optically-derived bathymetry map” from their visual landmarks. In addition to these fundamental topics of underwater navigation the paper has in-depth descriptions of the design and what modifications were made to off the shelf hardware. Lastly the paper discusses the many locations that the systems was tested, ranging from controlled laboratories to National Marine Sanctuary and Underwater Preserve in Thunder Bay. That aspect that makes this paper most interesting to us is that, unlike many other robotics systems, the PeRL testbed had a purely underwater focus and dealt with many of the same issues that we’ll be facing in our work with Stingray. Because underwater environments are more restrictive in many ways than non-underwater environments it is possible to survey current trends in terrestrial and aerial robotics research and to find cues of where future

underwater research is headed. It is for this reason that we looked at the latest work with visual SLAM in GPS restricted environments being carried out at MIT. Achtelik et al. [4] have been performing cutting visual SLAM research at MIT pairing laser rangefinders and stereo cameras. Their paper provides a short description of Micro Air Vehicles (MAVs) as well as some of the limitations/challenges unique that platform including limited payloads for computation, fast dynamics, and more. This is followed by the team’s design decisions and how they overcome each of the challenges of the platform. Along with this they describe the advantages of pairing laser rangefinders with stereo cameras focusing on the fact that each system provides complementary clues for odometry. This is followed by a description of the actual sensor hierarchy and how the architecture is setup to support it. There are 3 tiers of sensors, 2 real-time, and 1 non-real-time. The paper finishes with the description and explanation of their stereo camera visual odometry algorithm and the experiments and results performed with their quadcopter.

6. Conclusion vSLAM is a very promising way to provide increasingly affordable and accurate localization and mapping to all types of autonomous robotics even those inhibited by underwater environments. Although we have yet to produced our own functioning underwater vSLAM system we have be able to further the ROS support needed to do so. We have managed to solve great problems but sadly the unforeseen aspects of the project have hindered our ability to do as much as we had hoped. In addition (although we didn't discuss it much in this version of the report) we are making progress in porting the legacy Stingray code into ROS so that future development can be performed more easily. This includes the algorithms responsible for detecting buoys and pipes for the underwater competition the Stingray partakes in each year.

References [1] ROS Robot Operating System, Willow Garage Inc. www.ros.org [2] T. Bailey and H. Durrant-Whyte, “Simultaneous Localization and Mapping: Part 1” , “Simultaneous Localization and Mapping: Part 2 State of the Art”, IEEE Robotics & Automation Magazine, June 2006 [3] H. C. Brown, A. Kim, and R. M. Eustice, “An overview of autonomous underwater vehicle research and testbed at PeRL,” Marine Technology Society Journal, 2009. [4] Markus Achtelik, Abraham Bachrach, Ruijie He, Samuel Prentice, and Nicholas Roy. “Stereo vision and laser odometry for autonomous helicopters in gps-denied indoor environments”. In Proceedings of the SPIE Conference on Unmanned Systems Technology XI, Orlando, FL, 2009. [5] A. J. Davison, Y. González Cid and N. Kita - “Real-time 3D SLAM with wide-angle vision” Proc. IFAC Symposium on Intelligent Autonomous Vehicles, 2004 http://www.robots.ox.ac.uk/~lav/Papers/davison_etal_iav2004/davison_etal_iav2004.html [6] G Dudek, P Giguere, C Prahacs, S Saunderson, J Sattar and L Torres-Mendez, McGill University. M Jenkin, A German, A Hogue, A Ripsman, and J Zacher, York University. E Milios, H Liu, and P Zhang, Dalhousie University. M Buehler, Boston Dynamics. C Georgiades, MathWorks. - “AQUA: An Amphibious Autonomous Robot”, IEEE Computer magazine, January 2007 [7] Juan Manuel Saez, Andrew Hogue, Francisco Escolano and Michael Jenkin, “Underwater 3D SLAM through Entropy Minimization”, Proceedings of the 2006 IEEE International Conference on Robotics and Automation Orlando, Florida May 2006

Appendix A (Individual Completed Tasks) Nathan •

Performed literature survey, prepared oral report and other class assignments

•

Debugged camera calibration process and found errors in existing calibrations

•

Acquired and tested multiple new calibration boards

•

Used pigtailed cabling to achieve triggering for synced image pairs

•

Implemented fixed rate triggering

•

Incorporated fixed rate triggering in the dynamic reconfiguration GUI

•

Implemented SyncOut2 triggering to provide more closely synchronized image pairs

•

Incorporated SyncOut2 triggering in the dynamic reconfiguration GUI

•

Managed group meetings

•

Assigned tasks to undergraduate team members

•

Performed market survey and ordered a new computer for the lab

•

Performed market survey and found a dibond printer and distributor for final calibration board

Julian •

Surveyed the existing work on underwater vehicles, vSLAM and related works

•

Worked on the various assignments

•

Synced the cameras and specified the correct settings for the trigger to work, both trigger settings and manual frame rate

•

modified the launch files for manual exposure and gain and the correct values

•

Helped the undergrads by doing a svn demo, scripting and gdb for debugging

•

General undergrad help and pointed them in the right direction

•

Helped Nathan in the implementation work

Trevor •

Created skeleton 2 publisher/1 subscriber node

•

Analyzed original stingray code

•

Created XML launch files for Prosilica cameras, image processing and image view nodes

•

Helped create visionPort node and parameters

•

Helped create dynamic reconfiguration for visionPort node

•

Researched converting RGB image stream to HSV image stream

•

Ported find_buoy code into ROS

•

Debugged Prosilica camera calibration

•

Demoed proof of concept for ROS nodes

•

Gave project overview presentation to cse145 class

Norman •

built one node to communicate with two camera nodes.

•

Made the stereo view work by changing the launch files

•

Transformed part of the find_pipe code into ROS system

•

Established conversion from RGB node subscription to HSV output in ROS for bag files and prosilica_camera nodes.

•

Debugged the problem for reconfiguration and make it work for the camera choosing.

•

We have managed to also extract a video feed from one of the cameras of our choice and convert this stream into the YUV colorspace in order to use it with the bouy-detection algorithm used by the normal Stingray codebase.

Maximilian •

Calibrate cameras

•

Implement cameras in the vslam launch file

•

Setting start properties in launch files.

•

Implemented fixed frame rate speed selection in reconfigure gui.

•

Debugging the problem with the disparity view.(but i ran out of ideas)

Chad Morse •

Created and maintained launch files in order to debug viewStereo process chain for disparity image production.

•

Created and maintained launch files in order to debug vSLAM system for pointcloud image production.

•

Debugged Dynamic Reconfiguration process for visionPort view selection system.

•

Implemented Stingray algorithms on node subscription from Procillica Cameras in order to port Stingray code to the ROS framework.

•

Assisted in Calibration debug process for stereo cameras.

•

Established local system environment for stingray and ROS usage.