sonar_camera_reconstruction

This repo contains the code derived from the paper "Opti-Acoustic Scene Reconstruction in Highly Turbid Underwater Environments" (2025), which presents an imaging sonar and monocular camera merging system for scene reconstruction.
Paper (arXiv)

You are viewing the ROS 1 version of this code, if you are looking for a ROS2 version of this code it can be found here.

Dependencies

This codebase is ROS native and will require a ROS installation. It can be used without ROS, but will require some work.

- ROS Noetic
- Python3

Dependencies:

    sudo pip install catkin_tools scipy open3d gtsam
    sudo apt-get install ros-noetic-pybind11-catkin

    sudo apt-get -y install python3-dev python3-pip
    sudo pip install  mavproxy pymavlink cv2
    
    # install ROS dependencies
    sudo apt-get install ros-noetic-joy ros-noetic-cv-bridge ros-noetic-nav-core ros-noetic-cv-bridge ros-noetic-tf2-geometry-msgs

Presently, this codebase uses our sonar image system in Argonaut https://github.com/jake3991/Argonaut.git.

Set Up

    mkdir -p catkin_ws/src
    cd catkin_ws/src
    git clone https://github.com/ethz-asl/libpointmatcher.git
    git clone https://github.com/ethz-asl/libnabo.git  
    git clone https://github.com/jake3991/Argonaut.git
    git clone https://github.com/ivanacollg/sonar_camera_reconstruction.git
    cd ..
    catkin build
    source devel/setup.bash

Running Code

    roslaunch sonar_camera_reconstruction merge.launch

Download sample data

    rosbag play sample.bag --clock

Data sets

This dataset folder constains all the data used for the paper "Opti-Acoustic Scene Reconstruction in Highly Turbid Underwater Environments" (2025), which presents an imaging sonar and monocular camera merging system for scene reconstruction (Paper (arXiv)).

Each folder contains data from each scenario shown in the paper:

• tank_piers
• tank_sea_wall
• marina_pier
• marina_sea_wall

Additionally, tank scenarious include emulated turbidity examples types 5C, 7C, and 9C. STL files of ground truth structes are likewise included.

Each folder contains original ROS1 .bag data and converted data to ROS2 folder.

Running code for different scenarious

Different scenarious contain slightly different parameters for sonar range, monocular camera calibration, etc.

Change the environment argument in the merge.launch file to launch different parameters:

• marina_pier (default)
• marina_seawall
• tank (for all tank tests)

Example:

    roslaunch sonar_camera_reconstruction merge.launch environment:=tank

Documentation

Subscriber Topics:

• Camera image topic:

  • Default Name: /camera/image_raw/compressed
  • Type: sensor_msgs/msg/CompressedImage

• Sonar topic:

  • Default Name: /sonar_oculus_node/M750d/ping
  • Type: sonar_oculus/msg/OculusPing

• Robot Odometry topic:

  • Default Name: /bruce/slam/localization/odom
  • Type: nav_msgs/msg/Odometry

Publisher Topics:

• Segmented Image topic

  • Default Name: /sonar_camera_reconstruction/segmented_img/compressed
  • Type: sensor_msgs/msg/CompressedImage

• Sonar features Image topic

  • Default Name: /sonar_camera_reconstruction/feature_img/compressed
  • Type: sensor_msgs/msg/CompressedImage

• Point Cloud reconstruction

  • Default Name: /sonar_camera_reconstruction/cloud
  • Type: sensor_msgs/msg/PointCloud2

Parameter Files:

Monocular Camera parameters

• image_width: width of the image in pixels

• image_height: height of the image in pixels

• camera_matrix/data:

  • fx - focal length in pixels along the x axis
  • fy - focal length in pixels along the y axis
  • s - skew term (normally 0 unless your camera pixels are not perfectly rectangular)
  • (cx, cy) - coordinates of the principle point (optical center) in pixels
  • Complete camera matrix:

    [fx,  s,  cx,  
      0, fy,  cy,  
      0,  0,   1 ]
    

• distortion_coefficients/data: by default uses the plumb bob model, can be found through camera calibration

• Ts_c - Transformation Matrix from sonar to camera frame:

  • contains a rotation matrix R, a transformation matrix T, and a 4th row to make it homogeneous (never changes)
  • R is a 3x3 matrix with each column denoting the camera's x, y, and z axis directions in relation to the sonar axes in unit vector form.
  • T is a 1x3 matrix with each row denoting the x, y, and z coordinates of the origin of the camera frame in relation to the origin of the sonar frame in meters.
  • Complete transformation matrix:

     [Rxx, Ryx, Rzx, Tx,
      Rxy, Ryy, Rzy, Ty,
      Rxz, Ryz, Rzz, Tz,
       0,   0,   0,  1,]
    

Sonar parameters

• sonarRange: The maximum detection range in meters

• thresholdHorizontal: detection strength threshold for the horizontal sonars CFAR processing, higher thresholds remove both noise as well as weaker targets in favor of fewer, stronger targets

• verticalAperture: The vertical angular coverage in degrees

• sonar_features: True, publishe sonar feature images

• CFAR/Ntc: number of training cells

• CFAR/Ngc: number of guard cells

• CFAR/Pfa: false alarm rate

• CFAR/rank: matrix rank

Merge parameters

• min_area: minimum # of pixels in the image to be considered contact
• threshold_inv: True, boolean to choose whether to invert the threshold to look for dark or light areas in the image (Dark = True, Light = False)
• boundary: # of pixels in the boundary to not be included in the segmentation
• fast_performance: True, code will run faster but become slightly less precise

Citations

If you use this repo or any of the data provided please cite the following work:

@INPROCEEDINGS{11247733,
  author={Collado-Gonzalez, Ivana and McConnell, John and Szenher, Paul and Englot, Brendan},
  booktitle={2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 
  title={Opti-Acoustic Scene Reconstruction in Highly Turbid Underwater Environments}, 
  year={2025},
  volume={},
  number={},
  pages={1282-1289},
  keywords={Training;Visualization;Sonar;Turbidity;Object segmentation;Reconstruction algorithms;Sonar navigation;Real-time systems;Reproducibility of results;Image reconstruction},
  doi={10.1109/IROS60139.2025.11247733}}