├── .drone.yml ├── .gitignore ├── .pre-commit-config.yaml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── drawn_sonar.png ├── drawn_sonar_rect.png ├── fips ├── fips.yml ├── helpers ├── epilogue.txt ├── inferno_color_map.csv ├── make_color_map.py └── prologue.txt ├── include └── sonar_image_proc │ ├── AbstractSonarInterface.h │ ├── ColorMaps.h │ ├── DataStructures.h │ ├── DrawSonar.h │ ├── HistogramGenerator.h │ ├── OverlayImage.h │ └── SonarDrawer.h ├── launch └── sonar_postproc.launch ├── lib ├── AbstractSonarInterface.cpp ├── ColorMaps.cpp ├── HistogramGenerator.cpp ├── OldDrawSonar.cpp └── SonarDrawer.cpp ├── nodelet_plugins.xml ├── package.xml ├── python ├── .gitignore ├── README.md ├── draw_sonar │ └── __init__.py ├── histogram_drawer ├── ndarray_converter.cpp ├── ndarray_converter.h ├── old_setup.py ├── py_draw_sonar.cpp ├── sonar_image_proc │ ├── __init__.py │ └── sonar_msg_metadata.py └── test.py ├── ros ├── cfg │ └── DrawSonar.cfg ├── include │ └── sonar_image_proc │ │ └── sonar_image_msg_interface.h ├── src │ ├── draw_sonar_node.cpp │ ├── draw_sonar_nodelet.cpp │ ├── sonar_postprocessor_node.cpp │ └── sonar_postprocessor_nodelet.cpp └── tools │ └── bag2sonar.cpp ├── scripts ├── sonar_fov.py └── sonar_pointcloud.py ├── setup.py ├── sonar_image_proc.rosinstall └── test └── unit └── test_draw_sonar.cpp /.drone.yml: -------------------------------------------------------------------------------- 1 | kind: pipeline 2 | type: docker 3 | name: build 4 | 5 | platform: 6 | os: linux 7 | arch: amd64 8 | 9 | steps: 10 | ## Biuld in fips (debug with unittesting) 11 | - name: build-fips-unittest 12 | image: amarburg/lsdslam-dev-host 13 | commands: 14 | - ./fips set config linux-make-unittest 15 | - ./fips build 16 | 17 | ## Build in ros melodic 18 | # - name: build-ros-melodic 19 | # image: amarburg/drone-ci-ros-melodic:latest 20 | # commands: 21 | # - wget -O- https://gitlab.com/amarburg/ros_drone_ci/-/raw/master/bootstrap.sh | /bin/bash 22 | # environment: 23 | # WSTOOL_RECURSIVE: true 24 | # ROS_PACKAGES_TO_INSTALL: cv-bridge image-transport 25 | 26 | ## Build in ros noetic 27 | - name: build-ros-noetic 28 | image: amarburg/drone-ci-ros-noetic:latest 29 | commands: 30 | - wget -O- https://gitlab.com/amarburg/ros_drone_ci/-/raw/master/bootstrap.sh | /bin/bash 31 | environment: 32 | WSTOOL_RECURSIVE: true 33 | ROS_PACKAGES_TO_INSTALL: cv-bridge image-transport 34 | 35 | ## Trigger downstream builds on Github 36 | # - name: trigger-github-downstream 37 | # image: plugins/downstream 38 | # settings: 39 | # server: https://github.drone.camhd.science 40 | # fork: true 41 | # token: 42 | # from_secret: github_drone_token 43 | # repositories: 44 | # - apl-ocean-engineering/serdp_common 45 | # when: 46 | # event: 47 | # exclude: 48 | # - pull_request 49 | 50 | - name: slack 51 | image: plugins/slack 52 | settings: 53 | webhook: 54 | from_secret: slack_webhook 55 | when: 56 | status: [ success, failure ] 57 | event: 58 | exclude: 59 | - pull_request 60 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | 3 | *.mov 4 | 5 | # Compiled Object files 6 | *.slo 7 | *.lo 8 | *.o 9 | *.obj 10 | 11 | # Precompiled Headers 12 | *.gch 13 | *.pch 14 | 15 | # Compiled Dynamic libraries 16 | *.so 17 | *.dylib 18 | *.dll 19 | 20 | # Fortran module files 21 | *.mod 22 | *.smod 23 | 24 | # Compiled Static libraries 25 | *.lai 26 | *.la 27 | *.a 28 | *.lib 29 | 30 | # Executables 31 | *.exe 32 | *.out 33 | *.app 34 | 35 | .fips-* 36 | .history/* 37 | .gitignore 38 | python/sonar_image_proc/__pycache__/* 39 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # See https://pre-commit.com for more information 2 | # See https://pre-commit.com/hooks.html for more hooks 3 | repos: 4 | - repo: https://github.com/pre-commit/pre-commit-hooks 5 | rev: v3.2.0 6 | hooks: 7 | - id: trailing-whitespace 8 | - id: end-of-file-fixer 9 | - id: check-yaml 10 | - id: check-json 11 | - id: check-xml 12 | - id: check-added-large-files 13 | - id: mixed-line-ending 14 | - repo: https://github.com/pre-commit/mirrors-clang-format 15 | rev: v15.0.4 16 | hooks: 17 | - id: clang-format 18 | # Respect .clang-format if it exists, otherwise use Google 19 | args: ["--fallback-style=Google"] 20 | # 21 | # Uncomment the following to enable cpplint 22 | # 23 | #- repo: https://gitlab.com/daverona/pre-commit/cpp 24 | # rev: 0.8.0 25 | # hooks: 26 | # - id: cpplint 27 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(sonar_image_proc) 3 | 4 | if(FIPS_CONFIG AND NOT FIPS_IMPORT) 5 | cmake_minimum_required(VERSION 3.5) 6 | 7 | get_filename_component(FIPS_ROOT_DIR "../fips" ABSOLUTE) 8 | include("${FIPS_ROOT_DIR}/cmake/fips.cmake") 9 | 10 | fips_setup() 11 | else() 12 | find_package(catkin QUIET) 13 | endif() 14 | 15 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 16 | 17 | # find_package(Eigen3 REQUIRED ) 18 | find_package(Boost COMPONENTS filesystem system program_options REQUIRED) 19 | 20 | # # Default OpenCV version 21 | list(APPEND OPENCV_COMPONENTS core highgui imgproc) 22 | find_package(OpenCV COMPONENTS ${OPENCV_COMPONENTS} REQUIRED) 23 | 24 | if(FIPS_CONFIG) 25 | # # Set global include paths 26 | fips_include_directories( 27 | ${CMAKE_SOURCE_DIR}/include 28 | ${OpenCV_INCLUDE_DIRS} 29 | ) 30 | 31 | # The local library 32 | fips_begin_module(sonar_image_proc) 33 | 34 | fips_src(lib *.cpp) 35 | fips_libs(${Boost_LIBRARIES} ${OpenCV_LIBS}) 36 | 37 | fips_end_module() 38 | 39 | if(NOT FIPS_IMPORT) 40 | if(FIPS_UNITTESTS) 41 | gtest_begin(sonar_image_proc) 42 | fips_src(test/unit/) 43 | fips_deps(sonar_image_proc) 44 | gtest_end() 45 | endif() 46 | endif() 47 | 48 | fips_finish() 49 | 50 | else() 51 | find_package(catkin REQUIRED 52 | marine_acoustic_msgs 53 | cv_bridge 54 | image_transport 55 | nodelet 56 | nodelet_topic_tools 57 | OpenCV 58 | rosbag_storage 59 | dynamic_reconfigure 60 | ) 61 | 62 | catkin_python_setup() 63 | 64 | # This needs to come after `catkin_python_setup` and before `catkin_package` 65 | generate_dynamic_reconfigure_options( 66 | ros/cfg/DrawSonar.cfg 67 | ) 68 | catkin_package( 69 | INCLUDE_DIRS include/ ros/include/ 70 | LIBRARIES sonar_image_proc 71 | DEPENDS OpenCV 72 | 73 | # CATKIN_DEPENDS 74 | ) 75 | 76 | include_directories( 77 | include/ 78 | ros/include/ 79 | ${catkin_INCLUDE_DIRS} 80 | ${OpenCV_INCLUDE_DIRS} 81 | ) 82 | 83 | set(sonar_image_proc_SRCS 84 | lib/AbstractSonarInterface.cpp 85 | lib/ColorMaps.cpp 86 | lib/SonarDrawer.cpp 87 | lib/OldDrawSonar.cpp 88 | lib/HistogramGenerator.cpp 89 | ) 90 | 91 | # "true" library containing core functionality 92 | add_library(sonar_image_proc ${sonar_image_proc_SRCS}) 93 | target_link_libraries(sonar_image_proc ${catkin_LIBRARIES}) 94 | set_property(TARGET sonar_image_proc PROPERTY CXX_STANDARD 14) 95 | add_dependencies(sonar_image_proc ${PROJECT_NAME}_gencfg) 96 | 97 | # Dynamically loadable libraries containing nodelets 98 | add_library(sonar_image_proc_nodelets 99 | ros/src/draw_sonar_nodelet.cpp 100 | ros/src/sonar_postprocessor_nodelet.cpp) 101 | target_link_libraries(sonar_image_proc_nodelets ${catkin_LIBRARIES} sonar_image_proc) 102 | set_property(TARGET sonar_image_proc_nodelets PROPERTY CXX_STANDARD 14) 103 | 104 | # # Standalone nodes 105 | add_executable(draw_sonar_node ros/src/draw_sonar_node.cpp) 106 | target_link_libraries(draw_sonar_node 107 | sonar_image_proc 108 | ${catkin_LIBRARIES}) 109 | add_dependencies(draw_sonar_node ${catkin_EXPORTED_TARGETS}) 110 | set_property(TARGET draw_sonar_node PROPERTY CXX_STANDARD 14) 111 | 112 | add_executable(sonar_postprocessor_node ros/src/sonar_postprocessor_node.cpp) 113 | target_link_libraries(sonar_postprocessor_node 114 | sonar_image_proc 115 | ${catkin_LIBRARIES}) 116 | add_dependencies(sonar_postprocessor_node ${catkin_EXPORTED_TARGETS}) 117 | set_property(TARGET sonar_postprocessor_node PROPERTY CXX_STANDARD 14) 118 | 119 | # bag2sonar is **not** a conventional ROS node, but a standalone 120 | # executable which uses ros_storage to read ros bags 121 | add_executable(bag2sonar ros/tools/bag2sonar.cpp) 122 | target_link_libraries(bag2sonar 123 | sonar_image_proc 124 | ${catkin_LIBRARIES}) 125 | add_dependencies(bag2sonar ${catkin_EXPORTED_TARGETS}) 126 | set_property(TARGET bag2sonar PROPERTY CXX_STANDARD 14) 127 | 128 | install(TARGETS draw_sonar_node 129 | sonar_postprocessor_node 130 | bag2sonar 131 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 132 | 133 | install(TARGETS 134 | sonar_image_proc_nodelets 135 | sonar_image_proc 136 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 137 | 138 | install(DIRECTORY launch/ 139 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 140 | 141 | # install( DIRECTORY rqt_config/ 142 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rqt_config ) 143 | install(FILES nodelet_plugins.xml 144 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 145 | 146 | # # Install headers 147 | install(DIRECTORY include/${PROJECT_NAME}/ 148 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 149 | FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" 150 | PATTERN ".git" EXCLUDE) 151 | 152 | install(DIRECTORY ros/include/${PROJECT_NAME}/ 153 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 154 | FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" 155 | PATTERN ".git" EXCLUDE) 156 | 157 | # Install python scripts and modules 158 | catkin_install_python(PROGRAMS 159 | scripts/sonar_pointcloud.py 160 | scripts/sonar_fov.py 161 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | ) 163 | 164 | # # Disable python wrapper for now ... 165 | # 166 | # # == Stuff related to the Python wrapper == 167 | # pybind_add_module(py_draw_sonar SHARED 168 | # python/py_draw_sonar.cpp 169 | # python/ndarray_converter.cpp 170 | # lib/DrawSonar.cpp 171 | # lib/DataStructures.cpp 172 | # lib/ColorMaps.cpp 173 | # ) 174 | # target_link_libraries( py_draw_sonar ${OpenCV_LIBS} draw_sonar ) 175 | # 176 | # install(TARGETS py_draw_sonar 177 | # DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION} ) #${CATKIN_PACKAGE_PYTHON_DESTINATION}) 178 | endif() 179 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018-2020 University of Washington All rights reserved. 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | 13 | 3. Neither the name of the University of Washington nor the names of its 14 | contributors may be used to endorse or promote products derived from this 15 | software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE UNIVERSITY OF WASHINGTON AND CONTRIBUTORS "AS 18 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF WASHINGTON OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 23 | GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 24 | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 26 | OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # sonar_image_proc 2 | 3 | [![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) 4 | 5 | Code to draw data from forward-looking imaging sonars. 6 | 7 | If built for ROS, it will build a node/nodelet 8 | [draw_sonar](https://github.com/apl-ocean-engineering/libdraw_sonar/tree/master/src_ros) 9 | which subscribes to an 10 | [marine_acoustic_msgs/ProjectedSonarImage](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs/msg/ProjectedSonarImage.msg) 11 | and publishes a 12 | [sensor_msgs/Image](https://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html). 13 | 14 | The core library contains no ROS dependencies, and can be linked into non-ROS applications. 15 | 16 | # ROS Interfaces (draw_sonar_node) 17 | 18 | `rosrun sonar_image_proc draw_sonar_node` 19 | 20 | ## Subscribers 21 | 22 | Subscribes to the topic `sonar_image` of type [marine_acoustic_msgs/ProjectedSonarImage](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs/msg/ProjectedSonarImage.msg). 23 | 24 | 25 | ## Publishers 26 | 27 | By default publishes three [sensor_msgs/Image](https://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html) topics: 28 | 29 | * `drawn_sonar` contains an image of the sonar drawn with a cartesian projection 30 | with the origin (range = 0) of the sonar centered on the bottom edge of 31 | the image, and azimuth=0 extending vertically upwards in the image. By default, 32 | the image height is set by the number range bins, and the image width is 33 | automatically determined based on the height and the min/max azimuth of the 34 | sonar image. The color map used to convert the sonar intensity to RGB is set 35 | in code. 36 | 37 | ![](drawn_sonar.png) 38 | 39 | * `drawn_sonar_rect` is the contents of the SonarImage data mapped directly from polar to cartesian coordinates. 40 | Since the intensity data in the SonarImage is stored azimuth-major, the data is 41 | mapped into the image space with range in the X direction and azimuth in the Y 42 | direction: 43 | 44 | * Image width is the number of range bins in the data, with the minimum range 45 | on the left side and maximum range on the right side. 46 | 47 | * Image height is the number of azimuth bins in the data, with the lowest 48 | azimuth (typically the most negative) at the top, and most positive at the 49 | bottom. 50 | 51 | ![](drawn_sonar_rect.png) 52 | 53 | * `drawn_sonar_osd` adds guidelines and annotations to `drawn_sonar`. Overlay parameters can be configured in realtime. 54 | 55 | If the param `publish_timing` is `true`, the node will track the elapsed time to 56 | draw each sonar image and publish that information to the topic `sonar_image_proc_timing` 57 | as a [std_msgs/String](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/String.html) 58 | containing a JSON dict. 59 | 60 | If the param `publish_old` is `true`, the node will also draw the sonar using 61 | the old `draw_sonar` algorithm for comparison. 62 | 63 | ## Params 64 | 65 | If `max_range` is set to a non-zero value, images will be clipped/dropped to that max range (or the actual sonar range, whichever is smaller). 66 | 67 | If `publish_timing` is `true` the node will publish performance information as a 68 | JSON [string](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/String.html) 69 | to the topic `sonar_image_proc_timing`. Defaults to `true` 70 | 71 | If `publish_old` is `true` the node will also draw the sonar using the old 72 | algorithm to the topic `old_drawn_sonar`. Defaults to `false` 73 | 74 | If `publish_histogram` is `true` the node will publish a "raw" histogram information as a `UInt32MultiArray` to the topic `histogram`. It contains a vector of unsigned ints giving the count for each intensity value -- so for 8 bit data the vector will be 256 elements in length, and for 16-bit data it will be 65536 elements in length. 75 | 76 | # bag2sonar 77 | 78 | The program `bag2sonar` reads in a bagfile containing a `ProjectedSonarImage` topic, draws the sonar image and writes those images to *new* bagfile in a `Image` topic. 79 | 80 | Usage: 81 | 82 | ``` 83 | $ rosrun sonar_image_proc bag2sonar 84 | Usage: 85 | 86 | bag2sonar [options] 87 | 88 | Draw sonar from a bagfile: 89 | -h [ --help ] Display this help message 90 | -l [ --logscale ] Do logscale 91 | --min-db arg (=0) Min db 92 | --max-db arg (=0) Max db 93 | --osd If set, include the on-screen display 94 | in output 95 | -o [ --output-bag ] arg Name of output bagfile 96 | -t [ --output-topic ] arg (=/drawn_sonar) 97 | Topic for images in output bagfile 98 | ``` 99 | 100 | Note that `bag2sonar` is not a conventional ROS node, it is intended to run as a standalone commandline program. It uses `ros_storage` to read the input bagfile sequentially, rather than subscribing to a topic. 101 | 102 | # histogram_drawer 103 | 104 | `python/histogram_drawer` is a Python script which subscribes to the `histogram` topic and uses numpy+Matplotlib to bin the data (into a fixed set of 128 bin right now), and draw a plot to the topic `drawn_histogram`. 105 | 106 | # Python API 107 | 108 | Long term, I'd like to be able to call this drawing function from Python, 109 | however we're not there yet. 110 | 111 | There IS a totally separate python node that publishes a pointcloud 112 | for visualization in rviz: 113 | 114 | `rosrun sonar_image_proc sonar_pointcloud.py` 115 | 116 | 117 | # API 118 | 119 | Sonar drawing is implemented in the [SonarDrawer](include/sonar_image_proc/SonarDrawer.h) class, which takes an instance of an [AbstractSonarInterface](include/sonar_image_proc/AbstractSonarInterface.h) and returns a cv::Mat. SonarDrawer computes and stores pre-calculated matrices to accelerate the drawing. 120 | 121 | A convenience function [drawSonar](include/sonar_image_proc/DrawSonar.h) is also provided. It is a trivial wrapper which creates an instance of SonarDrawer then calls it. Calls to drawSonar do not retain the cached matrices and are less efficient. 122 | 123 | # Related Packages 124 | 125 | * [liboculus](https://github.com/apl-ocean-engineering/liboculus) provides network IO and data parsing for the Oculus sonar (non-ROS). 126 | * [oculus_sonar_driver](https://gitlab.com/apl-ocean-engineering/oculus_sonar_driver) provides a ROS node for interfacing with the Oculus sonar. 127 | * [marine_acoustic_msgs](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs) defines the ROS [ProjectedSonarImage](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs/msg/ProjectedSonarImage.msg) message type published by [oculus_sonar_driver](https://gitlab.com/apl-ocean-engineering/oculus_sonar_driver). 128 | * [rqt_sonar_image_view](https://github.com/apl-ocean-engineering/rqt_sonar_image_view) is an Rqt plugin for displaying sonar imagery (uses [sonar_image_proc](https://github.com/apl-ocean-engineering/sonar_image_proc)) 129 | 130 | 131 | # License 132 | 133 | Licensed under [BSD 3-clause license](LICENSE). 134 | -------------------------------------------------------------------------------- /drawn_sonar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apl-ocean-engineering/sonar_image_proc/5c39a302996826ac3555d98c41cc26fab2beafe9/drawn_sonar.png -------------------------------------------------------------------------------- /drawn_sonar_rect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apl-ocean-engineering/sonar_image_proc/5c39a302996826ac3555d98c41cc26fab2beafe9/drawn_sonar_rect.png -------------------------------------------------------------------------------- /fips: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """fips main entry""" 3 | import os 4 | import sys 5 | import subprocess 6 | proj_path = os.path.dirname(os.path.abspath(__file__)) 7 | fips_path = os.path.dirname(proj_path) + '/fips' 8 | if not os.path.isdir(fips_path) : 9 | print("\033[93m=== cloning fips build system to '{}':\033[0m".format(fips_path)) 10 | subprocess.call(['git', 'clone', 'https://github.com/amarburg/fips.git', fips_path]) 11 | sys.path.insert(0,fips_path) 12 | try : 13 | from mod import fips 14 | except ImportError : 15 | print("\033[91m[ERROR]\033[0m failed to initialize fips build system in '{}'".format(proj_path)) 16 | sys.exit(10) 17 | fips.run(fips_path, proj_path, sys.argv) 18 | -------------------------------------------------------------------------------- /fips.yml: -------------------------------------------------------------------------------- 1 | imports: 2 | fips-googletest: 3 | git: https://github.com/amarburg/fips-googletest.git 4 | cond: "FIPS_UNITTESTS" 5 | 6 | exports: 7 | modules: 8 | draw_sonar: . 9 | header-dirs: 10 | - include 11 | 12 | defines: 13 | FIPS_EXCEPTIONS: ON 14 | FIPS_RTTI: ON 15 | FIPS_UNITTESTS_RUN_AFTER_BUILD: ON 16 | -------------------------------------------------------------------------------- /helpers/epilogue.txt: -------------------------------------------------------------------------------- 1 | 2 | } // namespace sonar_image_proc 3 | -------------------------------------------------------------------------------- /helpers/inferno_color_map.csv: -------------------------------------------------------------------------------- 1 | 0.001462, 0.000466, 0.013866 2 | 0.002267, 0.001270, 0.018570 3 | 0.003299, 0.002249, 0.024239 4 | 0.004547, 0.003392, 0.030909 5 | 0.006006, 0.004692, 0.038558 6 | 0.007676, 0.006136, 0.046836 7 | 0.009561, 0.007713, 0.055143 8 | 0.011663, 0.009417, 0.063460 9 | 0.013995, 0.011225, 0.071862 10 | 0.016561, 0.013136, 0.080282 11 | 0.019373, 0.015133, 0.088767 12 | 0.022447, 0.017199, 0.097327 13 | 0.025793, 0.019331, 0.105930 14 | 0.029432, 0.021503, 0.114621 15 | 0.033385, 0.023702, 0.123397 16 | 0.037668, 0.025921, 0.132232 17 | 0.042253, 0.028139, 0.141141 18 | 0.046915, 0.030324, 0.150164 19 | 0.051644, 0.032474, 0.159254 20 | 0.056449, 0.034569, 0.168414 21 | 0.061340, 0.036590, 0.177642 22 | 0.066331, 0.038504, 0.186962 23 | 0.071429, 0.040294, 0.196354 24 | 0.076637, 0.041905, 0.205799 25 | 0.081962, 0.043328, 0.215289 26 | 0.087411, 0.044556, 0.224813 27 | 0.092990, 0.045583, 0.234358 28 | 0.098702, 0.046402, 0.243904 29 | 0.104551, 0.047008, 0.253430 30 | 0.110536, 0.047399, 0.262912 31 | 0.116656, 0.047574, 0.272321 32 | 0.122908, 0.047536, 0.281624 33 | 0.129285, 0.047293, 0.290788 34 | 0.135778, 0.046856, 0.299776 35 | 0.142378, 0.046242, 0.308553 36 | 0.149073, 0.045468, 0.317085 37 | 0.155850, 0.044559, 0.325338 38 | 0.162689, 0.043554, 0.333277 39 | 0.169575, 0.042489, 0.340874 40 | 0.176493, 0.041402, 0.348111 41 | 0.183429, 0.040329, 0.354971 42 | 0.190367, 0.039309, 0.361447 43 | 0.197297, 0.038400, 0.367535 44 | 0.204209, 0.037632, 0.373238 45 | 0.211095, 0.037030, 0.378563 46 | 0.217949, 0.036615, 0.383522 47 | 0.224763, 0.036405, 0.388129 48 | 0.231538, 0.036405, 0.392400 49 | 0.238273, 0.036621, 0.396353 50 | 0.244967, 0.037055, 0.400007 51 | 0.251620, 0.037705, 0.403378 52 | 0.258234, 0.038571, 0.406485 53 | 0.264810, 0.039647, 0.409345 54 | 0.271347, 0.040922, 0.411976 55 | 0.277850, 0.042353, 0.414392 56 | 0.284321, 0.043933, 0.416608 57 | 0.290763, 0.045644, 0.418637 58 | 0.297178, 0.047470, 0.420491 59 | 0.303568, 0.049396, 0.422182 60 | 0.309935, 0.051407, 0.423721 61 | 0.316282, 0.053490, 0.425116 62 | 0.322610, 0.055634, 0.426377 63 | 0.328921, 0.057827, 0.427511 64 | 0.335217, 0.060060, 0.428524 65 | 0.341500, 0.062325, 0.429425 66 | 0.347771, 0.064616, 0.430217 67 | 0.354032, 0.066925, 0.430906 68 | 0.360284, 0.069247, 0.431497 69 | 0.366529, 0.071579, 0.431994 70 | 0.372768, 0.073915, 0.432400 71 | 0.379001, 0.076253, 0.432719 72 | 0.385228, 0.078591, 0.432955 73 | 0.391453, 0.080927, 0.433109 74 | 0.397674, 0.083257, 0.433183 75 | 0.403894, 0.085580, 0.433179 76 | 0.410113, 0.087896, 0.433098 77 | 0.416331, 0.090203, 0.432943 78 | 0.422549, 0.092501, 0.432714 79 | 0.428768, 0.094790, 0.432412 80 | 0.434987, 0.097069, 0.432039 81 | 0.441207, 0.099338, 0.431594 82 | 0.447428, 0.101597, 0.431080 83 | 0.453651, 0.103848, 0.430498 84 | 0.459875, 0.106089, 0.429846 85 | 0.466100, 0.108322, 0.429125 86 | 0.472328, 0.110547, 0.428334 87 | 0.478558, 0.112764, 0.427475 88 | 0.484789, 0.114974, 0.426548 89 | 0.491022, 0.117179, 0.425552 90 | 0.497257, 0.119379, 0.424488 91 | 0.503493, 0.121575, 0.423356 92 | 0.509730, 0.123769, 0.422156 93 | 0.515967, 0.125960, 0.420887 94 | 0.522206, 0.128150, 0.419549 95 | 0.528444, 0.130341, 0.418142 96 | 0.534683, 0.132534, 0.416667 97 | 0.540920, 0.134729, 0.415123 98 | 0.547157, 0.136929, 0.413511 99 | 0.553392, 0.139134, 0.411829 100 | 0.559624, 0.141346, 0.410078 101 | 0.565854, 0.143567, 0.408258 102 | 0.572081, 0.145797, 0.406369 103 | 0.578304, 0.148039, 0.404411 104 | 0.584521, 0.150294, 0.402385 105 | 0.590734, 0.152563, 0.400290 106 | 0.596940, 0.154848, 0.398125 107 | 0.603139, 0.157151, 0.395891 108 | 0.609330, 0.159474, 0.393589 109 | 0.615513, 0.161817, 0.391219 110 | 0.621685, 0.164184, 0.388781 111 | 0.627847, 0.166575, 0.386276 112 | 0.633998, 0.168992, 0.383704 113 | 0.640135, 0.171438, 0.381065 114 | 0.646260, 0.173914, 0.378359 115 | 0.652369, 0.176421, 0.375586 116 | 0.658463, 0.178962, 0.372748 117 | 0.664540, 0.181539, 0.369846 118 | 0.670599, 0.184153, 0.366879 119 | 0.676638, 0.186807, 0.363849 120 | 0.682656, 0.189501, 0.360757 121 | 0.688653, 0.192239, 0.357603 122 | 0.694627, 0.195021, 0.354388 123 | 0.700576, 0.197851, 0.351113 124 | 0.706500, 0.200728, 0.347777 125 | 0.712396, 0.203656, 0.344383 126 | 0.718264, 0.206636, 0.340931 127 | 0.724103, 0.209670, 0.337424 128 | 0.729909, 0.212759, 0.333861 129 | 0.735683, 0.215906, 0.330245 130 | 0.741423, 0.219112, 0.326576 131 | 0.747127, 0.222378, 0.322856 132 | 0.752794, 0.225706, 0.319085 133 | 0.758422, 0.229097, 0.315266 134 | 0.764010, 0.232554, 0.311399 135 | 0.769556, 0.236077, 0.307485 136 | 0.775059, 0.239667, 0.303526 137 | 0.780517, 0.243327, 0.299523 138 | 0.785929, 0.247056, 0.295477 139 | 0.791293, 0.250856, 0.291390 140 | 0.796607, 0.254728, 0.287264 141 | 0.801871, 0.258674, 0.283099 142 | 0.807082, 0.262692, 0.278898 143 | 0.812239, 0.266786, 0.274661 144 | 0.817341, 0.270954, 0.270390 145 | 0.822386, 0.275197, 0.266085 146 | 0.827372, 0.279517, 0.261750 147 | 0.832299, 0.283913, 0.257383 148 | 0.837165, 0.288385, 0.252988 149 | 0.841969, 0.292933, 0.248564 150 | 0.846709, 0.297559, 0.244113 151 | 0.851384, 0.302260, 0.239636 152 | 0.855992, 0.307038, 0.235133 153 | 0.860533, 0.311892, 0.230606 154 | 0.865006, 0.316822, 0.226055 155 | 0.869409, 0.321827, 0.221482 156 | 0.873741, 0.326906, 0.216886 157 | 0.878001, 0.332060, 0.212268 158 | 0.882188, 0.337287, 0.207628 159 | 0.886302, 0.342586, 0.202968 160 | 0.890341, 0.347957, 0.198286 161 | 0.894305, 0.353399, 0.193584 162 | 0.898192, 0.358911, 0.188860 163 | 0.902003, 0.364492, 0.184116 164 | 0.905735, 0.370140, 0.179350 165 | 0.909390, 0.375856, 0.174563 166 | 0.912966, 0.381636, 0.169755 167 | 0.916462, 0.387481, 0.164924 168 | 0.919879, 0.393389, 0.160070 169 | 0.923215, 0.399359, 0.155193 170 | 0.926470, 0.405389, 0.150292 171 | 0.929644, 0.411479, 0.145367 172 | 0.932737, 0.417627, 0.140417 173 | 0.935747, 0.423831, 0.135440 174 | 0.938675, 0.430091, 0.130438 175 | 0.941521, 0.436405, 0.125409 176 | 0.944285, 0.442772, 0.120354 177 | 0.946965, 0.449191, 0.115272 178 | 0.949562, 0.455660, 0.110164 179 | 0.952075, 0.462178, 0.105031 180 | 0.954506, 0.468744, 0.099874 181 | 0.956852, 0.475356, 0.094695 182 | 0.959114, 0.482014, 0.089499 183 | 0.961293, 0.488716, 0.084289 184 | 0.963387, 0.495462, 0.079073 185 | 0.965397, 0.502249, 0.073859 186 | 0.967322, 0.509078, 0.068659 187 | 0.969163, 0.515946, 0.063488 188 | 0.970919, 0.522853, 0.058367 189 | 0.972590, 0.529798, 0.053324 190 | 0.974176, 0.536780, 0.048392 191 | 0.975677, 0.543798, 0.043618 192 | 0.977092, 0.550850, 0.039050 193 | 0.978422, 0.557937, 0.034931 194 | 0.979666, 0.565057, 0.031409 195 | 0.980824, 0.572209, 0.028508 196 | 0.981895, 0.579392, 0.026250 197 | 0.982881, 0.586606, 0.024661 198 | 0.983779, 0.593849, 0.023770 199 | 0.984591, 0.601122, 0.023606 200 | 0.985315, 0.608422, 0.024202 201 | 0.985952, 0.615750, 0.025592 202 | 0.986502, 0.623105, 0.027814 203 | 0.986964, 0.630485, 0.030908 204 | 0.987337, 0.637890, 0.034916 205 | 0.987622, 0.645320, 0.039886 206 | 0.987819, 0.652773, 0.045581 207 | 0.987926, 0.660250, 0.051750 208 | 0.987945, 0.667748, 0.058329 209 | 0.987874, 0.675267, 0.065257 210 | 0.987714, 0.682807, 0.072489 211 | 0.987464, 0.690366, 0.079990 212 | 0.987124, 0.697944, 0.087731 213 | 0.986694, 0.705540, 0.095694 214 | 0.986175, 0.713153, 0.103863 215 | 0.985566, 0.720782, 0.112229 216 | 0.984865, 0.728427, 0.120785 217 | 0.984075, 0.736087, 0.129527 218 | 0.983196, 0.743758, 0.138453 219 | 0.982228, 0.751442, 0.147565 220 | 0.981173, 0.759135, 0.156863 221 | 0.980032, 0.766837, 0.166353 222 | 0.978806, 0.774545, 0.176037 223 | 0.977497, 0.782258, 0.185923 224 | 0.976108, 0.789974, 0.196018 225 | 0.974638, 0.797692, 0.206332 226 | 0.973088, 0.805409, 0.216877 227 | 0.971468, 0.813122, 0.227658 228 | 0.969783, 0.820825, 0.238686 229 | 0.968041, 0.828515, 0.249972 230 | 0.966243, 0.836191, 0.261534 231 | 0.964394, 0.843848, 0.273391 232 | 0.962517, 0.851476, 0.285546 233 | 0.960626, 0.859069, 0.298010 234 | 0.958720, 0.866624, 0.310820 235 | 0.956834, 0.874129, 0.323974 236 | 0.954997, 0.881569, 0.337475 237 | 0.953215, 0.888942, 0.351369 238 | 0.951546, 0.896226, 0.365627 239 | 0.950018, 0.903409, 0.380271 240 | 0.948683, 0.910473, 0.395289 241 | 0.947594, 0.917399, 0.410665 242 | 0.946809, 0.924168, 0.426373 243 | 0.946392, 0.930761, 0.442367 244 | 0.946403, 0.937159, 0.458592 245 | 0.946903, 0.943348, 0.474970 246 | 0.947937, 0.949318, 0.491426 247 | 0.949545, 0.955063, 0.507860 248 | 0.951740, 0.960587, 0.524203 249 | 0.954529, 0.965896, 0.540361 250 | 0.957896, 0.971003, 0.556275 251 | 0.961812, 0.975924, 0.571925 252 | 0.966249, 0.980678, 0.587206 253 | 0.971162, 0.985282, 0.602154 254 | 0.976511, 0.989753, 0.616760 255 | 0.982257, 0.994109, 0.631017 256 | 0.988362, 0.998364, 0.644924 257 | -------------------------------------------------------------------------------- /helpers/make_color_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | from pathlib import Path 5 | import numpy as np 6 | 7 | if __name__ == "__main__": 8 | 9 | parser = argparse.ArgumentParser(description='Make .cpp source file for a colormap') 10 | 11 | parser.add_argument('--prologue', type=Path, default="prologue.txt", 12 | help='Test file containing boilerplate for the beginning of file') 13 | parser.add_argument('--epilogue', type=Path, default='epilogue.txt', 14 | help="Test file containing boilerplate for end of file") 15 | 16 | parser.add_argument('--csv', type=Path, nargs="*", 17 | help='CSV file containing') 18 | 19 | args = parser.parse_args() 20 | 21 | if args.prologue: 22 | with open(args.prologue) as fp: 23 | for line in fp: 24 | print(line.strip()) 25 | 26 | for csv_file in args.csv: 27 | data = np.loadtxt(csv_file, delimiter=',', dtype=float) 28 | 29 | print("const float ColorMap::float_data[%d][3] = {" % len(data)) 30 | for entry in data: 31 | print("{%f,%f,%f}," % (entry[0],entry[1],entry[2])) 32 | print("};") 33 | print() 34 | 35 | print("const float ColorMap::char_data[%d][3] = {" % len(data)) 36 | for entry in data: 37 | print("{%d,%d,%d}," % (int(entry[0]*255), 38 | int(entry[1]*255), 39 | int(entry[2]*255))) 40 | print("};") 41 | print() 42 | 43 | 44 | if args.epilogue: 45 | with open(args.epilogue) as fp: 46 | for line in fp: 47 | print(line) 48 | -------------------------------------------------------------------------------- /helpers/prologue.txt: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | // Color map data from 4 | // https://github.com/BIDS/colormap/blob/master/colormaps.py 5 | // 6 | // As used in matplotlib 7 | // As released under a CC0 license 8 | // 9 | // This file autogenerated with helpers/make_color_map.py 10 | 11 | #include "sonar_image_proc/ColorMaps.h" 12 | 13 | namespace sonar_image_proc { 14 | -------------------------------------------------------------------------------- /include/sonar_image_proc/AbstractSonarInterface.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace sonar_image_proc { 14 | 15 | typedef std::pair Bounds_t; 16 | extern const Bounds_t UnsetBounds; 17 | 18 | struct AzimuthRangeIndices { 19 | AzimuthRangeIndices(size_t a, size_t r) : _rangeIdx(r), _azimuthIdx(a) { ; } 20 | 21 | size_t range() const { return _rangeIdx; } 22 | size_t azimuth() const { return _azimuthIdx; } 23 | 24 | size_t _rangeIdx, _azimuthIdx; 25 | }; 26 | 27 | // Abstract class strictly for drawing sonar images 28 | // Designed as a "common abstact type" between the Blueprint 29 | // Subsea SimplePingResult and ROS ImagingSonarMsg 30 | struct AbstractSonarInterface { 31 | public: 32 | AbstractSonarInterface(); 33 | 34 | enum DataType_t { 35 | TYPE_NONE, 36 | TYPE_UINT8, 37 | TYPE_UINT16, 38 | TYPE_UINT32, 39 | TYPE_FLOAT32 40 | }; 41 | 42 | virtual DataType_t data_type() const = 0; 43 | 44 | // 45 | // azimuths are in **radians** 46 | // 47 | virtual const std::vector &azimuths() const = 0; 48 | 49 | int nBearings() const { return azimuths().size(); } 50 | __attribute__((deprecated)); 51 | float bearing(int n) const { return azimuths().at(n); } 52 | __attribute__((deprecated)); 53 | 54 | int nAzimuth() const { return azimuths().size(); } 55 | int nAzimuths() const { 56 | return azimuths().size(); 57 | } // Whoops, should be consistent 58 | float azimuth(int n) const { return azimuths().at(n); } 59 | 60 | Bounds_t azimuthBounds() const; 61 | 62 | float minAzimuth() const { return azimuthBounds().first; } 63 | float maxAzimuth() const { return azimuthBounds().second; } 64 | 65 | float minAzimuthTan() const { 66 | checkAzimuthBounds(); 67 | return _minAzimuthTan; 68 | } 69 | float maxAzimuthTan() const { 70 | checkAzimuthBounds(); 71 | return _maxAzimuthTan; 72 | } 73 | 74 | // 75 | // ranges are in **meters** 76 | // 77 | virtual const std::vector &ranges() const = 0; 78 | 79 | int nRanges() const { return ranges().size(); } 80 | float range(int n) const { return ranges().at(n); } 81 | 82 | Bounds_t rangeBounds() const; 83 | 84 | float minRange() const { return rangeBounds().first; } 85 | float maxRange() const { return rangeBounds().second; } 86 | 87 | float maxRangeSquared() const { 88 | checkRangeBounds(); 89 | return _maxRangeSquared; 90 | } 91 | 92 | // 93 | // Rather than dive into template magic, it's up to the 94 | // end user to understand the underlying data type 95 | // stored in a sonar data format. 96 | // 97 | // The "native" format is float ... so intensity_float() 98 | // is the only pure virtual. The default implementations of 99 | // _unit8 and _uint16 calculate from the float; but 100 | // depending on the original data format, it may be more efficient 101 | // to override these implementations 102 | // 103 | virtual float intensity_float(const AzimuthRangeIndices &idx) const = 0; 104 | 105 | virtual uint8_t intensity_uint8(const AzimuthRangeIndices &idx) const { 106 | return INT8_MAX * intensity_float(idx); 107 | } 108 | 109 | virtual uint16_t intensity_uint16(const AzimuthRangeIndices &idx) const { 110 | return INT16_MAX * intensity_float(idx); 111 | } 112 | 113 | virtual uint32_t intensity_uint32(const AzimuthRangeIndices &idx) const { 114 | return INT32_MAX * intensity_float(idx); 115 | } 116 | 117 | // Trivial wrappers. These will be deprecated eventually 118 | float intensity_float(size_t a, size_t r) const { 119 | return intensity_float(AzimuthRangeIndices(a, r)); 120 | } 121 | __attribute__((deprecated)); 122 | uint8_t intensity_uint8(size_t a, size_t r) const { 123 | return intensity_uint8(AzimuthRangeIndices(a, r)); 124 | } 125 | __attribute__((deprecated)); 126 | uint16_t intensity_uint16(size_t a, size_t r) const { 127 | return intensity_uint16(AzimuthRangeIndices(a, r)); 128 | } 129 | __attribute__((deprecated)); 130 | 131 | private: 132 | // In a few cases, need to "check and potentially calculate cached 133 | // value" without actually getting the value 134 | void checkRangeBounds() const; 135 | void checkAzimuthBounds() const; 136 | 137 | private: 138 | // Since we search extensively for the bounds 139 | // (rather than assuming the first and last are the bounds), 140 | // cache the results 141 | mutable Bounds_t _rangeBounds, _azimuthBounds; 142 | mutable float _minAzimuthTan, _maxAzimuthTan; 143 | mutable float _maxRangeSquared; 144 | }; 145 | 146 | } // namespace sonar_image_proc 147 | -------------------------------------------------------------------------------- /include/sonar_image_proc/ColorMaps.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #pragma once 5 | 6 | #include 7 | 8 | #include "sonar_image_proc/AbstractSonarInterface.h" 9 | 10 | namespace sonar_image_proc { 11 | 12 | using sonar_image_proc::AbstractSonarInterface; 13 | 14 | struct SonarColorMap { 15 | // A ColorMap is a mapping from one pixel of sonar data to 16 | // one pixel in the image. Each virtual function in 17 | // this class corresponds to one possible OpenCV pixel format. 18 | // 19 | // By default "lookup_cv32fc1()" is the only operation 20 | // implemented in this version. It returns the sonar pixel 21 | // as a float (in the interval [0,1]) 22 | // which OpenCV will interpret as a greyscale. 23 | // 24 | // The other virtuals are naive wrappers which convert that float 25 | // to other OpenCV formats ... though still representing the data 26 | // as a greyscale. 27 | // 28 | // Inherited classes can override any/all of the classes to 29 | // cover other input/output combinations. 30 | 31 | // The "native" operation returns a single greyscale values a as float 32 | virtual float lookup_cv32fc1(const AbstractSonarInterface &ping, 33 | const AzimuthRangeIndices &loc) const { 34 | return ping.intensity_float(loc); 35 | } 36 | 37 | virtual cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, 38 | const AzimuthRangeIndices &loc) const { 39 | const auto f = lookup_cv32fc1(ping, loc); 40 | return cv::Vec3b(f * 255, f * 255, f * 255); 41 | } 42 | 43 | virtual cv::Vec3f lookup_cv32fc3(const AbstractSonarInterface &ping, 44 | const AzimuthRangeIndices &loc) const { 45 | const auto f = lookup_cv32fc1(ping, loc); 46 | return cv::Vec3f(f, f, f); 47 | } 48 | }; 49 | 50 | //=== Implementations of specific color maps === 51 | struct MitchellColorMap : public SonarColorMap { 52 | cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, 53 | const AzimuthRangeIndices &loc) const override { 54 | const auto i = ping.intensity_float(loc); 55 | return cv::Vec3b(1 - i, i, i); 56 | } 57 | }; 58 | 59 | // Use color map from 60 | // https://github.com/BIDS/colormap/blob/master/colormaps.py 61 | // 62 | // As used in matplotlib 63 | // As released under a CC0 license 64 | struct InfernoColorMap : public SonarColorMap { 65 | static const float _inferno_data_float[][3]; 66 | static const float _inferno_data_uint8[][3]; 67 | 68 | // Minor optimization ... don't go through the intermediate Scalar 69 | cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, 70 | const AzimuthRangeIndices &loc) const override { 71 | const auto i = ping.intensity_uint8(loc); 72 | return cv::Vec3b(_inferno_data_uint8[i][0], _inferno_data_uint8[i][1], 73 | _inferno_data_uint8[i][2]); 74 | } 75 | 76 | cv::Vec3f lookup_cv32fc3(const AbstractSonarInterface &ping, 77 | const AzimuthRangeIndices &loc) const override { 78 | const auto i = ping.intensity_uint8(loc); 79 | return cv::Vec3f(_inferno_data_float[i][0], _inferno_data_float[i][1], 80 | _inferno_data_float[i][2]); 81 | } 82 | }; 83 | 84 | struct InfernoSaturationColorMap : public InfernoColorMap { 85 | // Minor optimization ... don't go through the intermediate Scalar 86 | cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, 87 | const AzimuthRangeIndices &loc) const override { 88 | const auto i = ping.intensity_uint8(loc); 89 | if (i == 255) { 90 | return cv::Vec3b(0, 255, 0); 91 | } else { 92 | return cv::Vec3b(_inferno_data_uint8[i][0], _inferno_data_uint8[i][1], 93 | _inferno_data_uint8[i][2]); 94 | } 95 | } 96 | 97 | cv::Vec3f lookup_cv32fc3(const AbstractSonarInterface &ping, 98 | const AzimuthRangeIndices &loc) const override { 99 | const auto i = ping.intensity_uint8(loc); 100 | if (i == 255) { 101 | return cv::Vec3f(0.0, 1.0, 0.0); 102 | } else { 103 | return cv::Vec3f(_inferno_data_float[i][0], _inferno_data_float[i][1], 104 | _inferno_data_float[i][2]); 105 | } 106 | } 107 | }; 108 | 109 | } // namespace sonar_image_proc 110 | -------------------------------------------------------------------------------- /include/sonar_image_proc/DataStructures.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace sonar_image_proc { 12 | 13 | // \todo. Are these used anymore? 14 | struct SonarPoint { 15 | SonarPoint(float _x, float _z) : x(_x), z(_z) { ; } 16 | float x; 17 | float z; 18 | }; 19 | 20 | SonarPoint bearingRange2Cartesian(float bearing, float range); 21 | 22 | } // namespace sonar_image_proc 23 | -------------------------------------------------------------------------------- /include/sonar_image_proc/DrawSonar.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | // This file contains the "functional" API. These are really just 4 | // thin wrappers around a single-use instance of SonarDrawer 5 | // 6 | // See SonarDrawer.h for a class-based API (which is more efficients) 7 | // as it can store and reuse intermediate results 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include "sonar_image_proc/AbstractSonarInterface.h" 15 | #include "sonar_image_proc/ColorMaps.h" 16 | #include "sonar_image_proc/SonarDrawer.h" 17 | 18 | namespace sonar_image_proc { 19 | 20 | inline cv::Mat drawSonar(const sonar_image_proc::AbstractSonarInterface &ping, 21 | const SonarColorMap &colorMap = InfernoColorMap(), 22 | const cv::Mat &image = cv::Mat(0, 0, CV_8UC3)) { 23 | SonarDrawer drawer; 24 | cv::Mat rectImage = drawer.drawRectSonarImage(ping, colorMap, image); 25 | return drawer.remapRectSonarImage(ping, rectImage); 26 | } 27 | 28 | // Maps the sonar ping to an RGB image. 29 | // rectImage is reshaped to be numRanges rows x numBearings columns 30 | // 31 | // If rectImage is either 8UC3 or 32FC3, it retains that type, otherwise 32 | // rectImage is converted to 8UC3 33 | // 34 | // Cell (0,0) is the color mapping of the data with the smallest range and 35 | // smallest (typically, most negative) bearing in the ping. 36 | // 37 | // Cell (nRange,0) is the data at the max range, most negative bearing 38 | // 39 | // Cell (nRange,nBearing) is the data at the max range, most positive bearing 40 | // 41 | inline cv::Mat drawSonarRectImage( 42 | const sonar_image_proc::AbstractSonarInterface &ping, 43 | const SonarColorMap &colorMap = InfernoColorMap(), 44 | const cv::Mat &rectImage = cv::Mat(0, 0, CV_8UC3)) { 45 | SonarDrawer drawer; 46 | return drawer.drawRectSonarImage(ping, colorMap, rectImage); 47 | } 48 | 49 | namespace old_api { 50 | 51 | // === Old / Legacy API below === 52 | 53 | // Given an sonar image, calculates the bounding rectangle required to 54 | // draw it. Assumes zero range (the point of the fan) occurs on the 55 | // bottom edge of the image. 56 | // Azimuth = 0 is straight ahead from the sonar (the vertical axis in the image) 57 | // Assuming azimuth = 0 is within the interval of [min azimuth, max azimuth] 58 | // then the Image height is pixPerRangeBin * (max range in bin / range 59 | // resolution) Image width is determined by the triangles defined by image 60 | // height and min/max azimuth 61 | // 62 | // If set, maxRange is used in lieu of the ping's native max range, 63 | // allowing truncation of the image 64 | cv::Size calculateImageSize( 65 | const sonar_image_proc::AbstractSonarInterface &ping, cv::Size hint, 66 | int pixPerRangeBin = 2, float maxRange = -1.0); 67 | 68 | cv::Mat drawSonar(const sonar_image_proc::AbstractSonarInterface &ping, 69 | cv::Mat &mat, 70 | const SonarColorMap &colorMap = InfernoColorMap(), 71 | float maxRange = -1.0); 72 | 73 | } // namespace old_api 74 | 75 | } // namespace sonar_image_proc 76 | -------------------------------------------------------------------------------- /include/sonar_image_proc/HistogramGenerator.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #pragma once 5 | 6 | #include "sonar_image_proc/AbstractSonarInterface.h" 7 | 8 | namespace sonar_image_proc { 9 | 10 | class HistogramGenerator { 11 | public: 12 | static std::vector Generate(const AbstractSonarInterface &ping); 13 | 14 | static std::vector GenerateUint8( 15 | const AbstractSonarInterface &ping); 16 | static std::vector GenerateUint16( 17 | const AbstractSonarInterface &ping); 18 | 19 | // Histogram generator for 32bit data produces 256 bins of log10(intensity) 20 | static std::vector GenerateUint32( 21 | const AbstractSonarInterface &ping); 22 | }; 23 | 24 | } // namespace sonar_image_proc 25 | -------------------------------------------------------------------------------- /include/sonar_image_proc/OverlayImage.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace sonar_image_proc { 5 | 6 | using cv::Mat; 7 | using cv::Vec; 8 | 9 | // Adapted from the code sample in the OpenCV documentation: 10 | // https://docs.opencv.org/4.x/d3/d63/classcv_1_1Mat.html#a33ee3bc402827f587a5ad64b568d6986 11 | // 12 | // \todo(@amarburg): Optimize? Loop unrolling? 13 | // 14 | 15 | template 16 | void overlayImage(const Mat &bg, const Mat &fg, Mat &dst) { 17 | typedef Vec VB; 18 | typedef Vec VF; 19 | 20 | const float alpha_scale = (float)std::numeric_limits::max(), 21 | inv_scale = 1.f / alpha_scale; 22 | 23 | #if (CV_VERSION_MAJOR >= 4) 24 | CV_Assert(bg.type() == cv::traits::Type::value && 25 | fg.type() == cv::traits::Type::value && bg.size() == fg.size()); 26 | #else 27 | CV_Assert(bg.type() == cv::DataType::type && 28 | fg.type() == cv::DataType::type && bg.size() == fg.size()); 29 | #endif 30 | 31 | dst.create(bg.size(), bg.type()); 32 | 33 | cv::MatConstIterator_ fit = fg.begin(), fit_end = fg.end(); 34 | cv::MatConstIterator_ bit = bg.begin(); 35 | cv::MatIterator_ dst_it = dst.begin(); 36 | 37 | for (; fit != fit_end; ++fit, ++bit, ++dst_it) { 38 | const auto fg_pix = *fit; 39 | const auto bg_pix = *bit; 40 | 41 | const float alpha = fg_pix[3] * inv_scale; 42 | const float beta = 1 - alpha; 43 | *dst_it = VB(cv::saturate_cast(fg_pix[0] * alpha + bg_pix[0] * beta), 44 | cv::saturate_cast(fg_pix[1] * alpha + bg_pix[1] * beta), 45 | cv::saturate_cast(fg_pix[2] * alpha + bg_pix[2] * beta)); 46 | } 47 | } 48 | 49 | // void overlayImage(const cv::Mat &background, const cv::Mat &foreground, 50 | // cv::Mat &output) { 51 | // background.copyTo(output); 52 | 53 | // // Only works with CV_8UC3 for background and 54 | // // CV_8UC4 for foreground 55 | // CV_Assert(bg.type() == traits::Type::value && 56 | // bg.size() == fg.size()); 57 | 58 | // MatConstIterator_ bit = background.begin(), 59 | // bit_end = background.end(); 60 | // MatConstIterator_ fit = foreground.begin(); 61 | 62 | // MatIterator_ oit = dst.begin(); 63 | 64 | // for (; bit != bit_end; ++bit, ++fit, ++oit) { 65 | 66 | // fg_pix = *fit, bg_pix = *bit; 67 | // float alpha = fg_pix[3] * inv_scale, beta = bg_pix[3] * inv_scale; 68 | // *dst_it = 69 | // VT(saturate_cast(fg_pix[0] * alpha + bg_pix[0] * beta), 70 | // saturate_cast(fg_pix[1] * alpha + bg_pix[1] * beta), 71 | // saturate_cast(fg_pix[2] * alpha + bg_pix[2] * beta), 72 | // saturate_cast((1 - (1 - alpha) * (1 - beta)) * alpha_scale)); 73 | // } 74 | 75 | // // // start at the row indicated by location, or at row 0 if location.y 76 | // is 77 | // // // negative. 78 | // // for (int fY = 0; (fY < background.rows) && (fY < foreground.rows); 79 | // ++fY) 80 | // // { 81 | 82 | // // // start at the column indicated by location, 83 | 84 | // // // or at column 0 if location.x is negative. 85 | // // for (int fX = 0; (fX < background.cols) && (fX < foreground.cols); 86 | // // ++fX) { 87 | 88 | // // // determine the opacity of the foregrond pixel, using its fourth 89 | // // (alpha) 90 | // // // channel. 91 | // // float alpha = foreground.at(fY, fX, 3) / 255.; 92 | // // // ((double)foreground 93 | // // // .data[fY * foreground.step + fX * foreground.channels() 94 | // + 95 | // // 3]) / 96 | // // // 255.; 97 | 98 | // // // and now combine the background and foreground pixel, using the 99 | // // // opacity, 100 | 101 | // // // but only if opacity > 0. 102 | // // for (int c = 0; alpha > 0 && c < output.channels(); ++c) { 103 | // // const unsigned char foregroundPx = 104 | // // foreground.at(fY, fX, c); 105 | // // const unsigned char backgroundPx = 106 | // // background.at(fY, fX, c); 107 | 108 | // // output.at(fY, fX, c) = 109 | // // backgroundPx * (1. - alpha) + foregroundPx * alpha; 110 | // // } 111 | // // } 112 | // // } 113 | // } 114 | }; // namespace sonar_image_proc 115 | -------------------------------------------------------------------------------- /include/sonar_image_proc/SonarDrawer.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | // This file contains the class-based API (which is more efficient) 4 | // as it can store and reuse intermediate results. 5 | // 6 | // See "DrawSonar.h" for the function-based API 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | #include "sonar_image_proc/AbstractSonarInterface.h" 14 | #include "sonar_image_proc/ColorMaps.h" 15 | 16 | namespace sonar_image_proc { 17 | 18 | using sonar_image_proc::AbstractSonarInterface; 19 | 20 | // Function in lib/OverlayImage.cpp 21 | void overlayImage(const cv::Mat &background, const cv::Mat &foreground, 22 | cv::Mat &output); 23 | 24 | #define OVERLAY_RW(var, tp, set, get) \ 25 | OverlayConfig &set(tp i) { \ 26 | var = i; \ 27 | return *this; \ 28 | } \ 29 | tp get() const { return var; } 30 | 31 | class SonarDrawer { 32 | public: 33 | struct OverlayConfig { 34 | public: 35 | int DEFAULT_LINE_THICKNESS = 1; 36 | float DEFAULT_LINE_ALPHA = 0.5; 37 | 38 | // range spacing of 0 means "calculate automatically" 39 | float DEFAULT_RANGE_SPACING = 0; 40 | 41 | float DEFAULT_RADIAL_SPACING = 20; // degrees 42 | bool DEFAULT_RADIAL_AT_ZERO = false; 43 | 44 | float DEFAULT_FONT_SCALE = 1.0; 45 | 46 | OverlayConfig() 47 | : line_thickness_(DEFAULT_LINE_THICKNESS), 48 | line_alpha_(DEFAULT_LINE_ALPHA), 49 | range_spacing_m_(DEFAULT_RANGE_SPACING), 50 | radial_spacing_deg_(DEFAULT_RADIAL_SPACING), 51 | radial_at_zero_(DEFAULT_RADIAL_AT_ZERO), 52 | font_scale_(DEFAULT_FONT_SCALE), 53 | line_color_(255, 255, 255) {} 54 | 55 | OVERLAY_RW(line_alpha_, float, setLineAlpha, lineAlpha) 56 | OVERLAY_RW(line_thickness_, int, setLineThickness, lineThickness) 57 | OVERLAY_RW(range_spacing_m_, float, setRangeSpacing, rangeSpacing) 58 | 59 | OVERLAY_RW(radial_spacing_deg_, float, setRadialSpacing, radialSpacing) 60 | OVERLAY_RW(radial_at_zero_, bool, setRadialAtZero, radialAtZero) 61 | OVERLAY_RW(font_scale_, float, setFontScale, fontScale) 62 | 63 | OverlayConfig &setLineColor(const cv::Vec3b &v) { 64 | line_color_ = v; 65 | return *this; 66 | } 67 | cv::Vec3b lineColor() const { return line_color_; } 68 | 69 | bool operator!=(const OverlayConfig &other) const { 70 | return (lineThickness() != other.lineThickness()) || 71 | (lineAlpha() != other.lineAlpha()) || 72 | (rangeSpacing() != other.rangeSpacing()) || 73 | (radialSpacing() != other.radialSpacing()) || 74 | (radialAtZero() != other.radialAtZero()) || 75 | (fontScale() != other.fontScale()) || 76 | (lineColor() != other.lineColor()); 77 | } 78 | 79 | private: 80 | int line_thickness_; 81 | float line_alpha_; 82 | float range_spacing_m_; 83 | float radial_spacing_deg_; 84 | bool radial_at_zero_; 85 | float font_scale_; 86 | 87 | cv::Vec3b line_color_; 88 | }; 89 | 90 | SonarDrawer(); 91 | 92 | // Calls drawRectSonarImage followed by remapRectSonarImage inline 93 | // The intermediate rectangular image is not returned, if required, 94 | // use the two functions individually... 95 | cv::Mat drawSonar(const AbstractSonarInterface &ping, 96 | const SonarColorMap &colorMap = InfernoColorMap(), 97 | const cv::Mat &image = cv::Mat(0, 0, CV_8UC3), 98 | bool addOverlay = false); 99 | 100 | // Maps the sonar ping to an RGB image. 101 | // rectImage is reshaped to be numRanges rows x numBearings columns 102 | // 103 | // If rectImage is either 8UC3 or 32FC3, it retains that type, otherwise 104 | // rectImage is converted to 8UC3 105 | // 106 | // Cell (0,0) is the color mapping of the data with the smallest range and 107 | // smallest (typically, most negative) bearing in the ping. 108 | // 109 | // Cell (nRange,0) is the data at the max range, most negative bearing 110 | // 111 | // Cell (nRange,nBearing) is the data at the max range, most positive 112 | // bearing 113 | // 114 | cv::Mat drawRectSonarImage(const AbstractSonarInterface &ping, 115 | const SonarColorMap &colorMap = InfernoColorMap(), 116 | const cv::Mat &rectImage = cv::Mat(0, 0, CV_8UC3)); 117 | 118 | cv::Mat remapRectSonarImage(const AbstractSonarInterface &ping, 119 | const cv::Mat &rectImage); 120 | 121 | // Creates a copy of sonarImage with the graphical overlay using the 122 | // configuration in overlayConfig 123 | cv::Mat drawOverlay(const AbstractSonarInterface &ping, 124 | const cv::Mat &sonarImage); 125 | 126 | OverlayConfig &overlayConfig() { return overlay_config_; } 127 | 128 | private: 129 | OverlayConfig overlay_config_; 130 | 131 | // Utility class which can generate and store the two cv::Mats 132 | // required for the cv::remap() function 133 | // 134 | // Also stores meta-information to determine if the map is 135 | // invalid and needs to be regenerated. 136 | struct Cached { 137 | public: 138 | Cached() { ; } 139 | 140 | protected: 141 | virtual bool isValid(const AbstractSonarInterface &ping) const; 142 | 143 | // Meta-information to validate map 144 | std::pair _rangeBounds, _azimuthBounds; 145 | int _numRanges, _numAzimuth; 146 | }; 147 | 148 | struct CachedMap : public Cached { 149 | public: 150 | CachedMap() : Cached() { ; } 151 | typedef std::pair MapPair; 152 | 153 | MapPair operator()(const AbstractSonarInterface &ping); 154 | 155 | private: 156 | bool isValid(const AbstractSonarInterface &ping) const override; 157 | void create(const AbstractSonarInterface &ping); 158 | 159 | cv::Mat _scMap1, _scMap2; 160 | } _map; 161 | 162 | struct CachedOverlay : public Cached { 163 | public: 164 | CachedOverlay() : Cached() { ; } 165 | 166 | const cv::Mat &operator()(const AbstractSonarInterface &ping, 167 | const cv::Mat &sonarImage, 168 | const OverlayConfig &config); 169 | 170 | private: 171 | bool isValid(const AbstractSonarInterface &ping, const cv::Mat &sonarImage, 172 | const OverlayConfig &config) const; 173 | 174 | void create(const AbstractSonarInterface &ping, const cv::Mat &sonarImage, 175 | const OverlayConfig &config); 176 | 177 | cv::Mat _overlay; 178 | OverlayConfig _config_used; 179 | 180 | } _overlay; 181 | 182 | }; // namespace sonar_image_procclassSonarDrawer 183 | 184 | } // namespace sonar_image_proc 185 | -------------------------------------------------------------------------------- /launch/sonar_postproc.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /lib/AbstractSonarInterface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #include "sonar_image_proc/AbstractSonarInterface.h" 5 | 6 | #include 7 | #include 8 | 9 | namespace sonar_image_proc { 10 | 11 | const Bounds_t UnsetBounds = Bounds_t(-1, -1); 12 | 13 | AbstractSonarInterface::AbstractSonarInterface() 14 | : _rangeBounds(UnsetBounds), _azimuthBounds(UnsetBounds) {} 15 | 16 | Bounds_t AbstractSonarInterface::azimuthBounds() const { 17 | checkAzimuthBounds(); 18 | return _azimuthBounds; 19 | } 20 | 21 | Bounds_t AbstractSonarInterface::rangeBounds() const { 22 | checkRangeBounds(); 23 | return _rangeBounds; 24 | } 25 | 26 | void AbstractSonarInterface::checkRangeBounds() const { 27 | if (_rangeBounds == UnsetBounds) { 28 | auto results = std::minmax_element(ranges().begin(), ranges().end()); 29 | _rangeBounds = std::make_pair(*(results.first), *(results.second)); 30 | 31 | _maxRangeSquared = _rangeBounds.second * _rangeBounds.second; 32 | } 33 | } 34 | 35 | void AbstractSonarInterface::checkAzimuthBounds() const { 36 | if (_azimuthBounds == UnsetBounds) { 37 | auto results = std::minmax_element(azimuths().begin(), azimuths().end()); 38 | _azimuthBounds = std::make_pair(*(results.first), *(results.second)); 39 | 40 | _minAzimuthTan = std::tan(_azimuthBounds.first); 41 | _maxAzimuthTan = std::tan(_azimuthBounds.second); 42 | } 43 | } 44 | 45 | } // namespace sonar_image_proc 46 | -------------------------------------------------------------------------------- /lib/ColorMaps.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | // Color map data from 4 | // https://github.com/BIDS/colormap/blob/master/colormaps.py 5 | // 6 | // As used in matplotlib 7 | // As released under a CC0 license 8 | // 9 | // This file autogenerated with helpers/make_color_map.py 10 | 11 | #include "sonar_image_proc/ColorMaps.h" 12 | 13 | namespace sonar_image_proc { 14 | 15 | const float InfernoColorMap::_inferno_data_float[256][3] = { 16 | {0.001462, 0.000466, 0.013866}, {0.002267, 0.001270, 0.018570}, 17 | {0.003299, 0.002249, 0.024239}, {0.004547, 0.003392, 0.030909}, 18 | {0.006006, 0.004692, 0.038558}, {0.007676, 0.006136, 0.046836}, 19 | {0.009561, 0.007713, 0.055143}, {0.011663, 0.009417, 0.063460}, 20 | {0.013995, 0.011225, 0.071862}, {0.016561, 0.013136, 0.080282}, 21 | {0.019373, 0.015133, 0.088767}, {0.022447, 0.017199, 0.097327}, 22 | {0.025793, 0.019331, 0.105930}, {0.029432, 0.021503, 0.114621}, 23 | {0.033385, 0.023702, 0.123397}, {0.037668, 0.025921, 0.132232}, 24 | {0.042253, 0.028139, 0.141141}, {0.046915, 0.030324, 0.150164}, 25 | {0.051644, 0.032474, 0.159254}, {0.056449, 0.034569, 0.168414}, 26 | {0.061340, 0.036590, 0.177642}, {0.066331, 0.038504, 0.186962}, 27 | {0.071429, 0.040294, 0.196354}, {0.076637, 0.041905, 0.205799}, 28 | {0.081962, 0.043328, 0.215289}, {0.087411, 0.044556, 0.224813}, 29 | {0.092990, 0.045583, 0.234358}, {0.098702, 0.046402, 0.243904}, 30 | {0.104551, 0.047008, 0.253430}, {0.110536, 0.047399, 0.262912}, 31 | {0.116656, 0.047574, 0.272321}, {0.122908, 0.047536, 0.281624}, 32 | {0.129285, 0.047293, 0.290788}, {0.135778, 0.046856, 0.299776}, 33 | {0.142378, 0.046242, 0.308553}, {0.149073, 0.045468, 0.317085}, 34 | {0.155850, 0.044559, 0.325338}, {0.162689, 0.043554, 0.333277}, 35 | {0.169575, 0.042489, 0.340874}, {0.176493, 0.041402, 0.348111}, 36 | {0.183429, 0.040329, 0.354971}, {0.190367, 0.039309, 0.361447}, 37 | {0.197297, 0.038400, 0.367535}, {0.204209, 0.037632, 0.373238}, 38 | {0.211095, 0.037030, 0.378563}, {0.217949, 0.036615, 0.383522}, 39 | {0.224763, 0.036405, 0.388129}, {0.231538, 0.036405, 0.392400}, 40 | {0.238273, 0.036621, 0.396353}, {0.244967, 0.037055, 0.400007}, 41 | {0.251620, 0.037705, 0.403378}, {0.258234, 0.038571, 0.406485}, 42 | {0.264810, 0.039647, 0.409345}, {0.271347, 0.040922, 0.411976}, 43 | {0.277850, 0.042353, 0.414392}, {0.284321, 0.043933, 0.416608}, 44 | {0.290763, 0.045644, 0.418637}, {0.297178, 0.047470, 0.420491}, 45 | {0.303568, 0.049396, 0.422182}, {0.309935, 0.051407, 0.423721}, 46 | {0.316282, 0.053490, 0.425116}, {0.322610, 0.055634, 0.426377}, 47 | {0.328921, 0.057827, 0.427511}, {0.335217, 0.060060, 0.428524}, 48 | {0.341500, 0.062325, 0.429425}, {0.347771, 0.064616, 0.430217}, 49 | {0.354032, 0.066925, 0.430906}, {0.360284, 0.069247, 0.431497}, 50 | {0.366529, 0.071579, 0.431994}, {0.372768, 0.073915, 0.432400}, 51 | {0.379001, 0.076253, 0.432719}, {0.385228, 0.078591, 0.432955}, 52 | {0.391453, 0.080927, 0.433109}, {0.397674, 0.083257, 0.433183}, 53 | {0.403894, 0.085580, 0.433179}, {0.410113, 0.087896, 0.433098}, 54 | {0.416331, 0.090203, 0.432943}, {0.422549, 0.092501, 0.432714}, 55 | {0.428768, 0.094790, 0.432412}, {0.434987, 0.097069, 0.432039}, 56 | {0.441207, 0.099338, 0.431594}, {0.447428, 0.101597, 0.431080}, 57 | {0.453651, 0.103848, 0.430498}, {0.459875, 0.106089, 0.429846}, 58 | {0.466100, 0.108322, 0.429125}, {0.472328, 0.110547, 0.428334}, 59 | {0.478558, 0.112764, 0.427475}, {0.484789, 0.114974, 0.426548}, 60 | {0.491022, 0.117179, 0.425552}, {0.497257, 0.119379, 0.424488}, 61 | {0.503493, 0.121575, 0.423356}, {0.509730, 0.123769, 0.422156}, 62 | {0.515967, 0.125960, 0.420887}, {0.522206, 0.128150, 0.419549}, 63 | {0.528444, 0.130341, 0.418142}, {0.534683, 0.132534, 0.416667}, 64 | {0.540920, 0.134729, 0.415123}, {0.547157, 0.136929, 0.413511}, 65 | {0.553392, 0.139134, 0.411829}, {0.559624, 0.141346, 0.410078}, 66 | {0.565854, 0.143567, 0.408258}, {0.572081, 0.145797, 0.406369}, 67 | {0.578304, 0.148039, 0.404411}, {0.584521, 0.150294, 0.402385}, 68 | {0.590734, 0.152563, 0.400290}, {0.596940, 0.154848, 0.398125}, 69 | {0.603139, 0.157151, 0.395891}, {0.609330, 0.159474, 0.393589}, 70 | {0.615513, 0.161817, 0.391219}, {0.621685, 0.164184, 0.388781}, 71 | {0.627847, 0.166575, 0.386276}, {0.633998, 0.168992, 0.383704}, 72 | {0.640135, 0.171438, 0.381065}, {0.646260, 0.173914, 0.378359}, 73 | {0.652369, 0.176421, 0.375586}, {0.658463, 0.178962, 0.372748}, 74 | {0.664540, 0.181539, 0.369846}, {0.670599, 0.184153, 0.366879}, 75 | {0.676638, 0.186807, 0.363849}, {0.682656, 0.189501, 0.360757}, 76 | {0.688653, 0.192239, 0.357603}, {0.694627, 0.195021, 0.354388}, 77 | {0.700576, 0.197851, 0.351113}, {0.706500, 0.200728, 0.347777}, 78 | {0.712396, 0.203656, 0.344383}, {0.718264, 0.206636, 0.340931}, 79 | {0.724103, 0.209670, 0.337424}, {0.729909, 0.212759, 0.333861}, 80 | {0.735683, 0.215906, 0.330245}, {0.741423, 0.219112, 0.326576}, 81 | {0.747127, 0.222378, 0.322856}, {0.752794, 0.225706, 0.319085}, 82 | {0.758422, 0.229097, 0.315266}, {0.764010, 0.232554, 0.311399}, 83 | {0.769556, 0.236077, 0.307485}, {0.775059, 0.239667, 0.303526}, 84 | {0.780517, 0.243327, 0.299523}, {0.785929, 0.247056, 0.295477}, 85 | {0.791293, 0.250856, 0.291390}, {0.796607, 0.254728, 0.287264}, 86 | {0.801871, 0.258674, 0.283099}, {0.807082, 0.262692, 0.278898}, 87 | {0.812239, 0.266786, 0.274661}, {0.817341, 0.270954, 0.270390}, 88 | {0.822386, 0.275197, 0.266085}, {0.827372, 0.279517, 0.261750}, 89 | {0.832299, 0.283913, 0.257383}, {0.837165, 0.288385, 0.252988}, 90 | {0.841969, 0.292933, 0.248564}, {0.846709, 0.297559, 0.244113}, 91 | {0.851384, 0.302260, 0.239636}, {0.855992, 0.307038, 0.235133}, 92 | {0.860533, 0.311892, 0.230606}, {0.865006, 0.316822, 0.226055}, 93 | {0.869409, 0.321827, 0.221482}, {0.873741, 0.326906, 0.216886}, 94 | {0.878001, 0.332060, 0.212268}, {0.882188, 0.337287, 0.207628}, 95 | {0.886302, 0.342586, 0.202968}, {0.890341, 0.347957, 0.198286}, 96 | {0.894305, 0.353399, 0.193584}, {0.898192, 0.358911, 0.188860}, 97 | {0.902003, 0.364492, 0.184116}, {0.905735, 0.370140, 0.179350}, 98 | {0.909390, 0.375856, 0.174563}, {0.912966, 0.381636, 0.169755}, 99 | {0.916462, 0.387481, 0.164924}, {0.919879, 0.393389, 0.160070}, 100 | {0.923215, 0.399359, 0.155193}, {0.926470, 0.405389, 0.150292}, 101 | {0.929644, 0.411479, 0.145367}, {0.932737, 0.417627, 0.140417}, 102 | {0.935747, 0.423831, 0.135440}, {0.938675, 0.430091, 0.130438}, 103 | {0.941521, 0.436405, 0.125409}, {0.944285, 0.442772, 0.120354}, 104 | {0.946965, 0.449191, 0.115272}, {0.949562, 0.455660, 0.110164}, 105 | {0.952075, 0.462178, 0.105031}, {0.954506, 0.468744, 0.099874}, 106 | {0.956852, 0.475356, 0.094695}, {0.959114, 0.482014, 0.089499}, 107 | {0.961293, 0.488716, 0.084289}, {0.963387, 0.495462, 0.079073}, 108 | {0.965397, 0.502249, 0.073859}, {0.967322, 0.509078, 0.068659}, 109 | {0.969163, 0.515946, 0.063488}, {0.970919, 0.522853, 0.058367}, 110 | {0.972590, 0.529798, 0.053324}, {0.974176, 0.536780, 0.048392}, 111 | {0.975677, 0.543798, 0.043618}, {0.977092, 0.550850, 0.039050}, 112 | {0.978422, 0.557937, 0.034931}, {0.979666, 0.565057, 0.031409}, 113 | {0.980824, 0.572209, 0.028508}, {0.981895, 0.579392, 0.026250}, 114 | {0.982881, 0.586606, 0.024661}, {0.983779, 0.593849, 0.023770}, 115 | {0.984591, 0.601122, 0.023606}, {0.985315, 0.608422, 0.024202}, 116 | {0.985952, 0.615750, 0.025592}, {0.986502, 0.623105, 0.027814}, 117 | {0.986964, 0.630485, 0.030908}, {0.987337, 0.637890, 0.034916}, 118 | {0.987622, 0.645320, 0.039886}, {0.987819, 0.652773, 0.045581}, 119 | {0.987926, 0.660250, 0.051750}, {0.987945, 0.667748, 0.058329}, 120 | {0.987874, 0.675267, 0.065257}, {0.987714, 0.682807, 0.072489}, 121 | {0.987464, 0.690366, 0.079990}, {0.987124, 0.697944, 0.087731}, 122 | {0.986694, 0.705540, 0.095694}, {0.986175, 0.713153, 0.103863}, 123 | {0.985566, 0.720782, 0.112229}, {0.984865, 0.728427, 0.120785}, 124 | {0.984075, 0.736087, 0.129527}, {0.983196, 0.743758, 0.138453}, 125 | {0.982228, 0.751442, 0.147565}, {0.981173, 0.759135, 0.156863}, 126 | {0.980032, 0.766837, 0.166353}, {0.978806, 0.774545, 0.176037}, 127 | {0.977497, 0.782258, 0.185923}, {0.976108, 0.789974, 0.196018}, 128 | {0.974638, 0.797692, 0.206332}, {0.973088, 0.805409, 0.216877}, 129 | {0.971468, 0.813122, 0.227658}, {0.969783, 0.820825, 0.238686}, 130 | {0.968041, 0.828515, 0.249972}, {0.966243, 0.836191, 0.261534}, 131 | {0.964394, 0.843848, 0.273391}, {0.962517, 0.851476, 0.285546}, 132 | {0.960626, 0.859069, 0.298010}, {0.958720, 0.866624, 0.310820}, 133 | {0.956834, 0.874129, 0.323974}, {0.954997, 0.881569, 0.337475}, 134 | {0.953215, 0.888942, 0.351369}, {0.951546, 0.896226, 0.365627}, 135 | {0.950018, 0.903409, 0.380271}, {0.948683, 0.910473, 0.395289}, 136 | {0.947594, 0.917399, 0.410665}, {0.946809, 0.924168, 0.426373}, 137 | {0.946392, 0.930761, 0.442367}, {0.946403, 0.937159, 0.458592}, 138 | {0.946903, 0.943348, 0.474970}, {0.947937, 0.949318, 0.491426}, 139 | {0.949545, 0.955063, 0.507860}, {0.951740, 0.960587, 0.524203}, 140 | {0.954529, 0.965896, 0.540361}, {0.957896, 0.971003, 0.556275}, 141 | {0.961812, 0.975924, 0.571925}, {0.966249, 0.980678, 0.587206}, 142 | {0.971162, 0.985282, 0.602154}, {0.976511, 0.989753, 0.616760}, 143 | {0.982257, 0.994109, 0.631017}, {0.988362, 0.998364, 0.644924}, 144 | }; 145 | 146 | const float InfernoColorMap::_inferno_data_uint8[256][3] = { 147 | {0, 0, 3}, {0, 0, 4}, {0, 0, 6}, {1, 0, 7}, 148 | {1, 1, 9}, {1, 1, 11}, {2, 1, 14}, {2, 2, 16}, 149 | {3, 2, 18}, {4, 3, 20}, {4, 3, 22}, {5, 4, 24}, 150 | {6, 4, 27}, {7, 5, 29}, {8, 6, 31}, {9, 6, 33}, 151 | {10, 7, 35}, {11, 7, 38}, {13, 8, 40}, {14, 8, 42}, 152 | {15, 9, 45}, {16, 9, 47}, {18, 10, 50}, {19, 10, 52}, 153 | {20, 11, 54}, {22, 11, 57}, {23, 11, 59}, {25, 11, 62}, 154 | {26, 11, 64}, {28, 12, 67}, {29, 12, 69}, {31, 12, 71}, 155 | {32, 12, 74}, {34, 11, 76}, {36, 11, 78}, {38, 11, 80}, 156 | {39, 11, 82}, {41, 11, 84}, {43, 10, 86}, {45, 10, 88}, 157 | {46, 10, 90}, {48, 10, 92}, {50, 9, 93}, {52, 9, 95}, 158 | {53, 9, 96}, {55, 9, 97}, {57, 9, 98}, {59, 9, 100}, 159 | {60, 9, 101}, {62, 9, 102}, {64, 9, 102}, {65, 9, 103}, 160 | {67, 10, 104}, {69, 10, 105}, {70, 10, 105}, {72, 11, 106}, 161 | {74, 11, 106}, {75, 12, 107}, {77, 12, 107}, {79, 13, 108}, 162 | {80, 13, 108}, {82, 14, 108}, {83, 14, 109}, {85, 15, 109}, 163 | {87, 15, 109}, {88, 16, 109}, {90, 17, 109}, {91, 17, 110}, 164 | {93, 18, 110}, {95, 18, 110}, {96, 19, 110}, {98, 20, 110}, 165 | {99, 20, 110}, {101, 21, 110}, {102, 21, 110}, {104, 22, 110}, 166 | {106, 23, 110}, {107, 23, 110}, {109, 24, 110}, {110, 24, 110}, 167 | {112, 25, 110}, {114, 25, 109}, {115, 26, 109}, {117, 27, 109}, 168 | {118, 27, 109}, {120, 28, 109}, {122, 28, 109}, {123, 29, 108}, 169 | {125, 29, 108}, {126, 30, 108}, {128, 31, 107}, {129, 31, 107}, 170 | {131, 32, 107}, {133, 32, 106}, {134, 33, 106}, {136, 33, 106}, 171 | {137, 34, 105}, {139, 34, 105}, {141, 35, 105}, {142, 36, 104}, 172 | {144, 36, 104}, {145, 37, 103}, {147, 37, 103}, {149, 38, 102}, 173 | {150, 38, 102}, {152, 39, 101}, {153, 40, 100}, {155, 40, 100}, 174 | {156, 41, 99}, {158, 41, 99}, {160, 42, 98}, {161, 43, 97}, 175 | {163, 43, 97}, {164, 44, 96}, {166, 44, 95}, {167, 45, 95}, 176 | {169, 46, 94}, {171, 46, 93}, {172, 47, 92}, {174, 48, 91}, 177 | {175, 49, 91}, {177, 49, 90}, {178, 50, 89}, {180, 51, 88}, 178 | {181, 51, 87}, {183, 52, 86}, {184, 53, 86}, {186, 54, 85}, 179 | {187, 55, 84}, {189, 55, 83}, {190, 56, 82}, {191, 57, 81}, 180 | {193, 58, 80}, {194, 59, 79}, {196, 60, 78}, {197, 61, 77}, 181 | {199, 62, 76}, {200, 62, 75}, {201, 63, 74}, {203, 64, 73}, 182 | {204, 65, 72}, {205, 66, 71}, {207, 68, 70}, {208, 69, 68}, 183 | {209, 70, 67}, {210, 71, 66}, {212, 72, 65}, {213, 73, 64}, 184 | {214, 74, 63}, {215, 75, 62}, {217, 77, 61}, {218, 78, 59}, 185 | {219, 79, 58}, {220, 80, 57}, {221, 82, 56}, {222, 83, 55}, 186 | {223, 84, 54}, {224, 86, 52}, {226, 87, 51}, {227, 88, 50}, 187 | {228, 90, 49}, {229, 91, 48}, {230, 92, 46}, {230, 94, 45}, 188 | {231, 95, 44}, {232, 97, 43}, {233, 98, 42}, {234, 100, 40}, 189 | {235, 101, 39}, {236, 103, 38}, {237, 104, 37}, {237, 106, 35}, 190 | {238, 108, 34}, {239, 109, 33}, {240, 111, 31}, {240, 112, 30}, 191 | {241, 114, 29}, {242, 116, 28}, {242, 117, 26}, {243, 119, 25}, 192 | {243, 121, 24}, {244, 122, 22}, {245, 124, 21}, {245, 126, 20}, 193 | {246, 128, 18}, {246, 129, 17}, {247, 131, 16}, {247, 133, 14}, 194 | {248, 135, 13}, {248, 136, 12}, {248, 138, 11}, {249, 140, 9}, 195 | {249, 142, 8}, {249, 144, 8}, {250, 145, 7}, {250, 147, 6}, 196 | {250, 149, 6}, {250, 151, 6}, {251, 153, 6}, {251, 155, 6}, 197 | {251, 157, 6}, {251, 158, 7}, {251, 160, 7}, {251, 162, 8}, 198 | {251, 164, 10}, {251, 166, 11}, {251, 168, 13}, {251, 170, 14}, 199 | {251, 172, 16}, {251, 174, 18}, {251, 176, 20}, {251, 177, 22}, 200 | {251, 179, 24}, {251, 181, 26}, {251, 183, 28}, {251, 185, 30}, 201 | {250, 187, 33}, {250, 189, 35}, {250, 191, 37}, {250, 193, 40}, 202 | {249, 195, 42}, {249, 197, 44}, {249, 199, 47}, {248, 201, 49}, 203 | {248, 203, 52}, {248, 205, 55}, {247, 207, 58}, {247, 209, 60}, 204 | {246, 211, 63}, {246, 213, 66}, {245, 215, 69}, {245, 217, 72}, 205 | {244, 219, 75}, {244, 220, 79}, {243, 222, 82}, {243, 224, 86}, 206 | {243, 226, 89}, {242, 228, 93}, {242, 230, 96}, {241, 232, 100}, 207 | {241, 233, 104}, {241, 235, 108}, {241, 237, 112}, {241, 238, 116}, 208 | {241, 240, 121}, {241, 242, 125}, {242, 243, 129}, {242, 244, 133}, 209 | {243, 246, 137}, {244, 247, 141}, {245, 248, 145}, {246, 250, 149}, 210 | {247, 251, 153}, {249, 252, 157}, {250, 253, 160}, {252, 254, 164}, 211 | }; 212 | 213 | } // namespace sonar_image_proc 214 | -------------------------------------------------------------------------------- /lib/HistogramGenerator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #include "sonar_image_proc/HistogramGenerator.h" 5 | 6 | #include 7 | 8 | namespace sonar_image_proc { 9 | 10 | std::vector HistogramGenerator::Generate( 11 | const AbstractSonarInterface &ping) { 12 | if (ping.data_type() == AbstractSonarInterface::TYPE_UINT8) 13 | return HistogramGenerator::GenerateUint8(ping); 14 | else if (ping.data_type() == AbstractSonarInterface::TYPE_UINT16) 15 | return HistogramGenerator::GenerateUint16(ping); 16 | else if (ping.data_type() == AbstractSonarInterface::TYPE_UINT32) 17 | return HistogramGenerator::GenerateUint32(ping); 18 | 19 | return std::vector(); 20 | } 21 | 22 | // \todo Some repetition, but these functions are pretty simple 23 | std::vector HistogramGenerator::GenerateUint8( 24 | const AbstractSonarInterface &ping) { 25 | std::vector result(256, 0); 26 | 27 | for (int r = 0; r < ping.nRanges(); r++) { 28 | for (int b = 0; b < ping.nBearings(); b++) { 29 | const auto val = ping.intensity_uint8(AzimuthRangeIndices(b, r)); 30 | result[val]++; 31 | } 32 | } 33 | 34 | return result; 35 | } 36 | 37 | std::vector HistogramGenerator::GenerateUint16( 38 | const AbstractSonarInterface &ping) { 39 | std::vector result(65536, 0); 40 | 41 | for (int r = 0; r < ping.nRanges(); r++) { 42 | for (int b = 0; b < ping.nBearings(); b++) { 43 | const auto val = ping.intensity_uint16(AzimuthRangeIndices(b, r)); 44 | 45 | result[val]++; 46 | } 47 | } 48 | 49 | return result; 50 | } 51 | 52 | std::vector HistogramGenerator::GenerateUint32( 53 | const AbstractSonarInterface &ping) { 54 | std::vector result(256, 0); 55 | 56 | const float logMax = log10(UINT32_MAX); 57 | 58 | for (int r = 0; r < ping.nRanges(); r++) { 59 | for (int b = 0; b < ping.nBearings(); b++) { 60 | const auto val = ping.intensity_uint32(AzimuthRangeIndices(b, r)); 61 | 62 | if (val == 0) continue; 63 | 64 | const float l = log10(val); 65 | 66 | const size_t idx = UINT8_MAX * (l / logMax); 67 | 68 | // std::cerr << "val = " << val << "; l = " << l << "; idx = " << idx << 69 | // std::endl; 70 | 71 | if ((idx < 0) || (idx > result.size())) continue; 72 | result[idx]++; 73 | } 74 | } 75 | 76 | return result; 77 | } 78 | 79 | } // namespace sonar_image_proc 80 | -------------------------------------------------------------------------------- /lib/OldDrawSonar.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | // Contains the old "legacy" (that is, noticably worse) implementation 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "sonar_image_proc/DrawSonar.h" 10 | 11 | #ifndef THETA_SHIFT 12 | #define THETA_SHIFT PI; 13 | #endif 14 | 15 | namespace sonar_image_proc { 16 | 17 | namespace old_api { 18 | 19 | using cv::Mat; 20 | using cv::Size; 21 | using std::vector; 22 | 23 | const float ThetaShift = 1.5 * M_PI; 24 | 25 | cv::Size calculateImageSize(const AbstractSonarInterface &ping, cv::Size hint, 26 | int pixPerRangeBin, float maxRange) { 27 | int h = hint.height, w = hint.width; 28 | 29 | if (w <= 0) { 30 | if (h <= 0) { 31 | const float rangeMax = ((maxRange > 0.0) ? maxRange : ping.maxRange()); 32 | const float rangeRes = 33 | (ping.maxRange() - ping.minRange()) / ping.nRanges(); 34 | 35 | const int nEffectiveRanges = ceil(rangeMax / rangeRes); 36 | 37 | h = nEffectiveRanges * pixPerRangeBin; 38 | } 39 | 40 | // Assume bearings are symmetric plus and minus 41 | // Bearings must be radians 42 | w = 2 * ceil(fabs(h * sin(ping.bearing(0)))); 43 | } else if (h <= 0) { 44 | h = (w / 2) / ceil(fabs(sin(ping.bearing(0)))); 45 | } 46 | 47 | // Ensure w and h are both divisible by zero 48 | if (w % 2) w++; 49 | if (h % 2) h++; 50 | 51 | return Size(w, h); 52 | } 53 | 54 | cv::Mat drawSonar(const AbstractSonarInterface &ping, const Mat &mat, 55 | const SonarColorMap &colorMap, float maxRange) { 56 | // Ensure mat is 8UC3; 57 | cv::Mat out(mat); 58 | out.create(mat.size(), CV_8UC3); 59 | out.setTo(cv::Vec3b(0, 0, 0)); 60 | 61 | const int nRanges = ping.nRanges(); 62 | const int nBeams = ping.nBearings(); 63 | 64 | const float rangeMax = (maxRange > 0.0 ? maxRange : ping.maxRange()); 65 | 66 | // Calculate effective range resolution of the output image. 67 | // The sensor's original resolution 68 | const float rangeRes = (ping.maxRange() - ping.minRange()) / ping.nRanges(); 69 | 70 | // How many ranges are required to go from 0 to rangeMax (since the 71 | // sensor starts at some minimum) 72 | const int nEffectiveRanges = ceil(rangeMax / rangeRes); 73 | 74 | // Todo. Calculate offset for non-zero minimum ranges 75 | const unsigned int radius = mat.size().height; 76 | // This effectively flips the origin, presumably so that it will appear 77 | // at the bottom fo the image. 78 | const cv::Point origin(mat.size().width / 2, mat.size().height); 79 | 80 | // QUESTION: Why the factor of 2? 81 | // If I understand correctly, binThickness is the width 82 | // of the range-bin, in pixels. 83 | const float binThickness = 2 * ceil(radius / nEffectiveRanges); 84 | 85 | struct BearingEntry { 86 | float begin, center, end; 87 | 88 | BearingEntry(float b, float c, float e) : begin(b), center(c), end(e) { ; } 89 | }; 90 | 91 | vector angles; 92 | angles.reserve(nBeams); 93 | 94 | for (int b = 0; b < nBeams; ++b) { 95 | const float center = ping.bearing(b); 96 | float begin = 0.0, end = 0.0; 97 | 98 | if (b == 0) { 99 | end = (ping.bearing(b + 1) + center) / 2.0; 100 | begin = 2 * center - end; 101 | 102 | } else if (b == nBeams - 1) { 103 | begin = angles[b - 1].end; 104 | end = 2 * center - begin; 105 | 106 | } else { 107 | begin = angles[b - 1].end; 108 | end = (ping.bearing(b + 1) + center) / 2.0; 109 | } 110 | 111 | angles.push_back(BearingEntry(begin, center, end)); 112 | } 113 | 114 | for (int r = 0; r < nRanges; ++r) { 115 | if (ping.range(r) > rangeMax) continue; 116 | 117 | for (int b = 0; b < nBeams; ++b) { 118 | const float range = ping.range(r); 119 | 120 | // QUESTION: Why are we rotating here? 121 | const float begin = angles[b].begin + ThetaShift, 122 | end = angles[b].end + ThetaShift; 123 | 124 | const float rad = static_cast(radius) * range / rangeMax; 125 | 126 | // Assume angles are in image frame x-right, y-down 127 | cv::ellipse(out, origin, cv::Size(rad, rad), 0, begin * 180 / M_PI, 128 | end * 180 / M_PI, 129 | colorMap.lookup_cv8uc3( 130 | ping, AzimuthRangeIndices(angles[b].center, range)), 131 | binThickness); 132 | } 133 | } 134 | 135 | return out; 136 | } 137 | 138 | } // namespace old_api 139 | } // namespace sonar_image_proc 140 | -------------------------------------------------------------------------------- /lib/SonarDrawer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "sonar_image_proc/DrawSonar.h" 9 | #include "sonar_image_proc/OverlayImage.h" 10 | 11 | namespace sonar_image_proc { 12 | 13 | using sonar_image_proc::AbstractSonarInterface; 14 | 15 | static float deg2radf(float deg) { return deg * M_PI / 180.0; } 16 | static float rad2degf(float rad) { return rad * 180.0 / M_PI; } 17 | 18 | SonarDrawer::SonarDrawer() { ; } 19 | 20 | cv::Mat SonarDrawer::drawRectSonarImage(const AbstractSonarInterface &ping, 21 | const SonarColorMap &colorMap, 22 | const cv::Mat &rectIn) { 23 | cv::Mat rect(rectIn); 24 | 25 | const cv::Size imgSize(ping.nRanges(), ping.nBearings()); 26 | 27 | if ((rect.type() == CV_8UC3) || (rect.type() == CV_32FC3) || 28 | (rect.type() == CV_32FC1)) { 29 | rect.create(imgSize, rect.type()); 30 | } else { 31 | rect.create(imgSize, CV_8UC3); 32 | } 33 | 34 | for (int r = 0; r < ping.nRanges(); r++) { 35 | for (int b = 0; b < ping.nBearings(); b++) { 36 | const AzimuthRangeIndices loc(b, r); 37 | 38 | if (rect.type() == CV_8UC3) { 39 | rect.at(cv::Point(r, b)) = colorMap.lookup_cv8uc3(ping, loc); 40 | } else if (rect.type() == CV_32FC3) { 41 | rect.at(cv::Point(r, b)) = 42 | colorMap.lookup_cv32fc3(ping, loc); 43 | } else if (rect.type() == CV_32FC1) { 44 | rect.at(cv::Point(r, b)) = colorMap.lookup_cv32fc1(ping, loc); 45 | } else { 46 | assert("Should never get here."); 47 | } 48 | } 49 | } 50 | 51 | return rect; 52 | } 53 | 54 | cv::Mat SonarDrawer::remapRectSonarImage(const AbstractSonarInterface &ping, 55 | const cv::Mat &rectImage) { 56 | cv::Mat out; 57 | const CachedMap::MapPair maps(_map(ping)); 58 | cv::remap(rectImage, out, maps.first, maps.second, cv::INTER_CUBIC, 59 | cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0)); 60 | 61 | return out; 62 | } 63 | 64 | cv::Mat SonarDrawer::drawOverlay(const AbstractSonarInterface &ping, 65 | const cv::Mat &sonarImage) { 66 | // Alpha blend overlay onto sonarImage 67 | cv::Mat output; 68 | overlayImage( 69 | sonarImage, _overlay(ping, sonarImage, overlayConfig()), output); 70 | 71 | return output; 72 | } 73 | 74 | cv::Mat SonarDrawer::drawSonar(const AbstractSonarInterface &ping, 75 | const SonarColorMap &colorMap, 76 | const cv::Mat &img, bool addOverlay) { 77 | cv::Mat rect = drawRectSonarImage(ping, colorMap, img); 78 | cv::Mat sonar = remapRectSonarImage(ping, rect); 79 | if (addOverlay) { 80 | return drawOverlay(ping, sonar); 81 | } else { 82 | return sonar; 83 | } 84 | } 85 | 86 | // ==== SonarDrawer::Cached ==== 87 | 88 | bool SonarDrawer::Cached::isValid(const AbstractSonarInterface &ping) const { 89 | // Check for cache invalidation... 90 | if ((_numAzimuth != ping.nAzimuth()) || (_numRanges != ping.nRanges()) || 91 | (_rangeBounds != ping.rangeBounds() || 92 | (_azimuthBounds != ping.azimuthBounds()))) 93 | return false; 94 | 95 | return true; 96 | } 97 | 98 | // ==== SonarDrawer::CachedMap ==== 99 | 100 | SonarDrawer::CachedMap::MapPair SonarDrawer::CachedMap::operator()( 101 | const AbstractSonarInterface &ping) { 102 | // _scMap[12] are mutable to break out of const 103 | if (!isValid(ping)) create(ping); 104 | 105 | return std::make_pair(_scMap1, _scMap2); 106 | } 107 | 108 | // **assumes** this structure for the rectImage: 109 | // * It has nBearings cols and nRanges rows 110 | // 111 | void SonarDrawer::CachedMap::create(const AbstractSonarInterface &ping) { 112 | cv::Mat newmap; 113 | 114 | const int nRanges = ping.nRanges(); 115 | const auto azimuthBounds = ping.azimuthBounds(); 116 | 117 | const int minusWidth = floor(nRanges * sin(azimuthBounds.first)); 118 | const int plusWidth = ceil(nRanges * sin(azimuthBounds.second)); 119 | const int width = plusWidth - minusWidth; 120 | 121 | const int originx = abs(minusWidth); 122 | 123 | const cv::Size imgSize(width, nRanges); 124 | if ((width <= 0) || (nRanges <= 0)) return; 125 | 126 | newmap.create(imgSize, CV_32FC2); 127 | 128 | const float db = 129 | (azimuthBounds.second - azimuthBounds.first) / ping.nAzimuth(); 130 | 131 | for (int x = 0; x < newmap.cols; x++) { 132 | for (int y = 0; y < newmap.rows; y++) { 133 | // For cv::remap, a map is 134 | // 135 | // dst = src( mapx(x,y), mapy(x,y) ) 136 | // 137 | // That is, the map is the size of the dst array, 138 | // and contains the coords in the source image 139 | // for each pixel in the dst image. 140 | // 141 | // This map draws the sonar with range = 0 142 | // centered on the bottom edge of the resulting image 143 | // with increasing range along azimuth = 0 going 144 | // vertically upwards in the image 145 | 146 | // Calculate range and bearing of this pixel from origin 147 | const float dx = x - originx; 148 | const float dy = newmap.rows - y; 149 | 150 | const float range = sqrt(dx * dx + dy * dy); 151 | const float azimuth = atan2(dx, dy); 152 | 153 | float xp = range; 154 | 155 | //\todo This linear algorithm is not robust if the azimuths 156 | // are non-linear. Should implement a real interpolation... 157 | float yp = (azimuth - azimuthBounds.first) / db; 158 | 159 | newmap.at(cv::Point(x, y)) = cv::Vec2f(xp, yp); 160 | } 161 | } 162 | 163 | cv::convertMaps(newmap, cv::Mat(), _scMap1, _scMap2, CV_16SC2); 164 | 165 | // Save meta-information to check for cache expiry 166 | _numRanges = ping.nRanges(); 167 | _numAzimuth = ping.nBearings(); 168 | 169 | _rangeBounds = ping.rangeBounds(); 170 | _azimuthBounds = ping.azimuthBounds(); 171 | } 172 | 173 | bool SonarDrawer::CachedMap::isValid(const AbstractSonarInterface &ping) const { 174 | if (_scMap1.empty() || _scMap2.empty()) return false; 175 | 176 | return Cached::isValid(ping); 177 | } 178 | 179 | // === SonarDrawer::CachedOverlay === 180 | 181 | bool SonarDrawer::CachedOverlay::isValid(const AbstractSonarInterface &ping, 182 | const cv::Mat &sonarImage, 183 | const OverlayConfig &config) const { 184 | if (sonarImage.size() != _overlay.size()) return false; 185 | 186 | if (_config_used != config) return false; 187 | 188 | return Cached::isValid(ping); 189 | } 190 | 191 | const cv::Mat &SonarDrawer::CachedOverlay::operator()( 192 | const AbstractSonarInterface &ping, const cv::Mat &sonarImage, 193 | const OverlayConfig &config) { 194 | if (!isValid(ping, sonarImage, config)) create(ping, sonarImage, config); 195 | 196 | return _overlay; 197 | } 198 | 199 | // Converts sonar bearing (with sonar "forward" at bearing 0) to image 200 | // orientation with sonar "forward" point upward in the image, which is the -Y 201 | // direction in image coordinates. 202 | static float bearingToImage(float d) { return (-M_PI / 2) + d; } 203 | 204 | void SonarDrawer::CachedOverlay::create(const AbstractSonarInterface &ping, 205 | const cv::Mat &sonarImage, 206 | const OverlayConfig &config) { 207 | const cv::Size sz(sonarImage.size()); 208 | const cv::Point2f origin(sz.width / 2, sz.height); 209 | 210 | // Reset overlay 211 | _overlay = cv::Mat::zeros(sz, CV_8UC4); 212 | const cv::Vec3b color(config.lineColor()); 213 | const cv::Vec4b textColor(color[0], color[1], color[2], 255); 214 | const cv::Vec4b lineColor(color[0], color[1], color[2], 215 | config.lineAlpha() * 255); 216 | 217 | const float minAzimuth = ping.minAzimuth(); 218 | const float maxAzimuth = ping.maxAzimuth(); 219 | 220 | const float maxRange = ping.maxRange(); 221 | 222 | //== Draw arcs == 223 | float arcSpacing = config.rangeSpacing(); 224 | 225 | if (arcSpacing <= 0) { 226 | // Calculate automatically .. just a lame heuristic for now 227 | if (maxRange < 2) 228 | arcSpacing = 0.5; 229 | else if (maxRange < 5) 230 | arcSpacing = 1.0; 231 | else if (maxRange < 10) 232 | arcSpacing = 2.0; 233 | else if (maxRange < 50) 234 | arcSpacing = 10.0; 235 | else 236 | arcSpacing = 20.0; 237 | } 238 | 239 | const float minRange = arcSpacing; 240 | 241 | for (float r = minRange; r < ping.maxRange(); r += arcSpacing) { 242 | const float radiusPix = (r / maxRange) * sonarImage.size().height; 243 | 244 | cv::ellipse(_overlay, origin, cv::Size(radiusPix, radiusPix), 0, 245 | rad2degf(bearingToImage(minAzimuth)), 246 | rad2degf(bearingToImage(maxAzimuth)), lineColor, 247 | config.lineThickness()); 248 | 249 | { 250 | std::stringstream rstr; 251 | rstr << r; 252 | 253 | // Calculate location of string 254 | const float theta = bearingToImage(minAzimuth); 255 | 256 | // \todo{??} Should calculate this automatically ... not sure 257 | const cv::Point2f offset(-25, 20); 258 | 259 | const cv::Point2f pt(radiusPix * cos(theta) + origin.x + offset.x, 260 | radiusPix * sin(theta) + origin.y + offset.y); 261 | 262 | cv::putText(_overlay, rstr.str(), pt, cv::FONT_HERSHEY_PLAIN, 263 | config.fontScale(), textColor); 264 | } 265 | } 266 | 267 | // And one arc at max range 268 | cv::ellipse(_overlay, origin, sz, 0, rad2degf(bearingToImage(minAzimuth)), 269 | rad2degf(bearingToImage(maxAzimuth)), lineColor, 270 | config.lineThickness()); 271 | 272 | //== Draw radials == 273 | std::vector radials; 274 | 275 | // Configuration ... move later 276 | const float radialSpacing = deg2radf(config.radialSpacing()); 277 | 278 | radials.push_back(minAzimuth); 279 | radials.push_back(maxAzimuth); 280 | 281 | // If radialSpacing == 0. draw only the outline radial lines 282 | if (radialSpacing > 0) { 283 | if (config.radialAtZero()) { 284 | // \todo(@amarburg) to implement 285 | } else { 286 | for (float d = radialSpacing / 2; d > minAzimuth && d < maxAzimuth; 287 | d += radialSpacing) { 288 | radials.push_back(d); 289 | } 290 | for (float d = -radialSpacing / 2; d > minAzimuth && d < maxAzimuth; 291 | d -= radialSpacing) { 292 | radials.push_back(d); 293 | } 294 | } 295 | } 296 | 297 | // Sort and unique 298 | std::sort(radials.begin(), radials.end()); 299 | auto last = std::unique(radials.begin(), radials.end()); 300 | radials.erase(last, radials.end()); 301 | 302 | // And draw 303 | const float minRangePix = (minRange / maxRange) * sz.height; 304 | 305 | if (minRange < maxRange) { 306 | for (const auto b : radials) { 307 | const float theta = bearingToImage(b); 308 | 309 | const cv::Point2f begin(minRangePix * cos(theta) + origin.x, 310 | minRangePix * sin(theta) + origin.y); 311 | const cv::Point2f end(sz.height * cos(theta) + origin.x, 312 | sz.height * sin(theta) + origin.y); 313 | 314 | cv::line(_overlay, begin, end, lineColor, config.lineThickness()); 315 | } 316 | } 317 | 318 | _config_used = config; 319 | } 320 | 321 | } // namespace sonar_image_proc 322 | -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A nodelet version of the DrawSonar 5 | 6 | 7 | 8 | 9 | 10 | A nodelet version of the SonarPostprocessor 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sonar_image_proc 4 | 0.0.1 5 | Common code for manipulating imaging sonar data. 6 | 7 | Aaron Marburg 8 | 9 | 10 | 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | roscpp 17 | rospy 18 | 19 | marine_acoustic_msgs 20 | cv_bridge 21 | image_transport 22 | nodelet_topic_tools 23 | std_msgs 24 | 25 | dynamic_reconfigure 26 | nodelet 27 | rosbag_storage 28 | 29 | dynamic_reconfigure 30 | nodelet 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /python/.gitignore: -------------------------------------------------------------------------------- 1 | *.egg-info/ 2 | -------------------------------------------------------------------------------- /python/README.md: -------------------------------------------------------------------------------- 1 | 2 | Borrows heavily from: 3 | 4 | [https://github.com/edmBernard/pybind11_opencv_numpy/blob/master/example.cpp](https://github.com/edmBernard/pybind11_opencv_numpy/blob/master/example.cpp) 5 | 6 | and 7 | 8 | [https://github.com/pybind/python_example](https://github.com/pybind/python_example) 9 | -------------------------------------------------------------------------------- /python/draw_sonar/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from py_draw_sonar import * 3 | 4 | ## A trivial test function to check if the module is loading properly 5 | def pyadd(a,b): 6 | return a+b 7 | -------------------------------------------------------------------------------- /python/histogram_drawer: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | from std_msgs.msg import UInt32MultiArray 6 | from sensor_msgs.msg import Image 7 | from cv_bridge import CvBridge 8 | 9 | import io 10 | import numpy as np 11 | import matplotlib 12 | matplotlib.use('agg') # turn off interactive backend 13 | import matplotlib.pyplot as plt 14 | 15 | class HistogramDrawer: 16 | 17 | def __init__(self): 18 | 19 | self.pub = rospy.Publisher("drawn_histogram", Image, queue_size=10) 20 | self.sub = rospy.Subscriber("histogram", UInt32MultiArray, self.rx_histogram) 21 | 22 | self.bin_max = 0 23 | 24 | def rx_histogram(self, data): 25 | #print(data.data) 26 | 27 | xmax = len(data.data) 28 | 29 | ## Regardless, compress to 128 bins .. this algorithm assumes the new 30 | # number of bins is a factor of the original histogram bins 31 | d = np.reshape(data.data, (128,-1), 'C') 32 | 33 | new_histogram = np.sum(d,axis=1) 34 | 35 | self.bin_max = np.max((self.bin_max, max(new_histogram))) 36 | 37 | #print(new_histogram) 38 | fig, ax = plt.subplots() 39 | 40 | plt.yscale("log") 41 | ax.plot(np.linspace(0,xmax,128), new_histogram,'b.-') 42 | ax.set_xlabel("Intensity") 43 | ax.set_ylabel("Count") 44 | ax.set_ylim([1, self.bin_max*2]) 45 | 46 | with io.BytesIO() as buff: 47 | fig.savefig(buff, format='raw') 48 | buff.seek(0) 49 | data = np.frombuffer(buff.getvalue(), dtype=np.uint8) 50 | w, h = fig.canvas.get_width_height() 51 | im = data.reshape((int(h), int(w), -1)) 52 | 53 | # Drop alpha channel 54 | im = im[:,:,:3] 55 | 56 | bridge = CvBridge() 57 | image_message = bridge.cv2_to_imgmsg(im, encoding="rgb8") 58 | 59 | self.pub.publish(image_message) 60 | 61 | plt.close(fig) 62 | 63 | 64 | 65 | if __name__ == "__main__": 66 | 67 | rospy.init_node('histogram_drawer', anonymous=True) 68 | 69 | drawer = HistogramDrawer() 70 | 71 | rospy.spin(); 72 | -------------------------------------------------------------------------------- /python/ndarray_converter.cpp: -------------------------------------------------------------------------------- 1 | // borrowed in spirit from 2 | // https://github.com/yati-sagade/opencv-ndarray-conversion MIT License 3 | 4 | #include "ndarray_converter.h" 5 | 6 | #define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION 7 | #include 8 | 9 | #if PY_VERSION_HEX >= 0x03000000 10 | #define PyInt_Check PyLong_Check 11 | #define PyInt_AsLong PyLong_AsLong 12 | #endif 13 | 14 | struct Tmp { 15 | const char *name; 16 | 17 | Tmp(const char *name) : name(name) {} 18 | }; 19 | 20 | Tmp info("return value"); 21 | 22 | bool NDArrayConverter::init_numpy() { 23 | // this has to be in this file, since PyArray_API is defined as static 24 | import_array1(false); 25 | return true; 26 | } 27 | 28 | /* 29 | * The following conversion functions are taken/adapted from OpenCV's cv2.cpp 30 | * file inside modules/python/src2 folder (OpenCV 3.1.0) 31 | */ 32 | 33 | static PyObject *opencv_error = 0; 34 | 35 | static int failmsg(const char *fmt, ...) { 36 | char str[1000]; 37 | 38 | va_list ap; 39 | va_start(ap, fmt); 40 | vsnprintf(str, sizeof(str), fmt, ap); 41 | va_end(ap); 42 | 43 | PyErr_SetString(PyExc_TypeError, str); 44 | return 0; 45 | } 46 | 47 | class PyAllowThreads { 48 | public: 49 | PyAllowThreads() : _state(PyEval_SaveThread()) {} 50 | ~PyAllowThreads() { PyEval_RestoreThread(_state); } 51 | 52 | private: 53 | PyThreadState *_state; 54 | }; 55 | 56 | class PyEnsureGIL { 57 | public: 58 | PyEnsureGIL() : _state(PyGILState_Ensure()) {} 59 | ~PyEnsureGIL() { PyGILState_Release(_state); } 60 | 61 | private: 62 | PyGILState_STATE _state; 63 | }; 64 | 65 | #define ERRWRAP2(expr) \ 66 | try { \ 67 | PyAllowThreads allowThreads; \ 68 | expr; \ 69 | } catch (const cv::Exception &e) { \ 70 | PyErr_SetString(opencv_error, e.what()); \ 71 | return 0; \ 72 | } 73 | 74 | using namespace cv; 75 | 76 | class NumpyAllocator : public MatAllocator { 77 | public: 78 | NumpyAllocator() { stdAllocator = Mat::getStdAllocator(); } 79 | ~NumpyAllocator() {} 80 | 81 | UMatData *allocate(PyObject *o, int dims, const int *sizes, int type, 82 | size_t *step) const { 83 | UMatData *u = new UMatData(this); 84 | u->data = u->origdata = (uchar *)PyArray_DATA((PyArrayObject *)o); 85 | npy_intp *_strides = PyArray_STRIDES((PyArrayObject *)o); 86 | for (int i = 0; i < dims - 1; i++) step[i] = (size_t)_strides[i]; 87 | step[dims - 1] = CV_ELEM_SIZE(type); 88 | u->size = sizes[0] * step[0]; 89 | u->userdata = o; 90 | return u; 91 | } 92 | 93 | #if CV_MAJOR_VERSION < 4 94 | UMatData *allocate(int dims0, const int *sizes, int type, void *data, 95 | size_t *step, int flags, UMatUsageFlags usageFlags) const 96 | #else 97 | UMatData *allocate(int dims0, const int *sizes, int type, void *data, 98 | size_t *step, AccessFlag flags, 99 | UMatUsageFlags usageFlags) const 100 | #endif 101 | { 102 | if (data != 0) { 103 | CV_Error(Error::StsAssert, "The data should normally be NULL!"); 104 | // probably this is safe to do in such extreme case 105 | return stdAllocator->allocate(dims0, sizes, type, data, step, flags, 106 | usageFlags); 107 | } 108 | PyEnsureGIL gil; 109 | 110 | int depth = CV_MAT_DEPTH(type); 111 | int cn = CV_MAT_CN(type); 112 | const int f = (int)(sizeof(size_t) / 8); 113 | int typenum = depth == CV_8U ? NPY_UBYTE 114 | : depth == CV_8S ? NPY_BYTE 115 | : depth == CV_16U ? NPY_USHORT 116 | : depth == CV_16S ? NPY_SHORT 117 | : depth == CV_32S ? NPY_INT 118 | : depth == CV_32F ? NPY_FLOAT 119 | : depth == CV_64F ? NPY_DOUBLE 120 | : f * NPY_ULONGLONG + (f ^ 1) * NPY_UINT; 121 | int i, dims = dims0; 122 | cv::AutoBuffer _sizes(dims + 1); 123 | for (i = 0; i < dims; i++) _sizes[i] = sizes[i]; 124 | if (cn > 1) _sizes[dims++] = cn; 125 | PyObject *o = PyArray_SimpleNew(dims, _sizes, typenum); 126 | if (!o) 127 | CV_Error_(Error::StsError, 128 | ("The numpy array of typenum=%d, ndims=%d can not be created", 129 | typenum, dims)); 130 | return allocate(o, dims0, sizes, type, step); 131 | } 132 | 133 | #if CV_MAJOR_VERSION < 4 134 | bool allocate(UMatData *u, int accessFlags, UMatUsageFlags usageFlags) const 135 | #else 136 | bool allocate(UMatData *u, AccessFlag accessFlags, 137 | UMatUsageFlags usageFlags) const 138 | #endif 139 | { 140 | return stdAllocator->allocate(u, accessFlags, usageFlags); 141 | } 142 | 143 | void deallocate(UMatData *u) const { 144 | if (!u) return; 145 | PyEnsureGIL gil; 146 | CV_Assert(u->urefcount >= 0); 147 | CV_Assert(u->refcount >= 0); 148 | if (u->refcount == 0) { 149 | PyObject *o = (PyObject *)u->userdata; 150 | Py_XDECREF(o); 151 | delete u; 152 | } 153 | } 154 | 155 | const MatAllocator *stdAllocator; 156 | }; 157 | 158 | NumpyAllocator g_numpyAllocator; 159 | 160 | bool NDArrayConverter::toMat(PyObject *o, Mat &m) { 161 | bool allowND = true; 162 | if (!o || o == Py_None) { 163 | if (!m.data) m.allocator = &g_numpyAllocator; 164 | return true; 165 | } 166 | 167 | if (PyInt_Check(o)) { 168 | double v[] = {static_cast(PyInt_AsLong((PyObject *)o)), 0., 0., 0.}; 169 | m = Mat(4, 1, CV_64F, v).clone(); 170 | return true; 171 | } 172 | if (PyFloat_Check(o)) { 173 | double v[] = {PyFloat_AsDouble((PyObject *)o), 0., 0., 0.}; 174 | m = Mat(4, 1, CV_64F, v).clone(); 175 | return true; 176 | } 177 | if (PyTuple_Check(o)) { 178 | int i, sz = (int)PyTuple_Size((PyObject *)o); 179 | m = Mat(sz, 1, CV_64F); 180 | for (i = 0; i < sz; i++) { 181 | PyObject *oi = PyTuple_GET_ITEM(o, i); 182 | if (PyInt_Check(oi)) 183 | m.at(i) = (double)PyInt_AsLong(oi); 184 | else if (PyFloat_Check(oi)) 185 | m.at(i) = (double)PyFloat_AsDouble(oi); 186 | else { 187 | failmsg("%s is not a numerical tuple", info.name); 188 | m.release(); 189 | return false; 190 | } 191 | } 192 | return true; 193 | } 194 | 195 | if (!PyArray_Check(o)) { 196 | failmsg("%s is not a numpy array, neither a scalar", info.name); 197 | return false; 198 | } 199 | 200 | PyArrayObject *oarr = (PyArrayObject *)o; 201 | 202 | bool needcopy = false, needcast = false; 203 | int typenum = PyArray_TYPE(oarr), new_typenum = typenum; 204 | int type = typenum == NPY_UBYTE ? CV_8U 205 | : typenum == NPY_BYTE ? CV_8S 206 | : typenum == NPY_USHORT ? CV_16U 207 | : typenum == NPY_SHORT ? CV_16S 208 | : typenum == NPY_INT ? CV_32S 209 | : typenum == NPY_INT32 ? CV_32S 210 | : typenum == NPY_FLOAT ? CV_32F 211 | : typenum == NPY_DOUBLE ? CV_64F 212 | : -1; 213 | 214 | if (type < 0) { 215 | if (typenum == NPY_INT64 || typenum == NPY_UINT64 || typenum == NPY_LONG) { 216 | needcopy = needcast = true; 217 | new_typenum = NPY_INT; 218 | type = CV_32S; 219 | } else { 220 | failmsg("%s data type = %d is not supported", info.name, typenum); 221 | return false; 222 | } 223 | } 224 | 225 | #ifndef CV_MAX_DIM 226 | const int CV_MAX_DIM = 32; 227 | #endif 228 | 229 | int ndims = PyArray_NDIM(oarr); 230 | if (ndims >= CV_MAX_DIM) { 231 | failmsg("%s dimensionality (=%d) is too high", info.name, ndims); 232 | return false; 233 | } 234 | 235 | int size[CV_MAX_DIM + 1]; 236 | size_t step[CV_MAX_DIM + 1]; 237 | size_t elemsize = CV_ELEM_SIZE1(type); 238 | const npy_intp *_sizes = PyArray_DIMS(oarr); 239 | const npy_intp *_strides = PyArray_STRIDES(oarr); 240 | bool ismultichannel = ndims == 3 && _sizes[2] <= CV_CN_MAX; 241 | 242 | for (int i = ndims - 1; i >= 0 && !needcopy; i--) { 243 | // these checks handle cases of 244 | // a) multi-dimensional (ndims > 2) arrays, as well as simpler 1- and 245 | // 2-dimensional cases b) transposed arrays, where _strides[] elements go 246 | // in non-descending order c) flipped arrays, where some of _strides[] 247 | // elements are negative 248 | // the _sizes[i] > 1 is needed to avoid spurious copies when 249 | // NPY_RELAXED_STRIDES is set 250 | if ((i == ndims - 1 && _sizes[i] > 1 && (size_t)_strides[i] != elemsize) || 251 | (i < ndims - 1 && _sizes[i] > 1 && _strides[i] < _strides[i + 1])) 252 | needcopy = true; 253 | } 254 | 255 | if (ismultichannel && _strides[1] != (npy_intp)elemsize * _sizes[2]) 256 | needcopy = true; 257 | 258 | if (needcopy) { 259 | // if (info.outputarg) 260 | //{ 261 | // failmsg("Layout of the output array %s is incompatible with cv::Mat 262 | // (step[ndims-1] != elemsize or step[1] != elemsize*nchannels)", 263 | // info.name); return false; 264 | // } 265 | 266 | if (needcast) { 267 | o = PyArray_Cast(oarr, new_typenum); 268 | oarr = (PyArrayObject *)o; 269 | } else { 270 | oarr = PyArray_GETCONTIGUOUS(oarr); 271 | o = (PyObject *)oarr; 272 | } 273 | 274 | _strides = PyArray_STRIDES(oarr); 275 | } 276 | 277 | // Normalize strides in case NPY_RELAXED_STRIDES is set 278 | size_t default_step = elemsize; 279 | for (int i = ndims - 1; i >= 0; --i) { 280 | size[i] = (int)_sizes[i]; 281 | if (size[i] > 1) { 282 | step[i] = (size_t)_strides[i]; 283 | default_step = step[i] * size[i]; 284 | } else { 285 | step[i] = default_step; 286 | default_step *= size[i]; 287 | } 288 | } 289 | 290 | // handle degenerate case 291 | if (ndims == 0) { 292 | size[ndims] = 1; 293 | step[ndims] = elemsize; 294 | ndims++; 295 | } 296 | 297 | if (ismultichannel) { 298 | ndims--; 299 | type |= CV_MAKETYPE(0, size[2]); 300 | } 301 | 302 | if (ndims > 2 && !allowND) { 303 | failmsg("%s has more than 2 dimensions", info.name); 304 | return false; 305 | } 306 | 307 | m = Mat(ndims, size, type, PyArray_DATA(oarr), step); 308 | m.u = g_numpyAllocator.allocate(o, ndims, size, type, step); 309 | m.addref(); 310 | 311 | if (!needcopy) { 312 | Py_INCREF(o); 313 | } 314 | m.allocator = &g_numpyAllocator; 315 | 316 | return true; 317 | } 318 | 319 | PyObject *NDArrayConverter::toNDArray(const cv::Mat &m) { 320 | if (!m.data) Py_RETURN_NONE; 321 | Mat temp, *p = (Mat *)&m; 322 | if (!p->u || p->allocator != &g_numpyAllocator) { 323 | temp.allocator = &g_numpyAllocator; 324 | ERRWRAP2(m.copyTo(temp)); 325 | p = &temp; 326 | } 327 | PyObject *o = (PyObject *)p->u->userdata; 328 | Py_INCREF(o); 329 | return o; 330 | } 331 | -------------------------------------------------------------------------------- /python/ndarray_converter.h: -------------------------------------------------------------------------------- 1 | #ifndef __NDARRAY_CONVERTER_H__ 2 | #define __NDARRAY_CONVERTER_H__ 3 | 4 | #include 5 | 6 | #include 7 | 8 | class NDArrayConverter { 9 | public: 10 | // must call this first, or the other routines don't work! 11 | static bool init_numpy(); 12 | 13 | static bool toMat(PyObject *o, cv::Mat &m); 14 | static PyObject *toNDArray(const cv::Mat &mat); 15 | }; 16 | 17 | // 18 | // Define the type converter 19 | // 20 | 21 | #include 22 | 23 | namespace pybind11 { 24 | namespace detail { 25 | 26 | template <> 27 | struct type_caster { 28 | public: 29 | PYBIND11_TYPE_CASTER(cv::Mat, _("numpy.ndarray")); 30 | 31 | bool load(handle src, bool) { 32 | return NDArrayConverter::toMat(src.ptr(), value); 33 | } 34 | 35 | static handle cast(const cv::Mat &m, return_value_policy, handle defval) { 36 | return handle(NDArrayConverter::toNDArray(m)); 37 | } 38 | }; 39 | 40 | } // namespace detail 41 | } // namespace pybind11 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /python/old_setup.py: -------------------------------------------------------------------------------- 1 | # 2 | # Much of this copied from https://github.com/edmBernard/pybind11_opencv_numpy 3 | # 4 | 5 | import fnmatch 6 | import os 7 | from os.path import dirname, exists, join 8 | from setuptools import find_packages, setup, Extension 9 | from setuptools.command.build_ext import build_ext 10 | import subprocess 11 | import sys 12 | import setuptools 13 | 14 | # 15 | # pybind-specific compilation stuff 16 | # 17 | 18 | class get_pybind_include(object): 19 | """Helper class to determine the pybind11 include path 20 | 21 | The purpose of this class is to postpone importing pybind11 22 | until it is actually installed, so that the ``get_include()`` 23 | method can be invoked. """ 24 | 25 | def __init__(self, user=False): 26 | self.user = user 27 | 28 | def __str__(self): 29 | import pybind11 30 | return pybind11.get_include(self.user) 31 | 32 | # As of Python 3.6, CCompiler has a `has_flag` method. 33 | # cf http://bugs.python.org/issue26689 34 | def has_flag(compiler, flagname): 35 | """Return a boolean indicating whether a flag name is supported on 36 | the specified compiler. 37 | """ 38 | import tempfile 39 | with tempfile.NamedTemporaryFile('w', suffix='.cpp') as f: 40 | f.write('int main (int argc, char **argv) { return 0; }') 41 | try: 42 | compiler.compile([f.name], extra_postargs=[flagname]) 43 | except setuptools.distutils.errors.CompileError: 44 | return False 45 | return True 46 | 47 | 48 | def cpp_flag(compiler): 49 | """Return the -std=c++[11/14] compiler flag. 50 | 51 | The c++14 is preferred over c++11 (when it is available). 52 | """ 53 | if has_flag(compiler, '-std=c++14'): 54 | return '-std=c++14' 55 | elif has_flag(compiler, '-std=c++11'): 56 | return '-std=c++11' 57 | else: 58 | raise RuntimeError('Unsupported compiler -- at least C++11 support ' 59 | 'is needed!') 60 | 61 | 62 | class BuildExt(build_ext): 63 | """A custom build extension for adding compiler-specific options.""" 64 | c_opts = { 65 | 'msvc': ['/EHsc'], 66 | 'unix': [], 67 | } 68 | 69 | if sys.platform == 'darwin': 70 | c_opts['unix'] += ['-stdlib=libc++', '-mmacosx-version-min=10.7'] 71 | 72 | def build_extensions(self): 73 | ct = self.compiler.compiler_type 74 | opts = self.c_opts.get(ct, []) 75 | 76 | for arg in ext_modules[0].extra_compile_args: 77 | opts.append(arg) 78 | 79 | if ct == 'unix': 80 | opts.append('-s') # strip 81 | opts.append('-g0') # remove debug symbols 82 | opts.append(cpp_flag(self.compiler)) 83 | if has_flag(self.compiler, '-fvisibility=hidden'): 84 | opts.append('-fvisibility=hidden') 85 | for ext in self.extensions: 86 | ext.extra_compile_args = opts 87 | build_ext.build_extensions(self) 88 | 89 | ext_modules = [ 90 | Extension( 91 | 'serdp_common', 92 | [ 93 | 'serdp_common_py.cpp', 94 | 'ndarray_converter.cpp', 95 | '../lib/DrawSonar.cpp', 96 | '../lib/DataStructures.cpp' 97 | ], 98 | include_dirs=[ 99 | # Path to pybind11 headers 100 | get_pybind_include(), 101 | get_pybind_include(user=True), 102 | "../include/" 103 | ], 104 | extra_compile_args=['-DABSTRACT_SONAR_INTERFACE_ONLY'], 105 | libraries=['opencv_core', 'opencv_highgui'], 106 | language='c++', 107 | ), 108 | ] 109 | 110 | setup( 111 | name='serdp_common', 112 | version='0.1', 113 | author='Aaron Marburg', 114 | author_email='amarburg@uw.edu', 115 | packages=find_packages(), 116 | ext_modules=ext_modules, 117 | install_requires=None, 118 | cmdclass={'build_ext': BuildExt}, 119 | zip_safe=False, 120 | ) 121 | -------------------------------------------------------------------------------- /python/py_draw_sonar.cpp: -------------------------------------------------------------------------------- 1 | // #include 2 | // 3 | // #include 4 | // 5 | // // #include "ndarray_converter.h" 6 | // // #include "serdp_common/DrawSonar.h" 7 | // 8 | // namespace py = pybind11; 9 | // 10 | // 11 | // cv::Mat read_image(std::string image_name) { 12 | // #if CV_MAJOR_VERSION < 4 13 | // cv::Mat image = cv::imread(image_name, CV_LOAD_IMAGE_COLOR); 14 | // #else 15 | // cv::Mat image = cv::imread(image_name, cv::IMREAD_COLOR); 16 | // #endif 17 | // return image; 18 | // } 19 | // 20 | // // class AddClass { 21 | // // public: 22 | // // AddClass(int value) : value(value) {} 23 | // // 24 | // // cv::Mat add(cv::Mat input) { 25 | // // return input + this->value; 26 | // // } 27 | // // 28 | // // private: 29 | // // int value; 30 | // // }; 31 | // 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "draw_sonar/DataStructures.h" 43 | #include "draw_sonar/DrawSonar.h" 44 | #include "ndarray_converter.h" 45 | 46 | namespace py = pybind11; 47 | 48 | // Sample functions from examples used to bootstrap the project 49 | 50 | cv::Mat read_image(std::string image_name) { 51 | #if CV_MAJOR_VERSION < 4 52 | cv::Mat image = cv::imread(image_name, CV_LOAD_IMAGE_COLOR); 53 | #else 54 | cv::Mat image = cv::imread(image_name, cv::IMREAD_COLOR); 55 | #endif 56 | return image; 57 | } 58 | 59 | cv::Mat passthru(cv::Mat image) { return image; } 60 | 61 | cv::Mat cloneimg(cv::Mat image) { return image.clone(); } 62 | 63 | int add(int i, int j) { return i + j; } 64 | 65 | class AddClass { 66 | public: 67 | AddClass(int value) : value(value) {} 68 | 69 | cv::Mat add(cv::Mat input) { return input + this->value; } 70 | 71 | private: 72 | int value; 73 | }; 74 | 75 | int calculateImageWidth(int height) { return 100; } 76 | 77 | /// 78 | PYBIND11_MODULE(py_draw_sonar, m) { 79 | NDArrayConverter::init_numpy(); 80 | 81 | m.def("read_image", &read_image, "A function that read an image", 82 | py::arg("image")); 83 | 84 | m.def("passthru", &passthru, "Passthru function", py::arg("image")); 85 | m.def("clone", &cloneimg, "Clone function", py::arg("image")); 86 | 87 | m.def("cppadd", &add, "A function which adds two numbers"); 88 | 89 | m.def("calculateImageWidth", &calculateImageWidth, 90 | "Given a sonar height, calculate width in pixels"); 91 | 92 | py::class_(m, "AddClass") 93 | .def(py::init()) 94 | .def("add", &AddClass::add); 95 | } 96 | -------------------------------------------------------------------------------- /python/sonar_image_proc/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apl-ocean-engineering/sonar_image_proc/5c39a302996826ac3555d98c41cc26fab2beafe9/python/sonar_image_proc/__init__.py -------------------------------------------------------------------------------- /python/sonar_image_proc/sonar_msg_metadata.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | """ 3 | Copyright 2023 University of Washington Applied Physics Laboratory 4 | Author: Marc Micatka & Laura Lindzey 5 | """ 6 | 7 | from __future__ import annotations # use type of class in member function annotation. 8 | 9 | import numpy as np 10 | import typing 11 | from marine_acoustic_msgs.msg import ProjectedSonarImage 12 | 13 | 14 | class SonarImageMetadata(object): 15 | def __init__(self, sonar_image_msg: ProjectedSonarImage): 16 | """ 17 | Metadata for a sonar image, containing all information necessary 18 | to compute its geometry. 19 | NOTE(lindzey): excludes beamwidths because those are not used when 20 | deciding what elevation angles to publish. 21 | """ 22 | self.num_angles = len(sonar_image_msg.beam_directions) 23 | self.num_ranges = len(sonar_image_msg.ranges) 24 | self.ranges = np.array(sonar_image_msg.ranges) 25 | self.min_range = np.min(sonar_image_msg.ranges) 26 | self.max_range = np.max(sonar_image_msg.ranges) 27 | 28 | xx = np.array([dir.x for dir in sonar_image_msg.beam_directions]) 29 | yy = np.array([dir.y for dir in sonar_image_msg.beam_directions]) 30 | zz = np.array([dir.z for dir in sonar_image_msg.beam_directions]) 31 | self.azimuths = np.arctan2(-1 * yy, np.sqrt(xx**2 + zz**2)) 32 | self.min_azimuth = np.min(self.azimuths) 33 | self.max_azimuth = np.max(self.azimuths) 34 | elev_beamwidth = np.median(sonar_image_msg.ping_info.tx_beamwidths) 35 | self.min_elevation = -0.5 * elev_beamwidth 36 | self.max_elevation = 0.5 * elev_beamwidth 37 | 38 | def __eq__(self, other: SonarImageMetadata) -> bool: 39 | """ 40 | Overrides the default implementation of == and != (along with is and is not) 41 | Determine whether all fields are "close enough" for the 42 | metadata to be the same. 43 | """ 44 | if self.num_angles != other.num_angles: 45 | return False 46 | if self.num_ranges != other.num_ranges: 47 | return False 48 | return np.allclose( 49 | [self.min_range, self.max_range, self.min_azimuth, self.max_azimuth], 50 | [other.min_range, other.max_range, other.min_azimuth, other.max_azimuth], 51 | ) 52 | 53 | def __str__(self) -> typing.String: 54 | """ 55 | Overrides the default implementation of print(SonarImageMetadata) 56 | """ 57 | ss = "SonarImageMetadata: {} beams, {} ranges, {:0.2f}=>{:0.2f} m, {:0.1f}=>{:0.1f} deg".format( 58 | self.num_angles, 59 | self.num_ranges, 60 | self.min_range, 61 | self.max_range, 62 | np.degrees(self.min_azimuth), 63 | np.degrees(self.max_azimuth), 64 | ) 65 | return ss 66 | -------------------------------------------------------------------------------- /python/test.py: -------------------------------------------------------------------------------- 1 | import draw_sonar 2 | 3 | print( draw_sonar.pyadd(1,2) ) 4 | print( draw_sonar.cppadd(1,2) ) 5 | -------------------------------------------------------------------------------- /ros/cfg/DrawSonar.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "sonar_image_proc" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("line_alpha", double_t, 0, "Alpha for lines", 0.5, 0, 1.0) 9 | gen.add("line_thickness", int_t, 0, "Line thickness", 1, 1, 5) 10 | 11 | gen.add("range_spacing", double_t, 0, "Spacing of range marks (0 for auto)", 0, 0, 100) 12 | gen.add("bearing_spacing", double_t, 0, "Spacing of bearing radials", 20, 0, 100) 13 | 14 | gen.add("log_scale", bool_t, 0, "Use log scale for intensity", False) 15 | gen.add("min_db", double_t, 0, "Minimum intensity in log scale (in db, 0 for full range)", 0, -221, 0) 16 | gen.add("max_db", double_t, 0, "Maximum intensity in log scale (in db, 0 for full range)", 0, -221, 0) 17 | 18 | exit(gen.generate(PACKAGE, "sonar_image_proc", "DrawSonar")) 19 | -------------------------------------------------------------------------------- /ros/include/sonar_image_proc/sonar_image_msg_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | // An implementation of an AbstractSonarInterface which 4 | // wraps a ROS marine_acoustic_msgs::SonarImage 5 | // 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include "sonar_image_proc/AbstractSonarInterface.h" 12 | 13 | namespace sonar_image_proc { 14 | 15 | using marine_acoustic_msgs::ProjectedSonarImage; 16 | using sonar_image_proc::AbstractSonarInterface; 17 | using std::vector; 18 | 19 | struct SonarImageMsgInterface 20 | : public sonar_image_proc::AbstractSonarInterface { 21 | explicit SonarImageMsgInterface( 22 | const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &ping) 23 | : _ping(ping), do_log_scale_(false) { 24 | // Vertical field of view is determined by comparing 25 | // z / sqrt(x^2 + y^2) to tan(elevation_beamwidth/2) 26 | _verticalTanSquared = 27 | // NOTE(lindzey): The old message assumed a constant elevation 28 | // beamwidth; 29 | // the multibeam people insisted that it be per-beam. For now, this 30 | // assumes that they're all the same. 31 | // TODO(lindzey): Look into whether averaging would be better, or if we 32 | // should create an array of verticalTanSquared. 33 | // TODO(lindzey): Handle empty-array case. 34 | std::pow(std::tan(ping->ping_info.tx_beamwidths[0] / 2.0), 2); 35 | 36 | for (const auto pt : ping->beam_directions) { 37 | auto az = atan2(-1 * pt.y, pt.z); 38 | _ping_azimuths.push_back(az); 39 | } 40 | } 41 | 42 | // n.b. do_log_scale is a mode switch which causes the intensity_* 43 | // functions in this SonarImageMsgInterface use a log (db) scale 44 | // for 32-bit data **ONLY** -- functionality is unchanged for 8- 45 | // and 16-bit data 46 | // 47 | // It maps a given 32-bit intensity value i to a fraction of the 48 | // full scale of a 32-bit uint in the interval [0.0, 1.0] 49 | // It then takes the db value of that: 50 | // 51 | // l = 10 * log( i / (2^32-1) ) 52 | // 53 | // Such that l is between 54 | // ~ -96 for i = 1 55 | // and 56 | // 0.0 for i = UINT32_MAX 57 | // 58 | // The intensity_* functions create a linear map m() from [min_db, max_db] 59 | // to the full range of the returned type (e.g. uint8, uint16, etc), 60 | // and returns m(l) 61 | // 62 | // If min_db is 0.0, then the full range value 63 | // min_db = (10*log(1/(2^32-1))) is used 64 | // If max_db is 0.0, then the full range value 65 | // max_db = 0.0 is used 66 | // 67 | // So for example if i = 2^31 (half the full range of a uint32), l = -3 db 68 | // This is approx 0.97 of the full scale in logspace 69 | // 70 | // With min_db = 0, max_db = 0 (full range) 71 | // intensity_uint8(i) will return approx 247 (0.97 * 255) 72 | // 73 | // With min_db = -10, max_db = 0 74 | // intensity_uint8(i) will return approx 178 (0.7 * 255) 75 | // 76 | // With min_db = -1, max_db = 0 77 | // l(i) is less than min_db and intensity_uint8(i) will return 0 78 | // 79 | // With min_db = 0, max_db = -10 80 | // l(i) is greater than max_db and intensity_uint8(i) will return 255 81 | // 82 | void do_log_scale(float min_db, float max_db) { 83 | do_log_scale_ = true; 84 | min_db_ = min_db; 85 | max_db_ = max_db; 86 | range_db_ = max_db - min_db; 87 | } 88 | 89 | AbstractSonarInterface::DataType_t data_type() const override { 90 | if (_ping->image.dtype == _ping->image.DTYPE_UINT8) 91 | return AbstractSonarInterface::TYPE_UINT8; 92 | else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) 93 | return AbstractSonarInterface::TYPE_UINT16; 94 | else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) 95 | return AbstractSonarInterface::TYPE_UINT32; 96 | 97 | // 98 | return AbstractSonarInterface::TYPE_NONE; 99 | } 100 | 101 | const std::vector &ranges() const override { return _ping->ranges; } 102 | 103 | const std::vector &azimuths() const override { return _ping_azimuths; } 104 | 105 | float verticalTanSquared() const { return _verticalTanSquared; } 106 | 107 | // When the underlying data is 8-bit, returns that exact value 108 | // from the underlying data 109 | // 110 | // If the underlying data is 16-bit, it returns a scaled value. 111 | uint8_t intensity_uint8(const AzimuthRangeIndices &idx) const override { 112 | if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { 113 | return intensity_float_log(idx) * UINT8_MAX; 114 | } 115 | 116 | if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { 117 | return read_uint8(idx); 118 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { 119 | return read_uint16(idx) >> 8; 120 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { 121 | return read_uint32(idx) >> 24; 122 | } 123 | 124 | return 0; 125 | } 126 | 127 | uint16_t intensity_uint16(const AzimuthRangeIndices &idx) const override { 128 | // Truncate 32bit intensities 129 | if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { 130 | return intensity_float_log(idx) * UINT16_MAX; 131 | } 132 | 133 | if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { 134 | return read_uint8(idx) << 8; 135 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { 136 | return read_uint16(idx); 137 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { 138 | return read_uint32(idx) >> 16; 139 | } 140 | return 0; 141 | } 142 | 143 | uint32_t intensity_uint32(const AzimuthRangeIndices &idx) const override { 144 | if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { 145 | return intensity_float_log(idx) * UINT32_MAX; 146 | } 147 | 148 | if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { 149 | return read_uint8(idx) << 24; 150 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { 151 | return read_uint16(idx) << 16; 152 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { 153 | return read_uint32(idx); 154 | } 155 | return 0; 156 | } 157 | 158 | float intensity_float(const AzimuthRangeIndices &idx) const override { 159 | if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { 160 | return intensity_float_log(idx); 161 | } 162 | 163 | if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { 164 | return static_cast(read_uint8(idx)) / UINT8_MAX; 165 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { 166 | return static_cast(read_uint16(idx)) / UINT16_MAX; 167 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { 168 | return static_cast(read_uint32(idx)) / UINT32_MAX; 169 | } 170 | return 0.0; 171 | } 172 | 173 | protected: 174 | marine_acoustic_msgs::ProjectedSonarImage::ConstPtr _ping; 175 | 176 | float _verticalTanSquared; 177 | std::vector _ping_azimuths; 178 | 179 | size_t index(const AzimuthRangeIndices &idx) const { 180 | int data_size; 181 | if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { 182 | data_size = 1; 183 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { 184 | data_size = 2; 185 | } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { 186 | data_size = 4; 187 | } else { 188 | assert(false); 189 | } 190 | 191 | return data_size * ((idx.range() * nBearings()) + idx.azimuth()); 192 | } 193 | 194 | // "raw" read functions. Assumes the data type has already been checked 195 | uint32_t read_uint8(const AzimuthRangeIndices &idx) const { 196 | assert(_ping->image.dtype == _ping->image.DTYPE_UINT8); 197 | const auto i = index(idx); 198 | 199 | return (_ping->image.data[i]); 200 | } 201 | 202 | uint32_t read_uint16(const AzimuthRangeIndices &idx) const { 203 | assert(_ping->image.dtype == _ping->image.DTYPE_UINT16); 204 | const auto i = index(idx); 205 | 206 | return (static_cast(_ping->image.data[i]) | 207 | (static_cast(_ping->image.data[i + 1]) << 8)); 208 | } 209 | 210 | uint32_t read_uint32(const AzimuthRangeIndices &idx) const { 211 | assert(_ping->image.dtype == _ping->image.DTYPE_UINT32); 212 | const auto i = index(idx); 213 | 214 | const uint32_t v = 215 | (static_cast(_ping->image.data[i]) | 216 | (static_cast(_ping->image.data[i + 1]) << 8) | 217 | (static_cast(_ping->image.data[i + 2]) << 16) | 218 | (static_cast(_ping->image.data[i + 3]) << 24)); 219 | return v; 220 | } 221 | 222 | float intensity_float_log(const AzimuthRangeIndices &idx) const { 223 | const auto intensity = read_uint32(idx); 224 | const auto v = 225 | log(static_cast(std::max((uint)1, intensity)) / UINT32_MAX) * 226 | 10; // dbm 227 | 228 | const auto min_db = (min_db_ == 0 ? log(1.0 / UINT32_MAX) * 10 : min_db_); 229 | 230 | return std::min(1.0, std::max(0.0, (v - min_db) / range_db_)); 231 | } 232 | 233 | bool do_log_scale_; 234 | float min_db_, max_db_, range_db_; 235 | }; 236 | 237 | } // namespace sonar_image_proc 238 | -------------------------------------------------------------------------------- /ros/src/draw_sonar_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #include "nodelet/loader.h" 5 | #include "ros/ros.h" 6 | 7 | int main(int argc, char **argv) { 8 | ros::init(argc, argv, "draw_sonar"); 9 | 10 | nodelet::Loader nodelet; 11 | nodelet::M_string remap(ros::names::getRemappings()); 12 | nodelet::V_string nargv; 13 | 14 | nodelet.load(ros::this_node::getName(), "sonar_image_proc/draw_sonar", remap, 15 | nargv); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /ros/src/draw_sonar_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021-2022 University of Washington Applied Physics Laboratory 2 | // Author: Aaron Marburg 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "marine_acoustic_msgs/ProjectedSonarImage.h" 10 | #include "nodelet/nodelet.h" 11 | #include "ros/ros.h" 12 | 13 | // For uploading drawn sonar images to Image topic 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "sonar_image_proc/ColorMaps.h" 22 | #include "sonar_image_proc/DrawSonar.h" 23 | #include "sonar_image_proc/HistogramGenerator.h" 24 | #include "sonar_image_proc/SonarDrawer.h" 25 | #include "sonar_image_proc/sonar_image_msg_interface.h" 26 | 27 | // Subscribes to sonar message topic, draws using opencv then publishes 28 | // result 29 | 30 | namespace draw_sonar { 31 | 32 | using namespace std; 33 | using namespace cv; 34 | 35 | using sonar_image_proc::SonarImageMsgInterface; 36 | using std_msgs::UInt32MultiArray; 37 | 38 | using sonar_image_proc::HistogramGenerator; 39 | 40 | using sonar_image_proc::InfernoColorMap; 41 | using sonar_image_proc::InfernoSaturationColorMap; 42 | using sonar_image_proc::SonarColorMap; 43 | 44 | class DrawSonarNodelet : public nodelet::Nodelet { 45 | public: 46 | DrawSonarNodelet() 47 | : Nodelet(), 48 | _maxRange(0.0), 49 | _colorMap(new InfernoColorMap) // Set a reasonable default 50 | { 51 | ; 52 | } 53 | 54 | virtual ~DrawSonarNodelet() { ; } 55 | 56 | private: 57 | virtual void onInit() { 58 | ros::NodeHandle nh = getMTNodeHandle(); 59 | ros::NodeHandle pnh = getMTPrivateNodeHandle(); 60 | 61 | pnh.param("max_range", _maxRange, 0.0); 62 | 63 | pnh.param("publish_old", _publishOldApi, false); 64 | pnh.param("publish_timing", _publishTiming, true); 65 | 66 | pnh.param("publish_histogram", _publishHistogram, false); 67 | 68 | std::string colorMapName; 69 | pnh.param("color_map", colorMapName, "inferno"); 70 | 71 | // Set color map 72 | setColorMap(colorMapName); 73 | 74 | if (_maxRange > 0.0) { 75 | NODELET_INFO_STREAM("Only drawing to max range " << _maxRange); 76 | } 77 | 78 | subSonarImage_ = nh.subscribe( 79 | "sonar_image", 10, &DrawSonarNodelet::sonarImageCallback, this); 80 | 81 | pub_ = nh.advertise("drawn_sonar", 10); 82 | osdPub_ = nh.advertise("drawn_sonar_osd", 10); 83 | rectPub_ = nh.advertise("drawn_sonar_rect", 10); 84 | 85 | if (_publishOldApi) 86 | oldPub_ = nh.advertise("old_drawn_sonar", 10); 87 | 88 | if (_publishTiming) 89 | timingPub_ = 90 | nh.advertise("sonar_image_proc_timing", 10); 91 | 92 | if (_publishHistogram) 93 | histogramPub_ = nh.advertise("histogram", 10); 94 | 95 | dyn_cfg_server_.reset(new DynamicReconfigureServer(pnh)); 96 | dyn_cfg_server_->setCallback(std::bind(&DrawSonarNodelet::dynCfgCallback, 97 | this, std::placeholders::_1, 98 | std::placeholders::_2)); 99 | 100 | ROS_DEBUG("draw_sonar ready to run..."); 101 | } 102 | 103 | void cvBridgeAndPublish( 104 | const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg, 105 | const cv::Mat &mat, ros::Publisher &pub) { 106 | cv_bridge::CvImage img_bridge(msg->header, 107 | sensor_msgs::image_encodings::RGB8, mat); 108 | 109 | sensor_msgs::Image output_msg; 110 | img_bridge.toImageMsg(output_msg); 111 | pub.publish(output_msg); 112 | } 113 | 114 | void sonarImageCallback( 115 | const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg) { 116 | ROS_FATAL_COND(!_colorMap, "Colormap is undefined, this shouldn't happen"); 117 | 118 | SonarImageMsgInterface interface(msg); 119 | if (log_scale_) { 120 | interface.do_log_scale(min_db_, max_db_); 121 | } 122 | 123 | ros::WallDuration oldApiElapsed, rectElapsed, mapElapsed, histogramElapsed; 124 | 125 | if (_publishOldApi) { 126 | ros::WallTime begin = ros::WallTime::now(); 127 | 128 | // Used to be a configurable parameter, but now only meaningful 129 | // in the deprecated API 130 | const int pixPerRangeBin = 2; 131 | 132 | cv::Size sz = sonar_image_proc::old_api::calculateImageSize( 133 | interface, cv::Size(0, 0), pixPerRangeBin, _maxRange); 134 | cv::Mat mat(sz, CV_8UC3); 135 | mat = sonar_image_proc::old_api::drawSonar(interface, mat, *_colorMap, 136 | _maxRange); 137 | 138 | cvBridgeAndPublish(msg, mat, oldPub_); 139 | 140 | oldApiElapsed = ros::WallTime::now() - begin; 141 | } 142 | 143 | if (_publishHistogram) { 144 | ros::WallTime begin = ros::WallTime::now(); 145 | 146 | auto histogramOut = UInt32MultiArray(); 147 | histogramOut.data = HistogramGenerator::Generate(interface); 148 | 149 | histogramPub_.publish(histogramOut); 150 | 151 | histogramElapsed = ros::WallTime::now() - begin; 152 | } 153 | 154 | { 155 | ros::WallTime begin = ros::WallTime::now(); 156 | 157 | cv::Mat rectMat = _sonarDrawer.drawRectSonarImage(interface, *_colorMap); 158 | 159 | // Rotate rectangular image to the more expected format where zero range 160 | // is at the bottom of the image, with negative azimuth to the right 161 | // aka (rotated 90 degrees CCW) 162 | cv::Mat rotatedRect; 163 | cv::rotate(rectMat, rotatedRect, cv::ROTATE_90_COUNTERCLOCKWISE); 164 | cvBridgeAndPublish(msg, rotatedRect, rectPub_); 165 | 166 | rectElapsed = ros::WallTime::now() - begin; 167 | begin = ros::WallTime::now(); 168 | 169 | cv::Mat sonarMat = _sonarDrawer.remapRectSonarImage(interface, rectMat); 170 | cvBridgeAndPublish(msg, sonarMat, pub_); 171 | 172 | if (osdPub_.getNumSubscribers() > 0) { 173 | cv::Mat osdMat = _sonarDrawer.drawOverlay(interface, sonarMat); 174 | cvBridgeAndPublish(msg, osdMat, osdPub_); 175 | } 176 | 177 | mapElapsed = ros::WallTime::now() - begin; 178 | } 179 | 180 | if (_publishTiming) { 181 | ostringstream output; 182 | 183 | output << "{"; 184 | output << "\"draw_total\" : " << (mapElapsed + rectElapsed).toSec(); 185 | output << ", \"rect\" : " << rectElapsed.toSec(); 186 | output << ", \"map\" : " << mapElapsed.toSec(); 187 | 188 | if (_publishOldApi) output << ", \"old_api\" : " << oldApiElapsed.toSec(); 189 | if (_publishHistogram) 190 | output << ", \"histogram\" : " << histogramElapsed.toSec(); 191 | 192 | output << "}"; 193 | 194 | std_msgs::String out_msg; 195 | out_msg.data = output.str(); 196 | 197 | timingPub_.publish(out_msg); 198 | } 199 | } 200 | 201 | void dynCfgCallback(sonar_image_proc::DrawSonarConfig &config, 202 | uint32_t level) { 203 | _sonarDrawer.overlayConfig() 204 | .setRangeSpacing(config.range_spacing) 205 | .setRadialSpacing(config.bearing_spacing) 206 | .setLineAlpha(config.line_alpha) 207 | .setLineThickness(config.line_thickness); 208 | 209 | log_scale_ = config.log_scale; 210 | min_db_ = config.min_db; 211 | max_db_ = config.max_db; 212 | } 213 | 214 | void setColorMap(const std::string &colorMapName) { 215 | // TBD actually implement the parameter processing here... 216 | _colorMap.reset(new InfernoSaturationColorMap()); 217 | } 218 | 219 | ros::Subscriber subSonarImage_; 220 | ros::Publisher pub_, rectPub_, osdPub_, oldPub_, timingPub_, histogramPub_; 221 | 222 | typedef dynamic_reconfigure::Server 223 | DynamicReconfigureServer; 224 | std::shared_ptr dyn_cfg_server_; 225 | 226 | sonar_image_proc::SonarDrawer _sonarDrawer; 227 | 228 | float _maxRange; 229 | bool _publishOldApi, _publishTiming, _publishHistogram; 230 | 231 | float min_db_, max_db_; 232 | bool log_scale_; 233 | 234 | std::unique_ptr _colorMap; 235 | }; 236 | 237 | } // namespace draw_sonar 238 | 239 | #include 240 | PLUGINLIB_EXPORT_CLASS(draw_sonar::DrawSonarNodelet, nodelet::Nodelet); 241 | -------------------------------------------------------------------------------- /ros/src/sonar_postprocessor_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 University of Washington Applied Physics Laboratory 2 | // 3 | 4 | #include "nodelet/loader.h" 5 | #include "ros/ros.h" 6 | 7 | int main(int argc, char **argv) { 8 | ros::init(argc, argv, "sonar_postprocessor"); 9 | 10 | nodelet::Loader nodelet; 11 | nodelet::M_string remap(ros::names::getRemappings()); 12 | nodelet::V_string nargv; 13 | 14 | nodelet.load(ros::this_node::getName(), 15 | "sonar_image_proc/sonar_postprocessor", remap, nargv); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /ros/src/sonar_postprocessor_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021-2022 University of Washington Applied Physics Laboratory 2 | // Author: Aaron Marburg 3 | 4 | #include 5 | 6 | #include "marine_acoustic_msgs/ProjectedSonarImage.h" 7 | #include "nodelet/nodelet.h" 8 | #include "ros/ros.h" 9 | #include "sonar_image_proc/sonar_image_msg_interface.h" 10 | 11 | namespace sonar_postprocessor { 12 | 13 | using marine_acoustic_msgs::ProjectedSonarImage; 14 | using sonar_image_proc::SonarImageMsgInterface; 15 | 16 | class SonarPostprocessorNodelet : public nodelet::Nodelet { 17 | public: 18 | SonarPostprocessorNodelet() : Nodelet() { ; } 19 | 20 | virtual ~SonarPostprocessorNodelet() { ; } 21 | 22 | private: 23 | virtual void onInit() { 24 | ros::NodeHandle nh = getMTNodeHandle(); 25 | ros::NodeHandle pnh = getMTPrivateNodeHandle(); 26 | 27 | pnh.param("gain", gain_, 1.0); 28 | pnh.param("gamma", gamma_, 0.0); 29 | 30 | subSonarImage_ = nh.subscribe( 31 | "sonar_image", 10, &SonarPostprocessorNodelet::sonarImageCallback, 32 | this); 33 | 34 | pubSonarImage_ = 35 | nh.advertise("sonar_image_postproc", 10); 36 | 37 | ROS_DEBUG("sonar_processor ready to run..."); 38 | } 39 | 40 | void sonarImageCallback( 41 | const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg) { 42 | SonarImageMsgInterface interface(msg); 43 | 44 | // For now, only postprocess 32bit images 45 | if (msg->image.dtype != msg->image.DTYPE_UINT32) { 46 | pubSonarImage_.publish(msg); 47 | return; 48 | } 49 | 50 | // Expect this will copy 51 | marine_acoustic_msgs::ProjectedSonarImage out = *msg; 52 | 53 | // For now, only 8-bit output is supported 54 | out.image.dtype = out.image.DTYPE_UINT8; 55 | out.image.data.clear(); 56 | out.image.data.reserve(interface.ranges().size() * 57 | interface.azimuths().size()); 58 | 59 | double logmin, logmax; 60 | 61 | for (unsigned int r_idx = 0; r_idx < interface.nRanges(); ++r_idx) { 62 | for (unsigned int a_idx = 0; a_idx < interface.nAzimuth(); ++a_idx) { 63 | sonar_image_proc::AzimuthRangeIndices idx(a_idx, r_idx); 64 | 65 | // const uint32_t pix = interface.intensity_uint32(idx); 66 | // const auto range = interface.range(r_idx); 67 | 68 | // Avoid log(0) 69 | auto intensity = interface.intensity_uint32(idx); 70 | auto v = log(std::max((uint)1, intensity)) / log(UINT32_MAX); 71 | 72 | if ((r_idx == 0) && (a_idx == 0)) { 73 | logmin = v; 74 | logmax = v; 75 | } else { 76 | logmin = std::min(v, logmin); 77 | logmax = std::max(v, logmax); 78 | } 79 | 80 | const float vmax = 1.0, threshold = 0.74; 81 | 82 | v = (v - threshold) / (vmax - threshold); 83 | v = std::min(1.0, std::max(0.0, v)); 84 | out.image.data.push_back(UINT8_MAX * v); 85 | 86 | // Just do the math in float for now 87 | // float i = static_cast(pix)/UINT32_MAX; 88 | 89 | // //ROS_INFO_STREAM(a_idx << "," << r_idx << " : " << pix << " => " << 90 | // i); 91 | 92 | // i *= gain_; 93 | 94 | // UINT8_MAX * i); 95 | } 96 | } 97 | 98 | // 99 | float dr = exp(logmax - logmin); 100 | pubSonarImage_.publish(out); 101 | } 102 | 103 | protected: 104 | ros::Subscriber subSonarImage_; 105 | ros::Publisher pubSonarImage_; 106 | 107 | float gain_, gamma_; 108 | }; 109 | 110 | } // namespace sonar_postprocessor 111 | 112 | #include 113 | PLUGINLIB_EXPORT_CLASS(sonar_postprocessor::SonarPostprocessorNodelet, 114 | nodelet::Nodelet); 115 | -------------------------------------------------------------------------------- /ros/tools/bag2sonar.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "sonar_image_proc/SonarDrawer.h" 13 | 14 | namespace po = boost::program_options; 15 | 16 | using std::string; 17 | using std::vector; 18 | 19 | class OutputWrapper { 20 | public: 21 | virtual void write( 22 | const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg, 23 | const cv::Mat &mat) = 0; 24 | }; 25 | 26 | class BagOutput : public OutputWrapper { 27 | public: 28 | BagOutput(const std::string &bagfile, const std::string topic) 29 | : _bag(bagfile, rosbag::bagmode::Write), _topic(topic) {} 30 | 31 | void write(const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg, 32 | const cv::Mat &mat) override { 33 | cv_bridge::CvImage img_bridge(msg->header, 34 | sensor_msgs::image_encodings::RGB8, mat); 35 | 36 | sensor_msgs::Image output_msg; 37 | img_bridge.toImageMsg(output_msg); 38 | 39 | // Retain original message's timestamp 40 | _bag.write(_topic, msg->header.stamp, output_msg); 41 | } 42 | 43 | rosbag::Bag _bag; 44 | std::string _topic; 45 | }; 46 | 47 | void print_help(const po::options_description &description) { 48 | std::cout << "Usage:" << std::endl; 49 | std::cout << std::endl; 50 | std::cout << " bag2sonar [options] " << std::endl; 51 | std::cout << std::endl; 52 | std::cout << description; 53 | exit(0); 54 | } 55 | 56 | int main(int argc, char **argv) { 57 | po::options_description public_description("Draw sonar from a bagfile"); 58 | 59 | // The following code with three differente po::option_descriptions in a 60 | // slight-of-hand to hide the positional argment "input-files" 61 | // otherwise it shows up in print_help() which is ugly 62 | // 63 | // clang-format off 64 | public_description.add_options() 65 | ("help,h", "Display this help message") 66 | ("logscale,l", po::bool_switch()->default_value(true), "Do logscale") 67 | ("min-db", po::value()->default_value(0), "Min db") 68 | ("max-db", po::value()->default_value(0), "Max db") 69 | ("osd", po::bool_switch()->default_value(true), "If set, include the on-screen display in output") 70 | ("output-bag,o", po::value(), "Name of output bagfile") 71 | ("output-topic,t", po::value()->default_value("/drawn_sonar"), "Topic for images in output bagfile"); 72 | 73 | po::options_description hidden_description(""); 74 | hidden_description.add_options() 75 | ("input-files", po::value>()->required(), "Input files"); 76 | // clang-format on 77 | 78 | po::options_description full_description(""); 79 | full_description.add(public_description).add(hidden_description); 80 | 81 | po::positional_options_description p; 82 | p.add("input-files", -1); 83 | 84 | po::variables_map vm; 85 | 86 | try { 87 | po::store(po::command_line_parser(argc, argv) 88 | .options(full_description) 89 | .positional(p) 90 | .run(), 91 | vm); 92 | po::notify(vm); 93 | 94 | } catch (const boost::program_options::required_option &e) { 95 | // This exception occurs when a required option (input-files) isn't supplied 96 | // Catch that and display a help message instead. 97 | print_help(public_description); 98 | } catch (const std::exception &e) { 99 | // Catch any other random errors and exit 100 | std::cerr << e.what() << std::endl; 101 | exit(-1); 102 | } 103 | 104 | if (vm.count("help")) { 105 | print_help(public_description); 106 | } else if (vm.count("input-files")) { 107 | if (vm.count("input-files") > 1) { 108 | std::cerr << "Can only process one file at a time" << std::endl; 109 | exit(-1); 110 | } 111 | 112 | std::vector> outputs; 113 | 114 | if (vm.count("output-bag")) { 115 | outputs.push_back(std::make_shared( 116 | vm["output-bag"].as(), vm["output-topic"].as())); 117 | } 118 | 119 | sonar_image_proc::SonarDrawer sonar_drawer; 120 | std::unique_ptr color_map( 121 | new sonar_image_proc::InfernoColorMap); 122 | 123 | std::vector files = 124 | vm["input-files"].as>(); 125 | for (std::string file : files) { 126 | std::cout << "Processing input file " << file << std::endl; 127 | 128 | rosbag::Bag bag(file, rosbag::bagmode::Read); 129 | 130 | std::cout << "Bagfile " << file << " is " << bag.getSize() << " bytes" 131 | << std::endl; 132 | 133 | rosbag::View view( 134 | bag, rosbag::TypeQuery("marine_acoustic_msgs/ProjectedSonarImage")); 135 | 136 | int count = 0; 137 | 138 | BOOST_FOREACH (rosbag::MessageInstance const m, view) { 139 | marine_acoustic_msgs::ProjectedSonarImage::ConstPtr msg = 140 | m.instantiate(); 141 | 142 | sonar_image_proc::SonarImageMsgInterface interface(msg); 143 | if (vm["logscale"].as()) { 144 | interface.do_log_scale(vm["min-db"].as(), 145 | vm["max-db"].as()); 146 | } 147 | 148 | cv::Mat rectMat = 149 | sonar_drawer.drawRectSonarImage(interface, *color_map); 150 | 151 | cv::Mat sonarMat = sonar_drawer.remapRectSonarImage(interface, rectMat); 152 | 153 | cv::Mat outMat; 154 | if (vm["osd"].as()) { 155 | outMat = sonar_drawer.drawOverlay(interface, sonarMat); 156 | } else { 157 | outMat = sonarMat; 158 | } 159 | 160 | for (auto output : outputs) { 161 | output->write(msg, outMat); 162 | } 163 | 164 | count++; 165 | 166 | if ((count % 100) == 0) { 167 | std::cout << "Processed " << count << " sonar frames" << std::endl; 168 | } 169 | } 170 | 171 | bag.close(); 172 | } 173 | } 174 | 175 | exit(0); 176 | } 177 | -------------------------------------------------------------------------------- /scripts/sonar_fov.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | """ 3 | Copyright 2023 University of Washington Applied Physics Laboratory 4 | Author: Marc Micatka & Laura Lindzey 5 | """ 6 | from __future__ import annotations # use type of class in member function annotation. 7 | import numpy as np 8 | import rospy 9 | 10 | from marine_acoustic_msgs.msg import ProjectedSonarImage 11 | from geometry_msgs.msg import Point 12 | from std_msgs.msg import Header 13 | from visualization_msgs.msg import Marker, MarkerArray 14 | 15 | from sonar_image_proc.sonar_msg_metadata import SonarImageMetadata 16 | 17 | 18 | def build_vector_list(msg_metadata): 19 | """ 20 | From sonar parameters, build the wedge as list of vectors 21 | """ 22 | xx = [] 23 | yy = [] 24 | zz = [] 25 | 26 | # Calculate ending arc of the FOV in 3D space 27 | for elevation in [msg_metadata.min_elevation, msg_metadata.max_elevation]: 28 | ce = np.cos(elevation) 29 | se = np.sin(elevation) 30 | ca = np.cos(msg_metadata.azimuths) 31 | sa = np.sin(msg_metadata.azimuths) 32 | 33 | z = msg_metadata.max_range * ce * ca 34 | y = -1 * msg_metadata.max_range * ce * sa 35 | x = np.full_like(z, fill_value=msg_metadata.max_range * se) 36 | xx.extend(x) 37 | yy.extend(y) 38 | zz.extend(z) 39 | 40 | # Add a point at the origin 41 | xx_new = np.insert(np.array(xx), 0, 0) 42 | yy_new = np.insert(np.array(yy), 0, 0) 43 | zz_new = np.insert(np.array(zz), 0, 0) 44 | 45 | # Vertices are all the points on the edge of the wedge 46 | # Points are at the origin and then at the end of every arc 47 | vertices = np.zeros(shape=(len(xx_new), 3)) 48 | vertices[:, 0] = xx_new 49 | vertices[:, 1] = yy_new 50 | vertices[:, 2] = zz_new 51 | 52 | # The vertices, faces, and connections are directional in nature 53 | # Because the mesh is one-sided and will be invisible certain directions 54 | # if the order is incorrect 55 | 56 | # Number of faces: 57 | vertex_cnt = len(xx_new) + 1 58 | face_cnt = vertex_cnt // 2 - 2 59 | split = (len(xx_new) - 1) // 2 60 | 61 | pt0 = np.zeros(shape=(face_cnt), dtype=np.uint) 62 | pt1 = np.arange(1, face_cnt + 1, dtype=np.uint) 63 | pt2 = pt1 + 1 64 | pt3 = pt2 + face_cnt 65 | pt4 = pt3 + 1 66 | top_face = np.stack([pt0, pt1, pt2]).T 67 | btm_face = np.stack([pt4, pt3, pt0]).T 68 | 69 | # Add triangles on sides: 70 | left_face = np.array([[0, split + 1, 1]], dtype=np.uint) 71 | right_face = np.array([[0, split, len(xx_new) - 1]], dtype=np.uint) 72 | 73 | # Add triangles between elevations: 74 | first_pt = np.arange(1, split, dtype=np.uint) 75 | second_pt = first_pt + 1 76 | third_pt = second_pt + split - 1 77 | fourth_pt = third_pt + 1 78 | 79 | first_connection = np.stack([first_pt, third_pt, fourth_pt]).T 80 | second_connection = np.stack([first_pt, fourth_pt, second_pt]).T 81 | elevation_faces = np.vstack([first_connection, second_connection]) 82 | 83 | faces = np.vstack([top_face, btm_face, left_face, right_face, elevation_faces]) 84 | 85 | # Create array of vectors 86 | vectors = [] 87 | for f in faces: 88 | for j in range(3): 89 | x, y, z = vertices[f[j], :] 90 | pt = Point(x, y, z) 91 | vectors.append(pt) 92 | return vectors 93 | 94 | 95 | class SonarFOV: 96 | def __init__(self): 97 | self.sub = rospy.Subscriber( 98 | "sonar_image", 99 | ProjectedSonarImage, 100 | self.sonar_image_callback, 101 | queue_size=1, 102 | buff_size=1000000, 103 | ) 104 | self.pub_fov = rospy.Publisher("sonar_fov", MarkerArray, queue_size=10) 105 | 106 | # Alpha transparency for wedge 107 | self.alpha = rospy.get_param("~alpha", 1.0) 108 | if isinstance(self.alpha, str): 109 | self.alpha = float(self.alpha) 110 | 111 | # RGB color for wedge 112 | self.color = rospy.get_param("~color", [0.0, 1.0, 0.0]) 113 | if isinstance(self.color, str): 114 | self.color = eval(self.color) 115 | 116 | self.vector_list = None 117 | self.sonar_msg_metadata = None 118 | 119 | def generate_marker_array(self, vector_list) -> MarkerArray: 120 | """ 121 | Create MarkerArray from the pre-computed FOV wedge. 122 | Sets the color and alpha from rosparams 123 | """ 124 | obj = Marker() 125 | obj.type = Marker.TRIANGLE_LIST 126 | obj.id = 1 127 | obj.points = vector_list 128 | obj.frame_locked = True 129 | 130 | obj.scale.x = 1.0 131 | obj.scale.y = 1.0 132 | obj.scale.z = 1.0 133 | 134 | obj.color.r = self.color[0] 135 | obj.color.g = self.color[1] 136 | obj.color.b = self.color[2] 137 | obj.color.a = self.alpha 138 | 139 | wedge = MarkerArray() 140 | wedge.markers = [obj] 141 | return wedge 142 | 143 | def sonar_image_callback(self, sonar_image_msg: ProjectedSonarImage): 144 | """ 145 | Callback to publish the marker array containing the FOV wedge. 146 | Only updates the geometry if parameters change. 147 | """ 148 | rospy.logdebug( 149 | "Received new image, seq %d at %f" 150 | % (sonar_image_msg.header.seq, sonar_image_msg.header.stamp.to_sec()) 151 | ) 152 | 153 | # For the sonar_fov script, the elevation steps need to be 2 154 | # The logic to draw the mesh is not generic to step counts 155 | new_metadata = SonarImageMetadata(sonar_image_msg) 156 | generate_fov_flag = False 157 | if self.sonar_msg_metadata is None: 158 | self.sonar_msg_metadata = new_metadata 159 | generate_fov_flag = True # generate the fov stl 160 | else: 161 | # Because the dictionary contains numpy arrays, a simple check for a == b does not work. 162 | # Using allclose because the range occasionally changes by fractions 163 | if ( 164 | self.sonar_msg_metadata is None 165 | or self.sonar_msg_metadata != new_metadata 166 | ): 167 | # things have changed, regenerate the fov 168 | rospy.logdebug("Updating Parameters of FOV") 169 | self.sonar_msg_metadata = new_metadata 170 | generate_fov_flag = True 171 | 172 | if generate_fov_flag: 173 | rospy.logdebug("Generating FOV mesh...") 174 | self.vector_list = build_vector_list(self.sonar_msg_metadata) 175 | 176 | wedge = self.generate_marker_array(self.vector_list) 177 | for obj in wedge.markers: 178 | obj.header = sonar_image_msg.header 179 | self.pub_fov.publish(wedge) 180 | 181 | 182 | if __name__ == "__main__": 183 | rospy.init_node("sonar_fov") 184 | fov_publisher = SonarFOV() 185 | rospy.spin() 186 | -------------------------------------------------------------------------------- /scripts/sonar_pointcloud.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | """ 3 | Copyright 2023 University of Washington Applied Physics Laboratory 4 | Author: Marc Micatka & Laura Lindzey 5 | """ 6 | 7 | from __future__ import annotations # use type of class in member function annotation. 8 | from matplotlib import cm 9 | import numpy as np 10 | import rospy 11 | import time 12 | from marine_acoustic_msgs.msg import ProjectedSonarImage, SonarImageData 13 | from sensor_msgs.msg import PointCloud2, PointField 14 | from std_msgs.msg import Header 15 | 16 | from sonar_image_proc.sonar_msg_metadata import SonarImageMetadata 17 | 18 | 19 | def make_geometry(sonar_msg_metadata: SonarImageMetadata, elevations) -> np.array: 20 | """ 21 | Vectorized geometry generation. 22 | Regenerates when there are changing parameters from the sonar. 23 | 24 | The original make_geometry() fxn was nested loops. We replaced it with a vectorized solution but 25 | for the purposes of making the indexing and geometry creation more clear, 26 | here is the original implementation: 27 | 28 | ces = np.cos(elevations) 29 | ses = np.sin(elevations) 30 | cas = np.cos(self.azimuths) 31 | sas = np.sin(self.azimuths) 32 | for kk, (ce, se) in enumerate(zip(ces, ses)): 33 | for ii, (ca, sa) in enumerate(zip(cas, sas)): 34 | for jj, distance in enumerate(self.ranges): 35 | idx = ii + jj * self.num_angles 36 | zz = distance * ce * ca 37 | yy = distance * ce * sa 38 | xx = distance * se 39 | points[kk][idx] = [xx, yy, zz] 40 | """ 41 | rospy.logdebug("Making Geometry") 42 | begin_time = time.time() 43 | # Pre-compute these values to speed up the loop 44 | # Compute the idxs to properly index into the points array 45 | idxs = np.arange(0, sonar_msg_metadata.num_angles * sonar_msg_metadata.num_ranges) 46 | idxs = idxs.reshape( 47 | sonar_msg_metadata.num_ranges, sonar_msg_metadata.num_angles 48 | ).flatten(order="F") 49 | 50 | # Compute the cosines and sines of elevations and azimuths 51 | ces = np.cos(elevations) 52 | ses = np.sin(elevations) 53 | cas = np.cos(sonar_msg_metadata.azimuths) 54 | sas = np.sin(sonar_msg_metadata.azimuths) 55 | 56 | # Allocate points 57 | new_shape = ( 58 | len(elevations), 59 | sonar_msg_metadata.num_ranges * sonar_msg_metadata.num_angles, 60 | 3, 61 | ) 62 | points = np.zeros(shape=(new_shape)) 63 | 64 | x_temp = np.tile( 65 | sonar_msg_metadata.ranges[np.newaxis, :] * ses[:, np.newaxis], 66 | reps=sonar_msg_metadata.num_angles, 67 | ).flatten() 68 | y_temp = ( 69 | sonar_msg_metadata.ranges[np.newaxis, np.newaxis, :] 70 | * ces[:, np.newaxis, np.newaxis] 71 | * sas[np.newaxis, :, np.newaxis] 72 | ).flatten() 73 | z_temp = ( 74 | sonar_msg_metadata.ranges[np.newaxis, np.newaxis, :] 75 | * ces[:, np.newaxis, np.newaxis] 76 | * cas[np.newaxis, :, np.newaxis] 77 | ).flatten() 78 | 79 | points[:, idxs, :] = np.stack([x_temp, y_temp, z_temp], axis=1).reshape(new_shape) 80 | 81 | total_time = time.time() - begin_time 82 | rospy.logdebug(f"Creating geometry took {1000*total_time:0.2f} ms") 83 | return points 84 | 85 | 86 | def make_color_lookup() -> np.array: 87 | """ 88 | Generates a lookup table from matplotlib inferno colormapping 89 | """ 90 | color_lookup = np.zeros(shape=(256, 4)) 91 | 92 | for aa in range(256): 93 | r, g, b, _ = cm.inferno(aa) 94 | alpha = aa / 256 95 | 96 | color_lookup[aa, :] = [r, g, b, alpha] 97 | return color_lookup 98 | 99 | 100 | class SonarPointcloud(object): 101 | """ 102 | Subscribe to ProjectedSonarImage messages and publish slices of the 103 | intensity array as rviz MarkerArrays. 104 | """ 105 | 106 | def __init__(self): 107 | # NB: if queue_size is set, have to be sure buff_size is sufficiently large, 108 | # since it can only discard messages that are fully in the buffer. 109 | # https://stackoverflow.com/questions/26415699/ros-subscriber-not-up-to-date 110 | # (200k didn't work, and sys.getsizeof doesn't return an accurate size of the whole object) 111 | self.sub = rospy.Subscriber( 112 | "sonar_image", 113 | ProjectedSonarImage, 114 | self.sonar_image_callback, 115 | queue_size=1, 116 | buff_size=1000000, 117 | ) 118 | self.pub = rospy.Publisher("sonar_cloud", PointCloud2, queue_size=1) 119 | 120 | # Flag to determine whether we publish ALL points or only non-zero points 121 | self.publish_all_points = rospy.get_param("~publish_all_points", False) 122 | 123 | # cmin is a color scaling parameter. The log-scaled and normed intensity values 124 | # get scaled to (cmin, 1.0) 125 | self.cmin = rospy.get_param("~cmin", 0.74) 126 | 127 | # threshold is the publish parameter if publish_all_points is false 128 | self.threshold = rospy.get_param("~threshold", 0) 129 | 130 | # Number of planes to add to the pointcloud projection. 131 | # Steps are evenly distributed between the max and min elevation angles. 132 | elev_steps = rospy.get_param("~elev_steps", 2) 133 | if isinstance(elev_steps, (float, str)): 134 | elev_steps = int(elev_steps) 135 | min_elev_deg = rospy.get_param("~min_elev_deg", -10) 136 | max_elev_deg = rospy.get_param("~max_elev_deg", 10) 137 | assert max_elev_deg >= min_elev_deg 138 | min_elev = np.radians(min_elev_deg) 139 | max_elev = np.radians(max_elev_deg) 140 | self.elevations = np.linspace(min_elev, max_elev, elev_steps) 141 | 142 | # x,y,z coordinates of points 143 | # [ELEVATION_IDX, INTENSITY_IDX, DIMENSION_IDX] 144 | # Shape chosen for ease of mapping coordinates to intensities 145 | self.geometry = None 146 | 147 | # We need to detect whether the geometry has changed. 148 | self.sonar_msg_metadata = None 149 | 150 | self.color_lookup = None 151 | self.output_points = None 152 | 153 | def normalize_intensity_array(self, image: SonarImageData): 154 | """ 155 | Process an intensity array into a parseable format for pointcloud generation 156 | Can handle 8bit or 32bit input data, will log scale output data 157 | 158 | Input intensities are on the range [0, INT_MAX]. 159 | Output intensities are log-scaled, normalized to [0, 1] 160 | """ 161 | if image.dtype == image.DTYPE_UINT8: 162 | data_type = np.uint8 163 | elif image.dtype == image.DTYPE_UINT32: 164 | data_type = np.uint32 165 | else: 166 | raise Exception("Only 8 bit and 32 bit data is supported!") 167 | 168 | intensities = np.frombuffer(image.data, dtype=data_type) 169 | new_intensites = intensities.astype(np.float32) 170 | # Scaling calculation is taken from sonar_image_proc/ros/src/sonar_postprocessor_nodelet.cpp 171 | # First we log scale the intensities 172 | 173 | # Avoid log(0) and scale full range of possible intensities to [0,1] 174 | vv = np.log(np.maximum(1, new_intensites)) / np.log(np.iinfo(data_type).max) 175 | return vv 176 | 177 | def sonar_image_callback(self, sonar_image_msg: ProjectedSonarImage): 178 | """ 179 | Convert img_msg into point cloud with color mappings via numpy. 180 | """ 181 | begin_time = time.time() 182 | rospy.logdebug( 183 | "Received new image, seq %d at %f" 184 | % (sonar_image_msg.header.seq, sonar_image_msg.header.stamp.to_sec()) 185 | ) 186 | header = Header() 187 | header = sonar_image_msg.header 188 | 189 | frame_id = rospy.get_param("~frame_id", None) 190 | if frame_id: 191 | header.frame_id = frame_id 192 | 193 | fields = [ 194 | PointField("x", 0, PointField.FLOAT32, 1), 195 | PointField("y", 4, PointField.FLOAT32, 1), 196 | PointField("z", 8, PointField.FLOAT32, 1), 197 | PointField("r", 12, PointField.FLOAT32, 1), 198 | PointField("g", 16, PointField.FLOAT32, 1), 199 | PointField("b", 20, PointField.FLOAT32, 1), 200 | PointField("a", 24, PointField.FLOAT32, 1), 201 | ] 202 | 203 | new_metadata = SonarImageMetadata(sonar_image_msg) 204 | if self.sonar_msg_metadata is None or self.sonar_msg_metadata != new_metadata: 205 | rospy.logdebug( 206 | f"Metadata updated! \n" 207 | f"Old: {self.sonar_msg_metadata} \n" 208 | f"New: {new_metadata}" 209 | ) 210 | 211 | self.sonar_msg_metadata = new_metadata 212 | self.geometry = make_geometry(self.sonar_msg_metadata, self.elevations) 213 | 214 | if self.color_lookup is None: 215 | self.color_lookup = make_color_lookup() 216 | t0 = time.time() 217 | 218 | normalized_intensities = self.normalize_intensity_array(sonar_image_msg.image) 219 | 220 | # Threshold pointcloud based on intensity if not publishing all points 221 | if self.publish_all_points: 222 | selected_intensities = normalized_intensities 223 | geometry = self.geometry 224 | else: 225 | pos_intensity_idx = np.where(normalized_intensities > self.threshold) 226 | selected_intensities = normalized_intensities[pos_intensity_idx] 227 | geometry = self.geometry[:, pos_intensity_idx[0]] 228 | 229 | num_original = len(normalized_intensities) 230 | num_selected = len(selected_intensities) 231 | rospy.logdebug( 232 | f"Filtering Results: (Pts>Thresh, Total): {num_selected, num_original}. " 233 | f"Frac: {(num_selected/num_original):.3f}" 234 | ) 235 | 236 | # Finally, apply colormap to intensity values. 237 | v_max = 1.0 238 | colors = (selected_intensities - self.cmin) / (v_max - self.cmin) 239 | c_clipped = np.clip(colors, a_min=0.0, a_max=1.0) 240 | c_uint8 = (np.iinfo(np.uint8).max * c_clipped).astype(np.uint8) 241 | 242 | # Allocate our output points 243 | npts = len(selected_intensities) 244 | self.output_points = np.zeros( 245 | (len(self.elevations) * npts, 7), dtype=np.float32 246 | ) 247 | 248 | # Expand out intensity array (for fast comparison) 249 | # The np.where call setting colors requires an array of shape (npts, 4). 250 | # selected_intensities has shape (npts,), but np.repeat requires (npts, 1). 251 | c_expanded = np.repeat(c_uint8[..., np.newaxis], 4, axis=1) 252 | elev_points = np.empty((npts, 7)) 253 | elev_points[:, 3:] = self.color_lookup[c_expanded[:, 0]] 254 | 255 | # Fill the output array 256 | for i in range(len(self.elevations)): 257 | elev_points[:, 0:3] = geometry[i, :, :] 258 | step = i * npts 259 | next_step = step + npts 260 | self.output_points[step:next_step, :] = elev_points 261 | 262 | t1 = time.time() 263 | n_pub = len(self.output_points) 264 | cloud_msg = PointCloud2( 265 | header=header, 266 | height=1, 267 | width=n_pub, 268 | is_dense=True, 269 | is_bigendian=False, 270 | fields=fields, 271 | point_step=7 * 4, 272 | row_step=7 * 4 * n_pub, 273 | data=self.output_points.tobytes(), 274 | ) 275 | 276 | dt1 = time.time() - t1 277 | dt0 = t1 - t0 278 | total_time = time.time() - begin_time 279 | self.pub.publish(cloud_msg) 280 | 281 | rospy.logdebug( 282 | f"published pointcloud: npts = {npts}, Find Pts = {dt0:0.5f} sec, " 283 | f"Convert to Cloud = {dt1:0.5f} sec. " 284 | f"Total Time = {total_time:0.3f} sec" 285 | ) 286 | 287 | 288 | if __name__ == "__main__": 289 | rospy.init_node("sonar_pointcloud") 290 | pointcloud_publisher = SonarPointcloud() 291 | rospy.spin() 292 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # DO NOT USE WITH pip 2 | # 3 | # Stub for ROS / catkin _only_ 4 | 5 | from distutils.core import setup 6 | 7 | setup( 8 | version='0.0.1', 9 | scripts=['python/histogram_drawer'], 10 | packages=['sonar_image_proc'], 11 | package_dir={'': 'python/'} 12 | ) 13 | -------------------------------------------------------------------------------- /sonar_image_proc.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: hydrographic_msgs, uri: "https://github.com/apl-ocean-engineering/hydrographic_msgs.git"} 2 | -------------------------------------------------------------------------------- /test/unit/test_draw_sonar.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | #include 6 | using namespace std; 7 | 8 | #include "sonar_image_proc/DrawSonar.h" 9 | using namespace sonar_image_proc; 10 | 11 | TEST(TestDrawSonar, NullTest) { ASSERT_TRUE(true); } 12 | --------------------------------------------------------------------------------