From 80e415530d5c1fbb629f7ed1cf05431f8201f1d4 Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Sun, 25 Jan 2026 16:56:12 -0800 Subject: [PATCH] Major restructure --- .drone.yml | 59 ---- .github/dependabot.yml | 6 + .github/mergify.yml | 18 + .github/workflows/ci.yaml | 31 ++ .github/workflows/format.yaml | 34 ++ {ros/cfg => cfg}/DrawSonar.cfg | 0 fips | 17 - fips.yml | 15 - helpers/epilogue.txt | 2 - helpers/inferno_color_map.csv | 256 -------------- helpers/make_color_map.py | 47 --- helpers/prologue.txt | 13 - .../sonar_image_proc/AbstractSonarInterface.h | 146 -------- include/sonar_image_proc/ColorMaps.h | 109 ------ include/sonar_image_proc/DataStructures.h | 22 -- include/sonar_image_proc/DrawSonar.h | 75 ---- include/sonar_image_proc/HistogramGenerator.h | 24 -- include/sonar_image_proc/OverlayImage.h | 114 ------ include/sonar_image_proc/SonarDrawer.h | 184 ---------- .../sonar_image_msg_interface.h | 0 lib/AbstractSonarInterface.cpp | 45 --- lib/ColorMaps.cpp | 213 ----------- lib/HistogramGenerator.cpp | 79 ----- lib/OldDrawSonar.cpp | 139 -------- lib/SonarDrawer.cpp | 321 ----------------- nodelet_plugins.xml | 13 - python/.gitignore | 1 - python/README.md | 8 - python/draw_sonar/__init__.py | 6 - python/histogram_drawer | 71 ---- python/ndarray_converter.cpp | 330 ------------------ python/ndarray_converter.h | 43 --- python/old_setup.py | 120 ------- python/py_draw_sonar.cpp | 95 ----- python/sonar_image_proc/__init__.py | 0 python/sonar_image_proc/sonar_msg_metadata.py | 65 ---- python/test.py | 4 - ros/src/draw_sonar_node.cpp | 19 - ros/src/sonar_postprocessor_node.cpp | 19 - ..._proc.rosinstall => sonar_image_proc.repos | 0 {ros/src => src}/draw_sonar_nodelet.cpp | 0 src/draw_sonar_parameters.yaml | 72 ++++ .../sonar_postprocessor_nodelet.cpp | 0 src/sonar_postprocessor_parameters.yaml | 14 + {ros/tools => tools}/bag2sonar.cpp | 0 45 files changed, 175 insertions(+), 2674 deletions(-) delete mode 100644 .drone.yml create mode 100644 .github/dependabot.yml create mode 100644 .github/mergify.yml create mode 100644 .github/workflows/ci.yaml create mode 100644 .github/workflows/format.yaml rename {ros/cfg => cfg}/DrawSonar.cfg (100%) delete mode 100755 fips delete mode 100644 fips.yml delete mode 100644 helpers/epilogue.txt delete mode 100644 helpers/inferno_color_map.csv delete mode 100755 helpers/make_color_map.py delete mode 100644 helpers/prologue.txt delete mode 100644 include/sonar_image_proc/AbstractSonarInterface.h delete mode 100644 include/sonar_image_proc/ColorMaps.h delete mode 100644 include/sonar_image_proc/DataStructures.h delete mode 100644 include/sonar_image_proc/DrawSonar.h delete mode 100644 include/sonar_image_proc/HistogramGenerator.h delete mode 100644 include/sonar_image_proc/OverlayImage.h delete mode 100644 include/sonar_image_proc/SonarDrawer.h rename {ros/include => include}/sonar_image_proc/sonar_image_msg_interface.h (100%) delete mode 100644 lib/AbstractSonarInterface.cpp delete mode 100644 lib/ColorMaps.cpp delete mode 100644 lib/HistogramGenerator.cpp delete mode 100644 lib/OldDrawSonar.cpp delete mode 100644 lib/SonarDrawer.cpp delete mode 100644 nodelet_plugins.xml delete mode 100644 python/.gitignore delete mode 100644 python/README.md delete mode 100644 python/draw_sonar/__init__.py delete mode 100755 python/histogram_drawer delete mode 100644 python/ndarray_converter.cpp delete mode 100644 python/ndarray_converter.h delete mode 100644 python/old_setup.py delete mode 100644 python/py_draw_sonar.cpp delete mode 100644 python/sonar_image_proc/__init__.py delete mode 100644 python/sonar_image_proc/sonar_msg_metadata.py delete mode 100644 python/test.py delete mode 100644 ros/src/draw_sonar_node.cpp delete mode 100644 ros/src/sonar_postprocessor_node.cpp rename sonar_image_proc.rosinstall => sonar_image_proc.repos (100%) rename {ros/src => src}/draw_sonar_nodelet.cpp (100%) create mode 100644 src/draw_sonar_parameters.yaml rename {ros/src => src}/sonar_postprocessor_nodelet.cpp (100%) create mode 100644 src/sonar_postprocessor_parameters.yaml rename {ros/tools => tools}/bag2sonar.cpp (100%) diff --git a/.drone.yml b/.drone.yml deleted file mode 100644 index 4538130..0000000 --- a/.drone.yml +++ /dev/null @@ -1,59 +0,0 @@ -kind: pipeline -type: docker -name: build - -platform: - os: linux - arch: amd64 - -steps: - ## Biuld in fips (debug with unittesting) -- name: build-fips-unittest - image: amarburg/lsdslam-dev-host - commands: - - ./fips set config linux-make-unittest - - ./fips build - - ## Build in ros melodic -# - name: build-ros-melodic -# image: amarburg/drone-ci-ros-melodic:latest -# commands: -# - wget -O- https://gitlab.com/amarburg/ros_drone_ci/-/raw/master/bootstrap.sh | /bin/bash -# environment: -# WSTOOL_RECURSIVE: true -# ROS_PACKAGES_TO_INSTALL: cv-bridge image-transport - - ## Build in ros noetic -- name: build-ros-noetic - image: amarburg/drone-ci-ros-noetic:latest - commands: - - wget -O- https://gitlab.com/amarburg/ros_drone_ci/-/raw/master/bootstrap.sh | /bin/bash - environment: - WSTOOL_RECURSIVE: true - ROS_PACKAGES_TO_INSTALL: cv-bridge image-transport - - ## Trigger downstream builds on Github -# - name: trigger-github-downstream -# image: plugins/downstream -# settings: -# server: https://github.drone.camhd.science -# fork: true -# token: -# from_secret: github_drone_token -# repositories: -# - apl-ocean-engineering/serdp_common -# when: -# event: -# exclude: -# - pull_request - -- name: slack - image: plugins/slack - settings: - webhook: - from_secret: slack_webhook - when: - status: [ success, failure ] - event: - exclude: - - pull_request diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..79fc83a --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,6 @@ +version: 2 +updates: + - package-ecosystem: github-actions + directory: "/" + schedule: + interval: "weekly" diff --git a/.github/mergify.yml b/.github/mergify.yml new file mode 100644 index 0000000..38eb702 --- /dev/null +++ b/.github/mergify.yml @@ -0,0 +1,18 @@ +pull_request_rules: + + - name: ask to resolve conflict + conditions: + - conflict + - author!=mergify[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: development targets main branch + conditions: + - base!=main + - author!=mergify[bot] + actions: + comment: + message: | + Please target the `main` branch for development, we will backport the changes to {{base}} for you if approved and if they don't break API. diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..f92fe45 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,31 @@ +name: ROS Industrial CI + +on: + push: + branches: + - main + pull_request: + workflow_dispatch: + +jobs: + industrial-ci: + name: Run Industrial CI + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + env: + # We include humble because there are still 22.04 systems (Jetson) out there + - {UPSTREAM_WORKSPACE: sonar_image_proc.repos, ROS_DISTRO: humble} + - {UPSTREAM_WORKSPACE: sonar_image_proc.repos, ROS_DISTRO: kilted} + - {UPSTREAM_WORKSPACE: sonar_image_proc.repos, ROS_DISTRO: rolling} + + steps: + - name: Checkout repository + uses: actions/checkout@v5 + with: + submodules: recursive + + - name: Run ROS Industrial CI + uses: ros-industrial/industrial_ci@master + env: ${{matrix.env}} diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml new file mode 100644 index 0000000..b558fa2 --- /dev/null +++ b/.github/workflows/format.yaml @@ -0,0 +1,34 @@ +name: Formatting (pre-commit) + +on: + pull_request: + push: + branches: + - main + workflow_dispatch: + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v5 + + - name: Setup Python + uses: actions/setup-python@v6 + with: + python-version: "3.10" + + - name: Install clang-format-14 + run: sudo apt-get install clang-format-14 + + - name: Run pre-commit + uses: pre-commit/action@v3.0.1 + id: precommit + + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit diff --git a/ros/cfg/DrawSonar.cfg b/cfg/DrawSonar.cfg similarity index 100% rename from ros/cfg/DrawSonar.cfg rename to cfg/DrawSonar.cfg diff --git a/fips b/fips deleted file mode 100755 index efdeb88..0000000 --- a/fips +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python -"""fips main entry""" -import os -import sys -import subprocess -proj_path = os.path.dirname(os.path.abspath(__file__)) -fips_path = os.path.dirname(proj_path) + '/fips' -if not os.path.isdir(fips_path) : - print("\033[93m=== cloning fips build system to '{}':\033[0m".format(fips_path)) - subprocess.call(['git', 'clone', 'https://github.com/amarburg/fips.git', fips_path]) -sys.path.insert(0,fips_path) -try : - from mod import fips -except ImportError : - print("\033[91m[ERROR]\033[0m failed to initialize fips build system in '{}'".format(proj_path)) - sys.exit(10) -fips.run(fips_path, proj_path, sys.argv) diff --git a/fips.yml b/fips.yml deleted file mode 100644 index ede8bb8..0000000 --- a/fips.yml +++ /dev/null @@ -1,15 +0,0 @@ -imports: - fips-googletest: - git: https://github.com/amarburg/fips-googletest.git - cond: "FIPS_UNITTESTS" - -exports: - modules: - draw_sonar: . - header-dirs: - - include - -defines: - FIPS_EXCEPTIONS: ON - FIPS_RTTI: ON - FIPS_UNITTESTS_RUN_AFTER_BUILD: ON diff --git a/helpers/epilogue.txt b/helpers/epilogue.txt deleted file mode 100644 index abe0a3e..0000000 --- a/helpers/epilogue.txt +++ /dev/null @@ -1,2 +0,0 @@ - -} // namespace sonar_image_proc diff --git a/helpers/inferno_color_map.csv b/helpers/inferno_color_map.csv deleted file mode 100644 index e9cb514..0000000 --- a/helpers/inferno_color_map.csv +++ /dev/null @@ -1,256 +0,0 @@ -0.001462, 0.000466, 0.013866 -0.002267, 0.001270, 0.018570 -0.003299, 0.002249, 0.024239 -0.004547, 0.003392, 0.030909 -0.006006, 0.004692, 0.038558 -0.007676, 0.006136, 0.046836 -0.009561, 0.007713, 0.055143 -0.011663, 0.009417, 0.063460 -0.013995, 0.011225, 0.071862 -0.016561, 0.013136, 0.080282 -0.019373, 0.015133, 0.088767 -0.022447, 0.017199, 0.097327 -0.025793, 0.019331, 0.105930 -0.029432, 0.021503, 0.114621 -0.033385, 0.023702, 0.123397 -0.037668, 0.025921, 0.132232 -0.042253, 0.028139, 0.141141 -0.046915, 0.030324, 0.150164 -0.051644, 0.032474, 0.159254 -0.056449, 0.034569, 0.168414 -0.061340, 0.036590, 0.177642 -0.066331, 0.038504, 0.186962 -0.071429, 0.040294, 0.196354 -0.076637, 0.041905, 0.205799 -0.081962, 0.043328, 0.215289 -0.087411, 0.044556, 0.224813 -0.092990, 0.045583, 0.234358 -0.098702, 0.046402, 0.243904 -0.104551, 0.047008, 0.253430 -0.110536, 0.047399, 0.262912 -0.116656, 0.047574, 0.272321 -0.122908, 0.047536, 0.281624 -0.129285, 0.047293, 0.290788 -0.135778, 0.046856, 0.299776 -0.142378, 0.046242, 0.308553 -0.149073, 0.045468, 0.317085 -0.155850, 0.044559, 0.325338 -0.162689, 0.043554, 0.333277 -0.169575, 0.042489, 0.340874 -0.176493, 0.041402, 0.348111 -0.183429, 0.040329, 0.354971 -0.190367, 0.039309, 0.361447 -0.197297, 0.038400, 0.367535 -0.204209, 0.037632, 0.373238 -0.211095, 0.037030, 0.378563 -0.217949, 0.036615, 0.383522 -0.224763, 0.036405, 0.388129 -0.231538, 0.036405, 0.392400 -0.238273, 0.036621, 0.396353 -0.244967, 0.037055, 0.400007 -0.251620, 0.037705, 0.403378 -0.258234, 0.038571, 0.406485 -0.264810, 0.039647, 0.409345 -0.271347, 0.040922, 0.411976 -0.277850, 0.042353, 0.414392 -0.284321, 0.043933, 0.416608 -0.290763, 0.045644, 0.418637 -0.297178, 0.047470, 0.420491 -0.303568, 0.049396, 0.422182 -0.309935, 0.051407, 0.423721 -0.316282, 0.053490, 0.425116 -0.322610, 0.055634, 0.426377 -0.328921, 0.057827, 0.427511 -0.335217, 0.060060, 0.428524 -0.341500, 0.062325, 0.429425 -0.347771, 0.064616, 0.430217 -0.354032, 0.066925, 0.430906 -0.360284, 0.069247, 0.431497 -0.366529, 0.071579, 0.431994 -0.372768, 0.073915, 0.432400 -0.379001, 0.076253, 0.432719 -0.385228, 0.078591, 0.432955 -0.391453, 0.080927, 0.433109 -0.397674, 0.083257, 0.433183 -0.403894, 0.085580, 0.433179 -0.410113, 0.087896, 0.433098 -0.416331, 0.090203, 0.432943 -0.422549, 0.092501, 0.432714 -0.428768, 0.094790, 0.432412 -0.434987, 0.097069, 0.432039 -0.441207, 0.099338, 0.431594 -0.447428, 0.101597, 0.431080 -0.453651, 0.103848, 0.430498 -0.459875, 0.106089, 0.429846 -0.466100, 0.108322, 0.429125 -0.472328, 0.110547, 0.428334 -0.478558, 0.112764, 0.427475 -0.484789, 0.114974, 0.426548 -0.491022, 0.117179, 0.425552 -0.497257, 0.119379, 0.424488 -0.503493, 0.121575, 0.423356 -0.509730, 0.123769, 0.422156 -0.515967, 0.125960, 0.420887 -0.522206, 0.128150, 0.419549 -0.528444, 0.130341, 0.418142 -0.534683, 0.132534, 0.416667 -0.540920, 0.134729, 0.415123 -0.547157, 0.136929, 0.413511 -0.553392, 0.139134, 0.411829 -0.559624, 0.141346, 0.410078 -0.565854, 0.143567, 0.408258 -0.572081, 0.145797, 0.406369 -0.578304, 0.148039, 0.404411 -0.584521, 0.150294, 0.402385 -0.590734, 0.152563, 0.400290 -0.596940, 0.154848, 0.398125 -0.603139, 0.157151, 0.395891 -0.609330, 0.159474, 0.393589 -0.615513, 0.161817, 0.391219 -0.621685, 0.164184, 0.388781 -0.627847, 0.166575, 0.386276 -0.633998, 0.168992, 0.383704 -0.640135, 0.171438, 0.381065 -0.646260, 0.173914, 0.378359 -0.652369, 0.176421, 0.375586 -0.658463, 0.178962, 0.372748 -0.664540, 0.181539, 0.369846 -0.670599, 0.184153, 0.366879 -0.676638, 0.186807, 0.363849 -0.682656, 0.189501, 0.360757 -0.688653, 0.192239, 0.357603 -0.694627, 0.195021, 0.354388 -0.700576, 0.197851, 0.351113 -0.706500, 0.200728, 0.347777 -0.712396, 0.203656, 0.344383 -0.718264, 0.206636, 0.340931 -0.724103, 0.209670, 0.337424 -0.729909, 0.212759, 0.333861 -0.735683, 0.215906, 0.330245 -0.741423, 0.219112, 0.326576 -0.747127, 0.222378, 0.322856 -0.752794, 0.225706, 0.319085 -0.758422, 0.229097, 0.315266 -0.764010, 0.232554, 0.311399 -0.769556, 0.236077, 0.307485 -0.775059, 0.239667, 0.303526 -0.780517, 0.243327, 0.299523 -0.785929, 0.247056, 0.295477 -0.791293, 0.250856, 0.291390 -0.796607, 0.254728, 0.287264 -0.801871, 0.258674, 0.283099 -0.807082, 0.262692, 0.278898 -0.812239, 0.266786, 0.274661 -0.817341, 0.270954, 0.270390 -0.822386, 0.275197, 0.266085 -0.827372, 0.279517, 0.261750 -0.832299, 0.283913, 0.257383 -0.837165, 0.288385, 0.252988 -0.841969, 0.292933, 0.248564 -0.846709, 0.297559, 0.244113 -0.851384, 0.302260, 0.239636 -0.855992, 0.307038, 0.235133 -0.860533, 0.311892, 0.230606 -0.865006, 0.316822, 0.226055 -0.869409, 0.321827, 0.221482 -0.873741, 0.326906, 0.216886 -0.878001, 0.332060, 0.212268 -0.882188, 0.337287, 0.207628 -0.886302, 0.342586, 0.202968 -0.890341, 0.347957, 0.198286 -0.894305, 0.353399, 0.193584 -0.898192, 0.358911, 0.188860 -0.902003, 0.364492, 0.184116 -0.905735, 0.370140, 0.179350 -0.909390, 0.375856, 0.174563 -0.912966, 0.381636, 0.169755 -0.916462, 0.387481, 0.164924 -0.919879, 0.393389, 0.160070 -0.923215, 0.399359, 0.155193 -0.926470, 0.405389, 0.150292 -0.929644, 0.411479, 0.145367 -0.932737, 0.417627, 0.140417 -0.935747, 0.423831, 0.135440 -0.938675, 0.430091, 0.130438 -0.941521, 0.436405, 0.125409 -0.944285, 0.442772, 0.120354 -0.946965, 0.449191, 0.115272 -0.949562, 0.455660, 0.110164 -0.952075, 0.462178, 0.105031 -0.954506, 0.468744, 0.099874 -0.956852, 0.475356, 0.094695 -0.959114, 0.482014, 0.089499 -0.961293, 0.488716, 0.084289 -0.963387, 0.495462, 0.079073 -0.965397, 0.502249, 0.073859 -0.967322, 0.509078, 0.068659 -0.969163, 0.515946, 0.063488 -0.970919, 0.522853, 0.058367 -0.972590, 0.529798, 0.053324 -0.974176, 0.536780, 0.048392 -0.975677, 0.543798, 0.043618 -0.977092, 0.550850, 0.039050 -0.978422, 0.557937, 0.034931 -0.979666, 0.565057, 0.031409 -0.980824, 0.572209, 0.028508 -0.981895, 0.579392, 0.026250 -0.982881, 0.586606, 0.024661 -0.983779, 0.593849, 0.023770 -0.984591, 0.601122, 0.023606 -0.985315, 0.608422, 0.024202 -0.985952, 0.615750, 0.025592 -0.986502, 0.623105, 0.027814 -0.986964, 0.630485, 0.030908 -0.987337, 0.637890, 0.034916 -0.987622, 0.645320, 0.039886 -0.987819, 0.652773, 0.045581 -0.987926, 0.660250, 0.051750 -0.987945, 0.667748, 0.058329 -0.987874, 0.675267, 0.065257 -0.987714, 0.682807, 0.072489 -0.987464, 0.690366, 0.079990 -0.987124, 0.697944, 0.087731 -0.986694, 0.705540, 0.095694 -0.986175, 0.713153, 0.103863 -0.985566, 0.720782, 0.112229 -0.984865, 0.728427, 0.120785 -0.984075, 0.736087, 0.129527 -0.983196, 0.743758, 0.138453 -0.982228, 0.751442, 0.147565 -0.981173, 0.759135, 0.156863 -0.980032, 0.766837, 0.166353 -0.978806, 0.774545, 0.176037 -0.977497, 0.782258, 0.185923 -0.976108, 0.789974, 0.196018 -0.974638, 0.797692, 0.206332 -0.973088, 0.805409, 0.216877 -0.971468, 0.813122, 0.227658 -0.969783, 0.820825, 0.238686 -0.968041, 0.828515, 0.249972 -0.966243, 0.836191, 0.261534 -0.964394, 0.843848, 0.273391 -0.962517, 0.851476, 0.285546 -0.960626, 0.859069, 0.298010 -0.958720, 0.866624, 0.310820 -0.956834, 0.874129, 0.323974 -0.954997, 0.881569, 0.337475 -0.953215, 0.888942, 0.351369 -0.951546, 0.896226, 0.365627 -0.950018, 0.903409, 0.380271 -0.948683, 0.910473, 0.395289 -0.947594, 0.917399, 0.410665 -0.946809, 0.924168, 0.426373 -0.946392, 0.930761, 0.442367 -0.946403, 0.937159, 0.458592 -0.946903, 0.943348, 0.474970 -0.947937, 0.949318, 0.491426 -0.949545, 0.955063, 0.507860 -0.951740, 0.960587, 0.524203 -0.954529, 0.965896, 0.540361 -0.957896, 0.971003, 0.556275 -0.961812, 0.975924, 0.571925 -0.966249, 0.980678, 0.587206 -0.971162, 0.985282, 0.602154 -0.976511, 0.989753, 0.616760 -0.982257, 0.994109, 0.631017 -0.988362, 0.998364, 0.644924 diff --git a/helpers/make_color_map.py b/helpers/make_color_map.py deleted file mode 100755 index 0cad913..0000000 --- a/helpers/make_color_map.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python3 - -import argparse -from pathlib import Path -import numpy as np - -if __name__ == "__main__": - - parser = argparse.ArgumentParser(description='Make .cpp source file for a colormap') - - parser.add_argument('--prologue', type=Path, default="prologue.txt", - help='Test file containing boilerplate for the beginning of file') - parser.add_argument('--epilogue', type=Path, default='epilogue.txt', - help="Test file containing boilerplate for end of file") - - parser.add_argument('--csv', type=Path, nargs="*", - help='CSV file containing') - - args = parser.parse_args() - - if args.prologue: - with open(args.prologue) as fp: - for line in fp: - print(line.strip()) - - for csv_file in args.csv: - data = np.loadtxt(csv_file, delimiter=',', dtype=float) - - print("const float ColorMap::float_data[%d][3] = {" % len(data)) - for entry in data: - print("{%f,%f,%f}," % (entry[0],entry[1],entry[2])) - print("};") - print() - - print("const float ColorMap::char_data[%d][3] = {" % len(data)) - for entry in data: - print("{%d,%d,%d}," % (int(entry[0]*255), - int(entry[1]*255), - int(entry[2]*255))) - print("};") - print() - - - if args.epilogue: - with open(args.epilogue) as fp: - for line in fp: - print(line) diff --git a/helpers/prologue.txt b/helpers/prologue.txt deleted file mode 100644 index d6c0965..0000000 --- a/helpers/prologue.txt +++ /dev/null @@ -1,13 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// -// Color map data from -// https://github.com/BIDS/colormap/blob/master/colormaps.py -// -// As used in matplotlib -// As released under a CC0 license -// -// This file autogenerated with helpers/make_color_map.py - -#include "sonar_image_proc/ColorMaps.h" - -namespace sonar_image_proc { diff --git a/include/sonar_image_proc/AbstractSonarInterface.h b/include/sonar_image_proc/AbstractSonarInterface.h deleted file mode 100644 index a5eff34..0000000 --- a/include/sonar_image_proc/AbstractSonarInterface.h +++ /dev/null @@ -1,146 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#pragma once - -#include -#include - -#include -#include -#include - -namespace sonar_image_proc { - -typedef std::pair Bounds_t; -extern const Bounds_t UnsetBounds; - -struct AzimuthRangeIndices { - AzimuthRangeIndices(size_t a, size_t r) : _rangeIdx(r), _azimuthIdx(a) { ; } - - size_t range() const { return _rangeIdx; } - size_t azimuth() const { return _azimuthIdx; } - - size_t _rangeIdx, _azimuthIdx; -}; - -// Abstract class strictly for drawing sonar images -// Designed as a "common abstact type" between the Blueprint -// Subsea SimplePingResult and ROS ImagingSonarMsg -struct AbstractSonarInterface { - public: - AbstractSonarInterface(); - - enum DataType_t { - TYPE_NONE, - TYPE_UINT8, - TYPE_UINT16, - TYPE_UINT32, - TYPE_FLOAT32 - }; - - virtual DataType_t data_type() const = 0; - - // - // azimuths are in **radians** - // - virtual const std::vector &azimuths() const = 0; - - int nBearings() const { return azimuths().size(); } - __attribute__((deprecated)); - float bearing(int n) const { return azimuths().at(n); } - __attribute__((deprecated)); - - int nAzimuth() const { return azimuths().size(); } - int nAzimuths() const { - return azimuths().size(); - } // Whoops, should be consistent - float azimuth(int n) const { return azimuths().at(n); } - - Bounds_t azimuthBounds() const; - - float minAzimuth() const { return azimuthBounds().first; } - float maxAzimuth() const { return azimuthBounds().second; } - - float minAzimuthTan() const { - checkAzimuthBounds(); - return _minAzimuthTan; - } - float maxAzimuthTan() const { - checkAzimuthBounds(); - return _maxAzimuthTan; - } - - // - // ranges are in **meters** - // - virtual const std::vector &ranges() const = 0; - - int nRanges() const { return ranges().size(); } - float range(int n) const { return ranges().at(n); } - - Bounds_t rangeBounds() const; - - float minRange() const { return rangeBounds().first; } - float maxRange() const { return rangeBounds().second; } - - float maxRangeSquared() const { - checkRangeBounds(); - return _maxRangeSquared; - } - - // - // Rather than dive into template magic, it's up to the - // end user to understand the underlying data type - // stored in a sonar data format. - // - // The "native" format is float ... so intensity_float() - // is the only pure virtual. The default implementations of - // _unit8 and _uint16 calculate from the float; but - // depending on the original data format, it may be more efficient - // to override these implementations - // - virtual float intensity_float(const AzimuthRangeIndices &idx) const = 0; - - virtual uint8_t intensity_uint8(const AzimuthRangeIndices &idx) const { - return INT8_MAX * intensity_float(idx); - } - - virtual uint16_t intensity_uint16(const AzimuthRangeIndices &idx) const { - return INT16_MAX * intensity_float(idx); - } - - virtual uint32_t intensity_uint32(const AzimuthRangeIndices &idx) const { - return INT32_MAX * intensity_float(idx); - } - - // Trivial wrappers. These will be deprecated eventually - float intensity_float(size_t a, size_t r) const { - return intensity_float(AzimuthRangeIndices(a, r)); - } - __attribute__((deprecated)); - uint8_t intensity_uint8(size_t a, size_t r) const { - return intensity_uint8(AzimuthRangeIndices(a, r)); - } - __attribute__((deprecated)); - uint16_t intensity_uint16(size_t a, size_t r) const { - return intensity_uint16(AzimuthRangeIndices(a, r)); - } - __attribute__((deprecated)); - - private: - // In a few cases, need to "check and potentially calculate cached - // value" without actually getting the value - void checkRangeBounds() const; - void checkAzimuthBounds() const; - - private: - // Since we search extensively for the bounds - // (rather than assuming the first and last are the bounds), - // cache the results - mutable Bounds_t _rangeBounds, _azimuthBounds; - mutable float _minAzimuthTan, _maxAzimuthTan; - mutable float _maxRangeSquared; -}; - -} // namespace sonar_image_proc diff --git a/include/sonar_image_proc/ColorMaps.h b/include/sonar_image_proc/ColorMaps.h deleted file mode 100644 index f098dba..0000000 --- a/include/sonar_image_proc/ColorMaps.h +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#pragma once - -#include - -#include "sonar_image_proc/AbstractSonarInterface.h" - -namespace sonar_image_proc { - -using sonar_image_proc::AbstractSonarInterface; - -struct SonarColorMap { - // A ColorMap is a mapping from one pixel of sonar data to - // one pixel in the image. Each virtual function in - // this class corresponds to one possible OpenCV pixel format. - // - // By default "lookup_cv32fc1()" is the only operation - // implemented in this version. It returns the sonar pixel - // as a float (in the interval [0,1]) - // which OpenCV will interpret as a greyscale. - // - // The other virtuals are naive wrappers which convert that float - // to other OpenCV formats ... though still representing the data - // as a greyscale. - // - // Inherited classes can override any/all of the classes to - // cover other input/output combinations. - - // The "native" operation returns a single greyscale values a as float - virtual float lookup_cv32fc1(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const { - return ping.intensity_float(loc); - } - - virtual cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const { - const auto f = lookup_cv32fc1(ping, loc); - return cv::Vec3b(f * 255, f * 255, f * 255); - } - - virtual cv::Vec3f lookup_cv32fc3(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const { - const auto f = lookup_cv32fc1(ping, loc); - return cv::Vec3f(f, f, f); - } -}; - -//=== Implementations of specific color maps === -struct MitchellColorMap : public SonarColorMap { - cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const override { - const auto i = ping.intensity_float(loc); - return cv::Vec3b(1 - i, i, i); - } -}; - -// Use color map from -// https://github.com/BIDS/colormap/blob/master/colormaps.py -// -// As used in matplotlib -// As released under a CC0 license -struct InfernoColorMap : public SonarColorMap { - static const float _inferno_data_float[][3]; - static const float _inferno_data_uint8[][3]; - - // Minor optimization ... don't go through the intermediate Scalar - cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const override { - const auto i = ping.intensity_uint8(loc); - return cv::Vec3b(_inferno_data_uint8[i][0], _inferno_data_uint8[i][1], - _inferno_data_uint8[i][2]); - } - - cv::Vec3f lookup_cv32fc3(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const override { - const auto i = ping.intensity_uint8(loc); - return cv::Vec3f(_inferno_data_float[i][0], _inferno_data_float[i][1], - _inferno_data_float[i][2]); - } -}; - -struct InfernoSaturationColorMap : public InfernoColorMap { - // Minor optimization ... don't go through the intermediate Scalar - cv::Vec3b lookup_cv8uc3(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const override { - const auto i = ping.intensity_uint8(loc); - if (i == 255) { - return cv::Vec3b(0, 255, 0); - } else { - return cv::Vec3b(_inferno_data_uint8[i][0], _inferno_data_uint8[i][1], - _inferno_data_uint8[i][2]); - } - } - - cv::Vec3f lookup_cv32fc3(const AbstractSonarInterface &ping, - const AzimuthRangeIndices &loc) const override { - const auto i = ping.intensity_uint8(loc); - if (i == 255) { - return cv::Vec3f(0.0, 1.0, 0.0); - } else { - return cv::Vec3f(_inferno_data_float[i][0], _inferno_data_float[i][1], - _inferno_data_float[i][2]); - } - } -}; - -} // namespace sonar_image_proc diff --git a/include/sonar_image_proc/DataStructures.h b/include/sonar_image_proc/DataStructures.h deleted file mode 100644 index 36fc720..0000000 --- a/include/sonar_image_proc/DataStructures.h +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#pragma once - -#include -#include - -#include - -namespace sonar_image_proc { - -// \todo. Are these used anymore? -struct SonarPoint { - SonarPoint(float _x, float _z) : x(_x), z(_z) { ; } - float x; - float z; -}; - -SonarPoint bearingRange2Cartesian(float bearing, float range); - -} // namespace sonar_image_proc diff --git a/include/sonar_image_proc/DrawSonar.h b/include/sonar_image_proc/DrawSonar.h deleted file mode 100644 index 74e1538..0000000 --- a/include/sonar_image_proc/DrawSonar.h +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// -// This file contains the "functional" API. These are really just -// thin wrappers around a single-use instance of SonarDrawer -// -// See SonarDrawer.h for a class-based API (which is more efficients) -// as it can store and reuse intermediate results - -#pragma once - -#include -#include - -#include "sonar_image_proc/AbstractSonarInterface.h" -#include "sonar_image_proc/ColorMaps.h" -#include "sonar_image_proc/SonarDrawer.h" - -namespace sonar_image_proc { - -inline cv::Mat drawSonar(const sonar_image_proc::AbstractSonarInterface &ping, - const SonarColorMap &colorMap = InfernoColorMap(), - const cv::Mat &image = cv::Mat(0, 0, CV_8UC3)) { - SonarDrawer drawer; - cv::Mat rectImage = drawer.drawRectSonarImage(ping, colorMap, image); - return drawer.remapRectSonarImage(ping, rectImage); -} - -// Maps the sonar ping to an RGB image. -// rectImage is reshaped to be numRanges rows x numBearings columns -// -// If rectImage is either 8UC3 or 32FC3, it retains that type, otherwise -// rectImage is converted to 8UC3 -// -// Cell (0,0) is the color mapping of the data with the smallest range and -// smallest (typically, most negative) bearing in the ping. -// -// Cell (nRange,0) is the data at the max range, most negative bearing -// -// Cell (nRange,nBearing) is the data at the max range, most positive bearing -// -inline cv::Mat drawSonarRectImage( - const sonar_image_proc::AbstractSonarInterface &ping, - const SonarColorMap &colorMap = InfernoColorMap(), - const cv::Mat &rectImage = cv::Mat(0, 0, CV_8UC3)) { - SonarDrawer drawer; - return drawer.drawRectSonarImage(ping, colorMap, rectImage); -} - -namespace old_api { - -// === Old / Legacy API below === - -// Given an sonar image, calculates the bounding rectangle required to -// draw it. Assumes zero range (the point of the fan) occurs on the -// bottom edge of the image. -// Azimuth = 0 is straight ahead from the sonar (the vertical axis in the image) -// Assuming azimuth = 0 is within the interval of [min azimuth, max azimuth] -// then the Image height is pixPerRangeBin * (max range in bin / range -// resolution) Image width is determined by the triangles defined by image -// height and min/max azimuth -// -// If set, maxRange is used in lieu of the ping's native max range, -// allowing truncation of the image -cv::Size calculateImageSize( - const sonar_image_proc::AbstractSonarInterface &ping, cv::Size hint, - int pixPerRangeBin = 2, float maxRange = -1.0); - -cv::Mat drawSonar(const sonar_image_proc::AbstractSonarInterface &ping, - cv::Mat &mat, - const SonarColorMap &colorMap = InfernoColorMap(), - float maxRange = -1.0); - -} // namespace old_api - -} // namespace sonar_image_proc diff --git a/include/sonar_image_proc/HistogramGenerator.h b/include/sonar_image_proc/HistogramGenerator.h deleted file mode 100644 index 1bbcb47..0000000 --- a/include/sonar_image_proc/HistogramGenerator.h +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#pragma once - -#include "sonar_image_proc/AbstractSonarInterface.h" - -namespace sonar_image_proc { - -class HistogramGenerator { - public: - static std::vector Generate(const AbstractSonarInterface &ping); - - static std::vector GenerateUint8( - const AbstractSonarInterface &ping); - static std::vector GenerateUint16( - const AbstractSonarInterface &ping); - - // Histogram generator for 32bit data produces 256 bins of log10(intensity) - static std::vector GenerateUint32( - const AbstractSonarInterface &ping); -}; - -} // namespace sonar_image_proc diff --git a/include/sonar_image_proc/OverlayImage.h b/include/sonar_image_proc/OverlayImage.h deleted file mode 100644 index 14df320..0000000 --- a/include/sonar_image_proc/OverlayImage.h +++ /dev/null @@ -1,114 +0,0 @@ -#include -#include - -namespace sonar_image_proc { - -using cv::Mat; -using cv::Vec; - -// Adapted from the code sample in the OpenCV documentation: -// https://docs.opencv.org/4.x/d3/d63/classcv_1_1Mat.html#a33ee3bc402827f587a5ad64b568d6986 -// -// \todo(@amarburg): Optimize? Loop unrolling? -// - -template -void overlayImage(const Mat &bg, const Mat &fg, Mat &dst) { - typedef Vec VB; - typedef Vec VF; - - const float alpha_scale = (float)std::numeric_limits::max(), - inv_scale = 1.f / alpha_scale; - -#if (CV_VERSION_MAJOR >= 4) - CV_Assert(bg.type() == cv::traits::Type::value && - fg.type() == cv::traits::Type::value && bg.size() == fg.size()); -#else - CV_Assert(bg.type() == cv::DataType::type && - fg.type() == cv::DataType::type && bg.size() == fg.size()); -#endif - - dst.create(bg.size(), bg.type()); - - cv::MatConstIterator_ fit = fg.begin(), fit_end = fg.end(); - cv::MatConstIterator_ bit = bg.begin(); - cv::MatIterator_ dst_it = dst.begin(); - - for (; fit != fit_end; ++fit, ++bit, ++dst_it) { - const auto fg_pix = *fit; - const auto bg_pix = *bit; - - const float alpha = fg_pix[3] * inv_scale; - const float beta = 1 - alpha; - *dst_it = VB(cv::saturate_cast(fg_pix[0] * alpha + bg_pix[0] * beta), - cv::saturate_cast(fg_pix[1] * alpha + bg_pix[1] * beta), - cv::saturate_cast(fg_pix[2] * alpha + bg_pix[2] * beta)); - } -} - -// void overlayImage(const cv::Mat &background, const cv::Mat &foreground, -// cv::Mat &output) { -// background.copyTo(output); - -// // Only works with CV_8UC3 for background and -// // CV_8UC4 for foreground -// CV_Assert(bg.type() == traits::Type::value && -// bg.size() == fg.size()); - -// MatConstIterator_ bit = background.begin(), -// bit_end = background.end(); -// MatConstIterator_ fit = foreground.begin(); - -// MatIterator_ oit = dst.begin(); - -// for (; bit != bit_end; ++bit, ++fit, ++oit) { - -// fg_pix = *fit, bg_pix = *bit; -// float alpha = fg_pix[3] * inv_scale, beta = bg_pix[3] * inv_scale; -// *dst_it = -// VT(saturate_cast(fg_pix[0] * alpha + bg_pix[0] * beta), -// saturate_cast(fg_pix[1] * alpha + bg_pix[1] * beta), -// saturate_cast(fg_pix[2] * alpha + bg_pix[2] * beta), -// saturate_cast((1 - (1 - alpha) * (1 - beta)) * alpha_scale)); -// } - -// // // start at the row indicated by location, or at row 0 if location.y -// is -// // // negative. -// // for (int fY = 0; (fY < background.rows) && (fY < foreground.rows); -// ++fY) -// // { - -// // // start at the column indicated by location, - -// // // or at column 0 if location.x is negative. -// // for (int fX = 0; (fX < background.cols) && (fX < foreground.cols); -// // ++fX) { - -// // // determine the opacity of the foregrond pixel, using its fourth -// // (alpha) -// // // channel. -// // float alpha = foreground.at(fY, fX, 3) / 255.; -// // // ((double)foreground -// // // .data[fY * foreground.step + fX * foreground.channels() -// + -// // 3]) / -// // // 255.; - -// // // and now combine the background and foreground pixel, using the -// // // opacity, - -// // // but only if opacity > 0. -// // for (int c = 0; alpha > 0 && c < output.channels(); ++c) { -// // const unsigned char foregroundPx = -// // foreground.at(fY, fX, c); -// // const unsigned char backgroundPx = -// // background.at(fY, fX, c); - -// // output.at(fY, fX, c) = -// // backgroundPx * (1. - alpha) + foregroundPx * alpha; -// // } -// // } -// // } -// } -}; // namespace sonar_image_proc diff --git a/include/sonar_image_proc/SonarDrawer.h b/include/sonar_image_proc/SonarDrawer.h deleted file mode 100644 index 63b4a93..0000000 --- a/include/sonar_image_proc/SonarDrawer.h +++ /dev/null @@ -1,184 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// -// This file contains the class-based API (which is more efficient) -// as it can store and reuse intermediate results. -// -// See "DrawSonar.h" for the function-based API - -#pragma once - -#include -#include - -#include "sonar_image_proc/AbstractSonarInterface.h" -#include "sonar_image_proc/ColorMaps.h" - -namespace sonar_image_proc { - -using sonar_image_proc::AbstractSonarInterface; - -// Function in lib/OverlayImage.cpp -void overlayImage(const cv::Mat &background, const cv::Mat &foreground, - cv::Mat &output); - -#define OVERLAY_RW(var, tp, set, get) \ - OverlayConfig &set(tp i) { \ - var = i; \ - return *this; \ - } \ - tp get() const { return var; } - -class SonarDrawer { - public: - struct OverlayConfig { - public: - int DEFAULT_LINE_THICKNESS = 1; - float DEFAULT_LINE_ALPHA = 0.5; - - // range spacing of 0 means "calculate automatically" - float DEFAULT_RANGE_SPACING = 0; - - float DEFAULT_RADIAL_SPACING = 20; // degrees - bool DEFAULT_RADIAL_AT_ZERO = false; - - float DEFAULT_FONT_SCALE = 1.0; - - OverlayConfig() - : line_thickness_(DEFAULT_LINE_THICKNESS), - line_alpha_(DEFAULT_LINE_ALPHA), - range_spacing_m_(DEFAULT_RANGE_SPACING), - radial_spacing_deg_(DEFAULT_RADIAL_SPACING), - radial_at_zero_(DEFAULT_RADIAL_AT_ZERO), - font_scale_(DEFAULT_FONT_SCALE), - line_color_(255, 255, 255) {} - - OVERLAY_RW(line_alpha_, float, setLineAlpha, lineAlpha) - OVERLAY_RW(line_thickness_, int, setLineThickness, lineThickness) - OVERLAY_RW(range_spacing_m_, float, setRangeSpacing, rangeSpacing) - - OVERLAY_RW(radial_spacing_deg_, float, setRadialSpacing, radialSpacing) - OVERLAY_RW(radial_at_zero_, bool, setRadialAtZero, radialAtZero) - OVERLAY_RW(font_scale_, float, setFontScale, fontScale) - - OverlayConfig &setLineColor(const cv::Vec3b &v) { - line_color_ = v; - return *this; - } - cv::Vec3b lineColor() const { return line_color_; } - - bool operator!=(const OverlayConfig &other) const { - return (lineThickness() != other.lineThickness()) || - (lineAlpha() != other.lineAlpha()) || - (rangeSpacing() != other.rangeSpacing()) || - (radialSpacing() != other.radialSpacing()) || - (radialAtZero() != other.radialAtZero()) || - (fontScale() != other.fontScale()) || - (lineColor() != other.lineColor()); - } - - private: - int line_thickness_; - float line_alpha_; - float range_spacing_m_; - float radial_spacing_deg_; - bool radial_at_zero_; - float font_scale_; - - cv::Vec3b line_color_; - }; - - SonarDrawer(); - - // Calls drawRectSonarImage followed by remapRectSonarImage inline - // The intermediate rectangular image is not returned, if required, - // use the two functions individually... - cv::Mat drawSonar(const AbstractSonarInterface &ping, - const SonarColorMap &colorMap = InfernoColorMap(), - const cv::Mat &image = cv::Mat(0, 0, CV_8UC3), - bool addOverlay = false); - - // Maps the sonar ping to an RGB image. - // rectImage is reshaped to be numRanges rows x numBearings columns - // - // If rectImage is either 8UC3 or 32FC3, it retains that type, otherwise - // rectImage is converted to 8UC3 - // - // Cell (0,0) is the color mapping of the data with the smallest range and - // smallest (typically, most negative) bearing in the ping. - // - // Cell (nRange,0) is the data at the max range, most negative bearing - // - // Cell (nRange,nBearing) is the data at the max range, most positive - // bearing - // - cv::Mat drawRectSonarImage(const AbstractSonarInterface &ping, - const SonarColorMap &colorMap = InfernoColorMap(), - const cv::Mat &rectImage = cv::Mat(0, 0, CV_8UC3)); - - cv::Mat remapRectSonarImage(const AbstractSonarInterface &ping, - const cv::Mat &rectImage); - - // Creates a copy of sonarImage with the graphical overlay using the - // configuration in overlayConfig - cv::Mat drawOverlay(const AbstractSonarInterface &ping, - const cv::Mat &sonarImage); - - OverlayConfig &overlayConfig() { return overlay_config_; } - - private: - OverlayConfig overlay_config_; - - // Utility class which can generate and store the two cv::Mats - // required for the cv::remap() function - // - // Also stores meta-information to determine if the map is - // invalid and needs to be regenerated. - struct Cached { - public: - Cached() { ; } - - protected: - virtual bool isValid(const AbstractSonarInterface &ping) const; - - // Meta-information to validate map - std::pair _rangeBounds, _azimuthBounds; - int _numRanges, _numAzimuth; - }; - - struct CachedMap : public Cached { - public: - CachedMap() : Cached() { ; } - typedef std::pair MapPair; - - MapPair operator()(const AbstractSonarInterface &ping); - - private: - bool isValid(const AbstractSonarInterface &ping) const override; - void create(const AbstractSonarInterface &ping); - - cv::Mat _scMap1, _scMap2; - } _map; - - struct CachedOverlay : public Cached { - public: - CachedOverlay() : Cached() { ; } - - const cv::Mat &operator()(const AbstractSonarInterface &ping, - const cv::Mat &sonarImage, - const OverlayConfig &config); - - private: - bool isValid(const AbstractSonarInterface &ping, const cv::Mat &sonarImage, - const OverlayConfig &config) const; - - void create(const AbstractSonarInterface &ping, const cv::Mat &sonarImage, - const OverlayConfig &config); - - cv::Mat _overlay; - OverlayConfig _config_used; - - } _overlay; - -}; // namespace sonar_image_procclassSonarDrawer - -} // namespace sonar_image_proc diff --git a/ros/include/sonar_image_proc/sonar_image_msg_interface.h b/include/sonar_image_proc/sonar_image_msg_interface.h similarity index 100% rename from ros/include/sonar_image_proc/sonar_image_msg_interface.h rename to include/sonar_image_proc/sonar_image_msg_interface.h diff --git a/lib/AbstractSonarInterface.cpp b/lib/AbstractSonarInterface.cpp deleted file mode 100644 index 9c72024..0000000 --- a/lib/AbstractSonarInterface.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#include "sonar_image_proc/AbstractSonarInterface.h" - -#include -#include - -namespace sonar_image_proc { - -const Bounds_t UnsetBounds = Bounds_t(-1, -1); - -AbstractSonarInterface::AbstractSonarInterface() - : _rangeBounds(UnsetBounds), _azimuthBounds(UnsetBounds) {} - -Bounds_t AbstractSonarInterface::azimuthBounds() const { - checkAzimuthBounds(); - return _azimuthBounds; -} - -Bounds_t AbstractSonarInterface::rangeBounds() const { - checkRangeBounds(); - return _rangeBounds; -} - -void AbstractSonarInterface::checkRangeBounds() const { - if (_rangeBounds == UnsetBounds) { - auto results = std::minmax_element(ranges().begin(), ranges().end()); - _rangeBounds = std::make_pair(*(results.first), *(results.second)); - - _maxRangeSquared = _rangeBounds.second * _rangeBounds.second; - } -} - -void AbstractSonarInterface::checkAzimuthBounds() const { - if (_azimuthBounds == UnsetBounds) { - auto results = std::minmax_element(azimuths().begin(), azimuths().end()); - _azimuthBounds = std::make_pair(*(results.first), *(results.second)); - - _minAzimuthTan = std::tan(_azimuthBounds.first); - _maxAzimuthTan = std::tan(_azimuthBounds.second); - } -} - -} // namespace sonar_image_proc diff --git a/lib/ColorMaps.cpp b/lib/ColorMaps.cpp deleted file mode 100644 index b7be98a..0000000 --- a/lib/ColorMaps.cpp +++ /dev/null @@ -1,213 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// -// Color map data from -// https://github.com/BIDS/colormap/blob/master/colormaps.py -// -// As used in matplotlib -// As released under a CC0 license -// -// This file autogenerated with helpers/make_color_map.py - -#include "sonar_image_proc/ColorMaps.h" - -namespace sonar_image_proc { - -const float InfernoColorMap::_inferno_data_float[256][3] = { - {0.001462, 0.000466, 0.013866}, {0.002267, 0.001270, 0.018570}, - {0.003299, 0.002249, 0.024239}, {0.004547, 0.003392, 0.030909}, - {0.006006, 0.004692, 0.038558}, {0.007676, 0.006136, 0.046836}, - {0.009561, 0.007713, 0.055143}, {0.011663, 0.009417, 0.063460}, - {0.013995, 0.011225, 0.071862}, {0.016561, 0.013136, 0.080282}, - {0.019373, 0.015133, 0.088767}, {0.022447, 0.017199, 0.097327}, - {0.025793, 0.019331, 0.105930}, {0.029432, 0.021503, 0.114621}, - {0.033385, 0.023702, 0.123397}, {0.037668, 0.025921, 0.132232}, - {0.042253, 0.028139, 0.141141}, {0.046915, 0.030324, 0.150164}, - {0.051644, 0.032474, 0.159254}, {0.056449, 0.034569, 0.168414}, - {0.061340, 0.036590, 0.177642}, {0.066331, 0.038504, 0.186962}, - {0.071429, 0.040294, 0.196354}, {0.076637, 0.041905, 0.205799}, - {0.081962, 0.043328, 0.215289}, {0.087411, 0.044556, 0.224813}, - {0.092990, 0.045583, 0.234358}, {0.098702, 0.046402, 0.243904}, - {0.104551, 0.047008, 0.253430}, {0.110536, 0.047399, 0.262912}, - {0.116656, 0.047574, 0.272321}, {0.122908, 0.047536, 0.281624}, - {0.129285, 0.047293, 0.290788}, {0.135778, 0.046856, 0.299776}, - {0.142378, 0.046242, 0.308553}, {0.149073, 0.045468, 0.317085}, - {0.155850, 0.044559, 0.325338}, {0.162689, 0.043554, 0.333277}, - {0.169575, 0.042489, 0.340874}, {0.176493, 0.041402, 0.348111}, - {0.183429, 0.040329, 0.354971}, {0.190367, 0.039309, 0.361447}, - {0.197297, 0.038400, 0.367535}, {0.204209, 0.037632, 0.373238}, - {0.211095, 0.037030, 0.378563}, {0.217949, 0.036615, 0.383522}, - {0.224763, 0.036405, 0.388129}, {0.231538, 0.036405, 0.392400}, - {0.238273, 0.036621, 0.396353}, {0.244967, 0.037055, 0.400007}, - {0.251620, 0.037705, 0.403378}, {0.258234, 0.038571, 0.406485}, - {0.264810, 0.039647, 0.409345}, {0.271347, 0.040922, 0.411976}, - {0.277850, 0.042353, 0.414392}, {0.284321, 0.043933, 0.416608}, - {0.290763, 0.045644, 0.418637}, {0.297178, 0.047470, 0.420491}, - {0.303568, 0.049396, 0.422182}, {0.309935, 0.051407, 0.423721}, - {0.316282, 0.053490, 0.425116}, {0.322610, 0.055634, 0.426377}, - {0.328921, 0.057827, 0.427511}, {0.335217, 0.060060, 0.428524}, - {0.341500, 0.062325, 0.429425}, {0.347771, 0.064616, 0.430217}, - {0.354032, 0.066925, 0.430906}, {0.360284, 0.069247, 0.431497}, - {0.366529, 0.071579, 0.431994}, {0.372768, 0.073915, 0.432400}, - {0.379001, 0.076253, 0.432719}, {0.385228, 0.078591, 0.432955}, - {0.391453, 0.080927, 0.433109}, {0.397674, 0.083257, 0.433183}, - {0.403894, 0.085580, 0.433179}, {0.410113, 0.087896, 0.433098}, - {0.416331, 0.090203, 0.432943}, {0.422549, 0.092501, 0.432714}, - {0.428768, 0.094790, 0.432412}, {0.434987, 0.097069, 0.432039}, - {0.441207, 0.099338, 0.431594}, {0.447428, 0.101597, 0.431080}, - {0.453651, 0.103848, 0.430498}, {0.459875, 0.106089, 0.429846}, - {0.466100, 0.108322, 0.429125}, {0.472328, 0.110547, 0.428334}, - {0.478558, 0.112764, 0.427475}, {0.484789, 0.114974, 0.426548}, - {0.491022, 0.117179, 0.425552}, {0.497257, 0.119379, 0.424488}, - {0.503493, 0.121575, 0.423356}, {0.509730, 0.123769, 0.422156}, - {0.515967, 0.125960, 0.420887}, {0.522206, 0.128150, 0.419549}, - {0.528444, 0.130341, 0.418142}, {0.534683, 0.132534, 0.416667}, - {0.540920, 0.134729, 0.415123}, {0.547157, 0.136929, 0.413511}, - {0.553392, 0.139134, 0.411829}, {0.559624, 0.141346, 0.410078}, - {0.565854, 0.143567, 0.408258}, {0.572081, 0.145797, 0.406369}, - {0.578304, 0.148039, 0.404411}, {0.584521, 0.150294, 0.402385}, - {0.590734, 0.152563, 0.400290}, {0.596940, 0.154848, 0.398125}, - {0.603139, 0.157151, 0.395891}, {0.609330, 0.159474, 0.393589}, - {0.615513, 0.161817, 0.391219}, {0.621685, 0.164184, 0.388781}, - {0.627847, 0.166575, 0.386276}, {0.633998, 0.168992, 0.383704}, - {0.640135, 0.171438, 0.381065}, {0.646260, 0.173914, 0.378359}, - {0.652369, 0.176421, 0.375586}, {0.658463, 0.178962, 0.372748}, - {0.664540, 0.181539, 0.369846}, {0.670599, 0.184153, 0.366879}, - {0.676638, 0.186807, 0.363849}, {0.682656, 0.189501, 0.360757}, - {0.688653, 0.192239, 0.357603}, {0.694627, 0.195021, 0.354388}, - {0.700576, 0.197851, 0.351113}, {0.706500, 0.200728, 0.347777}, - {0.712396, 0.203656, 0.344383}, {0.718264, 0.206636, 0.340931}, - {0.724103, 0.209670, 0.337424}, {0.729909, 0.212759, 0.333861}, - {0.735683, 0.215906, 0.330245}, {0.741423, 0.219112, 0.326576}, - {0.747127, 0.222378, 0.322856}, {0.752794, 0.225706, 0.319085}, - {0.758422, 0.229097, 0.315266}, {0.764010, 0.232554, 0.311399}, - {0.769556, 0.236077, 0.307485}, {0.775059, 0.239667, 0.303526}, - {0.780517, 0.243327, 0.299523}, {0.785929, 0.247056, 0.295477}, - {0.791293, 0.250856, 0.291390}, {0.796607, 0.254728, 0.287264}, - {0.801871, 0.258674, 0.283099}, {0.807082, 0.262692, 0.278898}, - {0.812239, 0.266786, 0.274661}, {0.817341, 0.270954, 0.270390}, - {0.822386, 0.275197, 0.266085}, {0.827372, 0.279517, 0.261750}, - {0.832299, 0.283913, 0.257383}, {0.837165, 0.288385, 0.252988}, - {0.841969, 0.292933, 0.248564}, {0.846709, 0.297559, 0.244113}, - {0.851384, 0.302260, 0.239636}, {0.855992, 0.307038, 0.235133}, - {0.860533, 0.311892, 0.230606}, {0.865006, 0.316822, 0.226055}, - {0.869409, 0.321827, 0.221482}, {0.873741, 0.326906, 0.216886}, - {0.878001, 0.332060, 0.212268}, {0.882188, 0.337287, 0.207628}, - {0.886302, 0.342586, 0.202968}, {0.890341, 0.347957, 0.198286}, - {0.894305, 0.353399, 0.193584}, {0.898192, 0.358911, 0.188860}, - {0.902003, 0.364492, 0.184116}, {0.905735, 0.370140, 0.179350}, - {0.909390, 0.375856, 0.174563}, {0.912966, 0.381636, 0.169755}, - {0.916462, 0.387481, 0.164924}, {0.919879, 0.393389, 0.160070}, - {0.923215, 0.399359, 0.155193}, {0.926470, 0.405389, 0.150292}, - {0.929644, 0.411479, 0.145367}, {0.932737, 0.417627, 0.140417}, - {0.935747, 0.423831, 0.135440}, {0.938675, 0.430091, 0.130438}, - {0.941521, 0.436405, 0.125409}, {0.944285, 0.442772, 0.120354}, - {0.946965, 0.449191, 0.115272}, {0.949562, 0.455660, 0.110164}, - {0.952075, 0.462178, 0.105031}, {0.954506, 0.468744, 0.099874}, - {0.956852, 0.475356, 0.094695}, {0.959114, 0.482014, 0.089499}, - {0.961293, 0.488716, 0.084289}, {0.963387, 0.495462, 0.079073}, - {0.965397, 0.502249, 0.073859}, {0.967322, 0.509078, 0.068659}, - {0.969163, 0.515946, 0.063488}, {0.970919, 0.522853, 0.058367}, - {0.972590, 0.529798, 0.053324}, {0.974176, 0.536780, 0.048392}, - {0.975677, 0.543798, 0.043618}, {0.977092, 0.550850, 0.039050}, - {0.978422, 0.557937, 0.034931}, {0.979666, 0.565057, 0.031409}, - {0.980824, 0.572209, 0.028508}, {0.981895, 0.579392, 0.026250}, - {0.982881, 0.586606, 0.024661}, {0.983779, 0.593849, 0.023770}, - {0.984591, 0.601122, 0.023606}, {0.985315, 0.608422, 0.024202}, - {0.985952, 0.615750, 0.025592}, {0.986502, 0.623105, 0.027814}, - {0.986964, 0.630485, 0.030908}, {0.987337, 0.637890, 0.034916}, - {0.987622, 0.645320, 0.039886}, {0.987819, 0.652773, 0.045581}, - {0.987926, 0.660250, 0.051750}, {0.987945, 0.667748, 0.058329}, - {0.987874, 0.675267, 0.065257}, {0.987714, 0.682807, 0.072489}, - {0.987464, 0.690366, 0.079990}, {0.987124, 0.697944, 0.087731}, - {0.986694, 0.705540, 0.095694}, {0.986175, 0.713153, 0.103863}, - {0.985566, 0.720782, 0.112229}, {0.984865, 0.728427, 0.120785}, - {0.984075, 0.736087, 0.129527}, {0.983196, 0.743758, 0.138453}, - {0.982228, 0.751442, 0.147565}, {0.981173, 0.759135, 0.156863}, - {0.980032, 0.766837, 0.166353}, {0.978806, 0.774545, 0.176037}, - {0.977497, 0.782258, 0.185923}, {0.976108, 0.789974, 0.196018}, - {0.974638, 0.797692, 0.206332}, {0.973088, 0.805409, 0.216877}, - {0.971468, 0.813122, 0.227658}, {0.969783, 0.820825, 0.238686}, - {0.968041, 0.828515, 0.249972}, {0.966243, 0.836191, 0.261534}, - {0.964394, 0.843848, 0.273391}, {0.962517, 0.851476, 0.285546}, - {0.960626, 0.859069, 0.298010}, {0.958720, 0.866624, 0.310820}, - {0.956834, 0.874129, 0.323974}, {0.954997, 0.881569, 0.337475}, - {0.953215, 0.888942, 0.351369}, {0.951546, 0.896226, 0.365627}, - {0.950018, 0.903409, 0.380271}, {0.948683, 0.910473, 0.395289}, - {0.947594, 0.917399, 0.410665}, {0.946809, 0.924168, 0.426373}, - {0.946392, 0.930761, 0.442367}, {0.946403, 0.937159, 0.458592}, - {0.946903, 0.943348, 0.474970}, {0.947937, 0.949318, 0.491426}, - {0.949545, 0.955063, 0.507860}, {0.951740, 0.960587, 0.524203}, - {0.954529, 0.965896, 0.540361}, {0.957896, 0.971003, 0.556275}, - {0.961812, 0.975924, 0.571925}, {0.966249, 0.980678, 0.587206}, - {0.971162, 0.985282, 0.602154}, {0.976511, 0.989753, 0.616760}, - {0.982257, 0.994109, 0.631017}, {0.988362, 0.998364, 0.644924}, -}; - -const float InfernoColorMap::_inferno_data_uint8[256][3] = { - {0, 0, 3}, {0, 0, 4}, {0, 0, 6}, {1, 0, 7}, - {1, 1, 9}, {1, 1, 11}, {2, 1, 14}, {2, 2, 16}, - {3, 2, 18}, {4, 3, 20}, {4, 3, 22}, {5, 4, 24}, - {6, 4, 27}, {7, 5, 29}, {8, 6, 31}, {9, 6, 33}, - {10, 7, 35}, {11, 7, 38}, {13, 8, 40}, {14, 8, 42}, - {15, 9, 45}, {16, 9, 47}, {18, 10, 50}, {19, 10, 52}, - {20, 11, 54}, {22, 11, 57}, {23, 11, 59}, {25, 11, 62}, - {26, 11, 64}, {28, 12, 67}, {29, 12, 69}, {31, 12, 71}, - {32, 12, 74}, {34, 11, 76}, {36, 11, 78}, {38, 11, 80}, - {39, 11, 82}, {41, 11, 84}, {43, 10, 86}, {45, 10, 88}, - {46, 10, 90}, {48, 10, 92}, {50, 9, 93}, {52, 9, 95}, - {53, 9, 96}, {55, 9, 97}, {57, 9, 98}, {59, 9, 100}, - {60, 9, 101}, {62, 9, 102}, {64, 9, 102}, {65, 9, 103}, - {67, 10, 104}, {69, 10, 105}, {70, 10, 105}, {72, 11, 106}, - {74, 11, 106}, {75, 12, 107}, {77, 12, 107}, {79, 13, 108}, - {80, 13, 108}, {82, 14, 108}, {83, 14, 109}, {85, 15, 109}, - {87, 15, 109}, {88, 16, 109}, {90, 17, 109}, {91, 17, 110}, - {93, 18, 110}, {95, 18, 110}, {96, 19, 110}, {98, 20, 110}, - {99, 20, 110}, {101, 21, 110}, {102, 21, 110}, {104, 22, 110}, - {106, 23, 110}, {107, 23, 110}, {109, 24, 110}, {110, 24, 110}, - {112, 25, 110}, {114, 25, 109}, {115, 26, 109}, {117, 27, 109}, - {118, 27, 109}, {120, 28, 109}, {122, 28, 109}, {123, 29, 108}, - {125, 29, 108}, {126, 30, 108}, {128, 31, 107}, {129, 31, 107}, - {131, 32, 107}, {133, 32, 106}, {134, 33, 106}, {136, 33, 106}, - {137, 34, 105}, {139, 34, 105}, {141, 35, 105}, {142, 36, 104}, - {144, 36, 104}, {145, 37, 103}, {147, 37, 103}, {149, 38, 102}, - {150, 38, 102}, {152, 39, 101}, {153, 40, 100}, {155, 40, 100}, - {156, 41, 99}, {158, 41, 99}, {160, 42, 98}, {161, 43, 97}, - {163, 43, 97}, {164, 44, 96}, {166, 44, 95}, {167, 45, 95}, - {169, 46, 94}, {171, 46, 93}, {172, 47, 92}, {174, 48, 91}, - {175, 49, 91}, {177, 49, 90}, {178, 50, 89}, {180, 51, 88}, - {181, 51, 87}, {183, 52, 86}, {184, 53, 86}, {186, 54, 85}, - {187, 55, 84}, {189, 55, 83}, {190, 56, 82}, {191, 57, 81}, - {193, 58, 80}, {194, 59, 79}, {196, 60, 78}, {197, 61, 77}, - {199, 62, 76}, {200, 62, 75}, {201, 63, 74}, {203, 64, 73}, - {204, 65, 72}, {205, 66, 71}, {207, 68, 70}, {208, 69, 68}, - {209, 70, 67}, {210, 71, 66}, {212, 72, 65}, {213, 73, 64}, - {214, 74, 63}, {215, 75, 62}, {217, 77, 61}, {218, 78, 59}, - {219, 79, 58}, {220, 80, 57}, {221, 82, 56}, {222, 83, 55}, - {223, 84, 54}, {224, 86, 52}, {226, 87, 51}, {227, 88, 50}, - {228, 90, 49}, {229, 91, 48}, {230, 92, 46}, {230, 94, 45}, - {231, 95, 44}, {232, 97, 43}, {233, 98, 42}, {234, 100, 40}, - {235, 101, 39}, {236, 103, 38}, {237, 104, 37}, {237, 106, 35}, - {238, 108, 34}, {239, 109, 33}, {240, 111, 31}, {240, 112, 30}, - {241, 114, 29}, {242, 116, 28}, {242, 117, 26}, {243, 119, 25}, - {243, 121, 24}, {244, 122, 22}, {245, 124, 21}, {245, 126, 20}, - {246, 128, 18}, {246, 129, 17}, {247, 131, 16}, {247, 133, 14}, - {248, 135, 13}, {248, 136, 12}, {248, 138, 11}, {249, 140, 9}, - {249, 142, 8}, {249, 144, 8}, {250, 145, 7}, {250, 147, 6}, - {250, 149, 6}, {250, 151, 6}, {251, 153, 6}, {251, 155, 6}, - {251, 157, 6}, {251, 158, 7}, {251, 160, 7}, {251, 162, 8}, - {251, 164, 10}, {251, 166, 11}, {251, 168, 13}, {251, 170, 14}, - {251, 172, 16}, {251, 174, 18}, {251, 176, 20}, {251, 177, 22}, - {251, 179, 24}, {251, 181, 26}, {251, 183, 28}, {251, 185, 30}, - {250, 187, 33}, {250, 189, 35}, {250, 191, 37}, {250, 193, 40}, - {249, 195, 42}, {249, 197, 44}, {249, 199, 47}, {248, 201, 49}, - {248, 203, 52}, {248, 205, 55}, {247, 207, 58}, {247, 209, 60}, - {246, 211, 63}, {246, 213, 66}, {245, 215, 69}, {245, 217, 72}, - {244, 219, 75}, {244, 220, 79}, {243, 222, 82}, {243, 224, 86}, - {243, 226, 89}, {242, 228, 93}, {242, 230, 96}, {241, 232, 100}, - {241, 233, 104}, {241, 235, 108}, {241, 237, 112}, {241, 238, 116}, - {241, 240, 121}, {241, 242, 125}, {242, 243, 129}, {242, 244, 133}, - {243, 246, 137}, {244, 247, 141}, {245, 248, 145}, {246, 250, 149}, - {247, 251, 153}, {249, 252, 157}, {250, 253, 160}, {252, 254, 164}, -}; - -} // namespace sonar_image_proc diff --git a/lib/HistogramGenerator.cpp b/lib/HistogramGenerator.cpp deleted file mode 100644 index a82e9aa..0000000 --- a/lib/HistogramGenerator.cpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#include "sonar_image_proc/HistogramGenerator.h" - -#include - -namespace sonar_image_proc { - -std::vector HistogramGenerator::Generate( - const AbstractSonarInterface &ping) { - if (ping.data_type() == AbstractSonarInterface::TYPE_UINT8) - return HistogramGenerator::GenerateUint8(ping); - else if (ping.data_type() == AbstractSonarInterface::TYPE_UINT16) - return HistogramGenerator::GenerateUint16(ping); - else if (ping.data_type() == AbstractSonarInterface::TYPE_UINT32) - return HistogramGenerator::GenerateUint32(ping); - - return std::vector(); -} - -// \todo Some repetition, but these functions are pretty simple -std::vector HistogramGenerator::GenerateUint8( - const AbstractSonarInterface &ping) { - std::vector result(256, 0); - - for (int r = 0; r < ping.nRanges(); r++) { - for (int b = 0; b < ping.nBearings(); b++) { - const auto val = ping.intensity_uint8(AzimuthRangeIndices(b, r)); - result[val]++; - } - } - - return result; -} - -std::vector HistogramGenerator::GenerateUint16( - const AbstractSonarInterface &ping) { - std::vector result(65536, 0); - - for (int r = 0; r < ping.nRanges(); r++) { - for (int b = 0; b < ping.nBearings(); b++) { - const auto val = ping.intensity_uint16(AzimuthRangeIndices(b, r)); - - result[val]++; - } - } - - return result; -} - -std::vector HistogramGenerator::GenerateUint32( - const AbstractSonarInterface &ping) { - std::vector result(256, 0); - - const float logMax = log10(UINT32_MAX); - - for (int r = 0; r < ping.nRanges(); r++) { - for (int b = 0; b < ping.nBearings(); b++) { - const auto val = ping.intensity_uint32(AzimuthRangeIndices(b, r)); - - if (val == 0) continue; - - const float l = log10(val); - - const size_t idx = UINT8_MAX * (l / logMax); - - // std::cerr << "val = " << val << "; l = " << l << "; idx = " << idx << - // std::endl; - - if ((idx < 0) || (idx > result.size())) continue; - result[idx]++; - } - } - - return result; -} - -} // namespace sonar_image_proc diff --git a/lib/OldDrawSonar.cpp b/lib/OldDrawSonar.cpp deleted file mode 100644 index 4cc018b..0000000 --- a/lib/OldDrawSonar.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// -// Contains the old "legacy" (that is, noticably worse) implementation - -#include -#include -#include - -#include "sonar_image_proc/DrawSonar.h" - -#ifndef THETA_SHIFT -#define THETA_SHIFT PI; -#endif - -namespace sonar_image_proc { - -namespace old_api { - -using cv::Mat; -using cv::Size; -using std::vector; - -const float ThetaShift = 1.5 * M_PI; - -cv::Size calculateImageSize(const AbstractSonarInterface &ping, cv::Size hint, - int pixPerRangeBin, float maxRange) { - int h = hint.height, w = hint.width; - - if (w <= 0) { - if (h <= 0) { - const float rangeMax = ((maxRange > 0.0) ? maxRange : ping.maxRange()); - const float rangeRes = - (ping.maxRange() - ping.minRange()) / ping.nRanges(); - - const int nEffectiveRanges = ceil(rangeMax / rangeRes); - - h = nEffectiveRanges * pixPerRangeBin; - } - - // Assume bearings are symmetric plus and minus - // Bearings must be radians - w = 2 * ceil(fabs(h * sin(ping.bearing(0)))); - } else if (h <= 0) { - h = (w / 2) / ceil(fabs(sin(ping.bearing(0)))); - } - - // Ensure w and h are both divisible by zero - if (w % 2) w++; - if (h % 2) h++; - - return Size(w, h); -} - -cv::Mat drawSonar(const AbstractSonarInterface &ping, const Mat &mat, - const SonarColorMap &colorMap, float maxRange) { - // Ensure mat is 8UC3; - cv::Mat out(mat); - out.create(mat.size(), CV_8UC3); - out.setTo(cv::Vec3b(0, 0, 0)); - - const int nRanges = ping.nRanges(); - const int nBeams = ping.nBearings(); - - const float rangeMax = (maxRange > 0.0 ? maxRange : ping.maxRange()); - - // Calculate effective range resolution of the output image. - // The sensor's original resolution - const float rangeRes = (ping.maxRange() - ping.minRange()) / ping.nRanges(); - - // How many ranges are required to go from 0 to rangeMax (since the - // sensor starts at some minimum) - const int nEffectiveRanges = ceil(rangeMax / rangeRes); - - // Todo. Calculate offset for non-zero minimum ranges - const unsigned int radius = mat.size().height; - // This effectively flips the origin, presumably so that it will appear - // at the bottom fo the image. - const cv::Point origin(mat.size().width / 2, mat.size().height); - - // QUESTION: Why the factor of 2? - // If I understand correctly, binThickness is the width - // of the range-bin, in pixels. - const float binThickness = 2 * ceil(radius / nEffectiveRanges); - - struct BearingEntry { - float begin, center, end; - - BearingEntry(float b, float c, float e) : begin(b), center(c), end(e) { ; } - }; - - vector angles; - angles.reserve(nBeams); - - for (int b = 0; b < nBeams; ++b) { - const float center = ping.bearing(b); - float begin = 0.0, end = 0.0; - - if (b == 0) { - end = (ping.bearing(b + 1) + center) / 2.0; - begin = 2 * center - end; - - } else if (b == nBeams - 1) { - begin = angles[b - 1].end; - end = 2 * center - begin; - - } else { - begin = angles[b - 1].end; - end = (ping.bearing(b + 1) + center) / 2.0; - } - - angles.push_back(BearingEntry(begin, center, end)); - } - - for (int r = 0; r < nRanges; ++r) { - if (ping.range(r) > rangeMax) continue; - - for (int b = 0; b < nBeams; ++b) { - const float range = ping.range(r); - - // QUESTION: Why are we rotating here? - const float begin = angles[b].begin + ThetaShift, - end = angles[b].end + ThetaShift; - - const float rad = static_cast(radius) * range / rangeMax; - - // Assume angles are in image frame x-right, y-down - cv::ellipse(out, origin, cv::Size(rad, rad), 0, begin * 180 / M_PI, - end * 180 / M_PI, - colorMap.lookup_cv8uc3( - ping, AzimuthRangeIndices(angles[b].center, range)), - binThickness); - } - } - - return out; -} - -} // namespace old_api -} // namespace sonar_image_proc diff --git a/lib/SonarDrawer.cpp b/lib/SonarDrawer.cpp deleted file mode 100644 index 392bfc5..0000000 --- a/lib/SonarDrawer.cpp +++ /dev/null @@ -1,321 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#include -#include -#include - -#include "sonar_image_proc/DrawSonar.h" -#include "sonar_image_proc/OverlayImage.h" - -namespace sonar_image_proc { - -using sonar_image_proc::AbstractSonarInterface; - -static float deg2radf(float deg) { return deg * M_PI / 180.0; } -static float rad2degf(float rad) { return rad * 180.0 / M_PI; } - -SonarDrawer::SonarDrawer() { ; } - -cv::Mat SonarDrawer::drawRectSonarImage(const AbstractSonarInterface &ping, - const SonarColorMap &colorMap, - const cv::Mat &rectIn) { - cv::Mat rect(rectIn); - - const cv::Size imgSize(ping.nRanges(), ping.nBearings()); - - if ((rect.type() == CV_8UC3) || (rect.type() == CV_32FC3) || - (rect.type() == CV_32FC1)) { - rect.create(imgSize, rect.type()); - } else { - rect.create(imgSize, CV_8UC3); - } - - for (int r = 0; r < ping.nRanges(); r++) { - for (int b = 0; b < ping.nBearings(); b++) { - const AzimuthRangeIndices loc(b, r); - - if (rect.type() == CV_8UC3) { - rect.at(cv::Point(r, b)) = colorMap.lookup_cv8uc3(ping, loc); - } else if (rect.type() == CV_32FC3) { - rect.at(cv::Point(r, b)) = - colorMap.lookup_cv32fc3(ping, loc); - } else if (rect.type() == CV_32FC1) { - rect.at(cv::Point(r, b)) = colorMap.lookup_cv32fc1(ping, loc); - } else { - assert("Should never get here."); - } - } - } - - return rect; -} - -cv::Mat SonarDrawer::remapRectSonarImage(const AbstractSonarInterface &ping, - const cv::Mat &rectImage) { - cv::Mat out; - const CachedMap::MapPair maps(_map(ping)); - cv::remap(rectImage, out, maps.first, maps.second, cv::INTER_CUBIC, - cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0)); - - return out; -} - -cv::Mat SonarDrawer::drawOverlay(const AbstractSonarInterface &ping, - const cv::Mat &sonarImage) { - // Alpha blend overlay onto sonarImage - cv::Mat output; - overlayImage( - sonarImage, _overlay(ping, sonarImage, overlayConfig()), output); - - return output; -} - -cv::Mat SonarDrawer::drawSonar(const AbstractSonarInterface &ping, - const SonarColorMap &colorMap, - const cv::Mat &img, bool addOverlay) { - cv::Mat rect = drawRectSonarImage(ping, colorMap, img); - cv::Mat sonar = remapRectSonarImage(ping, rect); - if (addOverlay) { - return drawOverlay(ping, sonar); - } else { - return sonar; - } -} - -// ==== SonarDrawer::Cached ==== - -bool SonarDrawer::Cached::isValid(const AbstractSonarInterface &ping) const { - // Check for cache invalidation... - if ((_numAzimuth != ping.nAzimuth()) || (_numRanges != ping.nRanges()) || - (_rangeBounds != ping.rangeBounds() || - (_azimuthBounds != ping.azimuthBounds()))) - return false; - - return true; -} - -// ==== SonarDrawer::CachedMap ==== - -SonarDrawer::CachedMap::MapPair SonarDrawer::CachedMap::operator()( - const AbstractSonarInterface &ping) { - // _scMap[12] are mutable to break out of const - if (!isValid(ping)) create(ping); - - return std::make_pair(_scMap1, _scMap2); -} - -// **assumes** this structure for the rectImage: -// * It has nBearings cols and nRanges rows -// -void SonarDrawer::CachedMap::create(const AbstractSonarInterface &ping) { - cv::Mat newmap; - - const int nRanges = ping.nRanges(); - const auto azimuthBounds = ping.azimuthBounds(); - - const int minusWidth = floor(nRanges * sin(azimuthBounds.first)); - const int plusWidth = ceil(nRanges * sin(azimuthBounds.second)); - const int width = plusWidth - minusWidth; - - const int originx = abs(minusWidth); - - const cv::Size imgSize(width, nRanges); - if ((width <= 0) || (nRanges <= 0)) return; - - newmap.create(imgSize, CV_32FC2); - - const float db = - (azimuthBounds.second - azimuthBounds.first) / ping.nAzimuth(); - - for (int x = 0; x < newmap.cols; x++) { - for (int y = 0; y < newmap.rows; y++) { - // For cv::remap, a map is - // - // dst = src( mapx(x,y), mapy(x,y) ) - // - // That is, the map is the size of the dst array, - // and contains the coords in the source image - // for each pixel in the dst image. - // - // This map draws the sonar with range = 0 - // centered on the bottom edge of the resulting image - // with increasing range along azimuth = 0 going - // vertically upwards in the image - - // Calculate range and bearing of this pixel from origin - const float dx = x - originx; - const float dy = newmap.rows - y; - - const float range = sqrt(dx * dx + dy * dy); - const float azimuth = atan2(dx, dy); - - float xp = range; - - //\todo This linear algorithm is not robust if the azimuths - // are non-linear. Should implement a real interpolation... - float yp = (azimuth - azimuthBounds.first) / db; - - newmap.at(cv::Point(x, y)) = cv::Vec2f(xp, yp); - } - } - - cv::convertMaps(newmap, cv::Mat(), _scMap1, _scMap2, CV_16SC2); - - // Save meta-information to check for cache expiry - _numRanges = ping.nRanges(); - _numAzimuth = ping.nBearings(); - - _rangeBounds = ping.rangeBounds(); - _azimuthBounds = ping.azimuthBounds(); -} - -bool SonarDrawer::CachedMap::isValid(const AbstractSonarInterface &ping) const { - if (_scMap1.empty() || _scMap2.empty()) return false; - - return Cached::isValid(ping); -} - -// === SonarDrawer::CachedOverlay === - -bool SonarDrawer::CachedOverlay::isValid(const AbstractSonarInterface &ping, - const cv::Mat &sonarImage, - const OverlayConfig &config) const { - if (sonarImage.size() != _overlay.size()) return false; - - if (_config_used != config) return false; - - return Cached::isValid(ping); -} - -const cv::Mat &SonarDrawer::CachedOverlay::operator()( - const AbstractSonarInterface &ping, const cv::Mat &sonarImage, - const OverlayConfig &config) { - if (!isValid(ping, sonarImage, config)) create(ping, sonarImage, config); - - return _overlay; -} - -// Converts sonar bearing (with sonar "forward" at bearing 0) to image -// orientation with sonar "forward" point upward in the image, which is the -Y -// direction in image coordinates. -static float bearingToImage(float d) { return (-M_PI / 2) + d; } - -void SonarDrawer::CachedOverlay::create(const AbstractSonarInterface &ping, - const cv::Mat &sonarImage, - const OverlayConfig &config) { - const cv::Size sz(sonarImage.size()); - const cv::Point2f origin(sz.width / 2, sz.height); - - // Reset overlay - _overlay = cv::Mat::zeros(sz, CV_8UC4); - const cv::Vec3b color(config.lineColor()); - const cv::Vec4b textColor(color[0], color[1], color[2], 255); - const cv::Vec4b lineColor(color[0], color[1], color[2], - config.lineAlpha() * 255); - - const float minAzimuth = ping.minAzimuth(); - const float maxAzimuth = ping.maxAzimuth(); - - const float maxRange = ping.maxRange(); - - //== Draw arcs == - float arcSpacing = config.rangeSpacing(); - - if (arcSpacing <= 0) { - // Calculate automatically .. just a lame heuristic for now - if (maxRange < 2) - arcSpacing = 0.5; - else if (maxRange < 5) - arcSpacing = 1.0; - else if (maxRange < 10) - arcSpacing = 2.0; - else if (maxRange < 50) - arcSpacing = 10.0; - else - arcSpacing = 20.0; - } - - const float minRange = arcSpacing; - - for (float r = minRange; r < ping.maxRange(); r += arcSpacing) { - const float radiusPix = (r / maxRange) * sonarImage.size().height; - - cv::ellipse(_overlay, origin, cv::Size(radiusPix, radiusPix), 0, - rad2degf(bearingToImage(minAzimuth)), - rad2degf(bearingToImage(maxAzimuth)), lineColor, - config.lineThickness()); - - { - std::stringstream rstr; - rstr << r; - - // Calculate location of string - const float theta = bearingToImage(minAzimuth); - - // \todo{??} Should calculate this automatically ... not sure - const cv::Point2f offset(-25, 20); - - const cv::Point2f pt(radiusPix * cos(theta) + origin.x + offset.x, - radiusPix * sin(theta) + origin.y + offset.y); - - cv::putText(_overlay, rstr.str(), pt, cv::FONT_HERSHEY_PLAIN, - config.fontScale(), textColor); - } - } - - // And one arc at max range - cv::ellipse(_overlay, origin, sz, 0, rad2degf(bearingToImage(minAzimuth)), - rad2degf(bearingToImage(maxAzimuth)), lineColor, - config.lineThickness()); - - //== Draw radials == - std::vector radials; - - // Configuration ... move later - const float radialSpacing = deg2radf(config.radialSpacing()); - - radials.push_back(minAzimuth); - radials.push_back(maxAzimuth); - - // If radialSpacing == 0. draw only the outline radial lines - if (radialSpacing > 0) { - if (config.radialAtZero()) { - // \todo(@amarburg) to implement - } else { - for (float d = radialSpacing / 2; d > minAzimuth && d < maxAzimuth; - d += radialSpacing) { - radials.push_back(d); - } - for (float d = -radialSpacing / 2; d > minAzimuth && d < maxAzimuth; - d -= radialSpacing) { - radials.push_back(d); - } - } - } - - // Sort and unique - std::sort(radials.begin(), radials.end()); - auto last = std::unique(radials.begin(), radials.end()); - radials.erase(last, radials.end()); - - // And draw - const float minRangePix = (minRange / maxRange) * sz.height; - - if (minRange < maxRange) { - for (const auto b : radials) { - const float theta = bearingToImage(b); - - const cv::Point2f begin(minRangePix * cos(theta) + origin.x, - minRangePix * sin(theta) + origin.y); - const cv::Point2f end(sz.height * cos(theta) + origin.x, - sz.height * sin(theta) + origin.y); - - cv::line(_overlay, begin, end, lineColor, config.lineThickness()); - } - } - - _config_used = config; -} - -} // namespace sonar_image_proc diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml deleted file mode 100644 index e61143b..0000000 --- a/nodelet_plugins.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - A nodelet version of the DrawSonar - - - - - - A nodelet version of the SonarPostprocessor - - - diff --git a/python/.gitignore b/python/.gitignore deleted file mode 100644 index 3feb78a..0000000 --- a/python/.gitignore +++ /dev/null @@ -1 +0,0 @@ -*.egg-info/ diff --git a/python/README.md b/python/README.md deleted file mode 100644 index aa27ca2..0000000 --- a/python/README.md +++ /dev/null @@ -1,8 +0,0 @@ - -Borrows heavily from: - -[https://github.com/edmBernard/pybind11_opencv_numpy/blob/master/example.cpp](https://github.com/edmBernard/pybind11_opencv_numpy/blob/master/example.cpp) - -and - -[https://github.com/pybind/python_example](https://github.com/pybind/python_example) diff --git a/python/draw_sonar/__init__.py b/python/draw_sonar/__init__.py deleted file mode 100644 index 705f190..0000000 --- a/python/draw_sonar/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ - -from py_draw_sonar import * - -## A trivial test function to check if the module is loading properly -def pyadd(a,b): - return a+b diff --git a/python/histogram_drawer b/python/histogram_drawer deleted file mode 100755 index 93a32a1..0000000 --- a/python/histogram_drawer +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python3 - -import rospy - -from std_msgs.msg import UInt32MultiArray -from sensor_msgs.msg import Image -from cv_bridge import CvBridge - -import io -import numpy as np -import matplotlib -matplotlib.use('agg') # turn off interactive backend -import matplotlib.pyplot as plt - -class HistogramDrawer: - - def __init__(self): - - self.pub = rospy.Publisher("drawn_histogram", Image, queue_size=10) - self.sub = rospy.Subscriber("histogram", UInt32MultiArray, self.rx_histogram) - - self.bin_max = 0 - - def rx_histogram(self, data): - #print(data.data) - - xmax = len(data.data) - - ## Regardless, compress to 128 bins .. this algorithm assumes the new - # number of bins is a factor of the original histogram bins - d = np.reshape(data.data, (128,-1), 'C') - - new_histogram = np.sum(d,axis=1) - - self.bin_max = np.max((self.bin_max, max(new_histogram))) - - #print(new_histogram) - fig, ax = plt.subplots() - - plt.yscale("log") - ax.plot(np.linspace(0,xmax,128), new_histogram,'b.-') - ax.set_xlabel("Intensity") - ax.set_ylabel("Count") - ax.set_ylim([1, self.bin_max*2]) - - with io.BytesIO() as buff: - fig.savefig(buff, format='raw') - buff.seek(0) - data = np.frombuffer(buff.getvalue(), dtype=np.uint8) - w, h = fig.canvas.get_width_height() - im = data.reshape((int(h), int(w), -1)) - - # Drop alpha channel - im = im[:,:,:3] - - bridge = CvBridge() - image_message = bridge.cv2_to_imgmsg(im, encoding="rgb8") - - self.pub.publish(image_message) - - plt.close(fig) - - - -if __name__ == "__main__": - - rospy.init_node('histogram_drawer', anonymous=True) - - drawer = HistogramDrawer() - - rospy.spin(); diff --git a/python/ndarray_converter.cpp b/python/ndarray_converter.cpp deleted file mode 100644 index 158999c..0000000 --- a/python/ndarray_converter.cpp +++ /dev/null @@ -1,330 +0,0 @@ -// borrowed in spirit from -// https://github.com/yati-sagade/opencv-ndarray-conversion MIT License - -#include "ndarray_converter.h" - -#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION -#include - -#if PY_VERSION_HEX >= 0x03000000 -#define PyInt_Check PyLong_Check -#define PyInt_AsLong PyLong_AsLong -#endif - -struct Tmp { - const char *name; - - Tmp(const char *name) : name(name) {} -}; - -Tmp info("return value"); - -bool NDArrayConverter::init_numpy() { - // this has to be in this file, since PyArray_API is defined as static - import_array1(false); - return true; -} - -/* - * The following conversion functions are taken/adapted from OpenCV's cv2.cpp - * file inside modules/python/src2 folder (OpenCV 3.1.0) - */ - -static PyObject *opencv_error = 0; - -static int failmsg(const char *fmt, ...) { - char str[1000]; - - va_list ap; - va_start(ap, fmt); - vsnprintf(str, sizeof(str), fmt, ap); - va_end(ap); - - PyErr_SetString(PyExc_TypeError, str); - return 0; -} - -class PyAllowThreads { - public: - PyAllowThreads() : _state(PyEval_SaveThread()) {} - ~PyAllowThreads() { PyEval_RestoreThread(_state); } - - private: - PyThreadState *_state; -}; - -class PyEnsureGIL { - public: - PyEnsureGIL() : _state(PyGILState_Ensure()) {} - ~PyEnsureGIL() { PyGILState_Release(_state); } - - private: - PyGILState_STATE _state; -}; - -#define ERRWRAP2(expr) \ - try { \ - PyAllowThreads allowThreads; \ - expr; \ - } catch (const cv::Exception &e) { \ - PyErr_SetString(opencv_error, e.what()); \ - return 0; \ - } - -using namespace cv; - -class NumpyAllocator : public MatAllocator { - public: - NumpyAllocator() { stdAllocator = Mat::getStdAllocator(); } - ~NumpyAllocator() {} - - UMatData *allocate(PyObject *o, int dims, const int *sizes, int type, - size_t *step) const { - UMatData *u = new UMatData(this); - u->data = u->origdata = (uchar *)PyArray_DATA((PyArrayObject *)o); - npy_intp *_strides = PyArray_STRIDES((PyArrayObject *)o); - for (int i = 0; i < dims - 1; i++) step[i] = (size_t)_strides[i]; - step[dims - 1] = CV_ELEM_SIZE(type); - u->size = sizes[0] * step[0]; - u->userdata = o; - return u; - } - -#if CV_MAJOR_VERSION < 4 - UMatData *allocate(int dims0, const int *sizes, int type, void *data, - size_t *step, int flags, UMatUsageFlags usageFlags) const -#else - UMatData *allocate(int dims0, const int *sizes, int type, void *data, - size_t *step, AccessFlag flags, - UMatUsageFlags usageFlags) const -#endif - { - if (data != 0) { - CV_Error(Error::StsAssert, "The data should normally be NULL!"); - // probably this is safe to do in such extreme case - return stdAllocator->allocate(dims0, sizes, type, data, step, flags, - usageFlags); - } - PyEnsureGIL gil; - - int depth = CV_MAT_DEPTH(type); - int cn = CV_MAT_CN(type); - const int f = (int)(sizeof(size_t) / 8); - int typenum = depth == CV_8U ? NPY_UBYTE - : depth == CV_8S ? NPY_BYTE - : depth == CV_16U ? NPY_USHORT - : depth == CV_16S ? NPY_SHORT - : depth == CV_32S ? NPY_INT - : depth == CV_32F ? NPY_FLOAT - : depth == CV_64F ? NPY_DOUBLE - : f * NPY_ULONGLONG + (f ^ 1) * NPY_UINT; - int i, dims = dims0; - cv::AutoBuffer _sizes(dims + 1); - for (i = 0; i < dims; i++) _sizes[i] = sizes[i]; - if (cn > 1) _sizes[dims++] = cn; - PyObject *o = PyArray_SimpleNew(dims, _sizes, typenum); - if (!o) - CV_Error_(Error::StsError, - ("The numpy array of typenum=%d, ndims=%d can not be created", - typenum, dims)); - return allocate(o, dims0, sizes, type, step); - } - -#if CV_MAJOR_VERSION < 4 - bool allocate(UMatData *u, int accessFlags, UMatUsageFlags usageFlags) const -#else - bool allocate(UMatData *u, AccessFlag accessFlags, - UMatUsageFlags usageFlags) const -#endif - { - return stdAllocator->allocate(u, accessFlags, usageFlags); - } - - void deallocate(UMatData *u) const { - if (!u) return; - PyEnsureGIL gil; - CV_Assert(u->urefcount >= 0); - CV_Assert(u->refcount >= 0); - if (u->refcount == 0) { - PyObject *o = (PyObject *)u->userdata; - Py_XDECREF(o); - delete u; - } - } - - const MatAllocator *stdAllocator; -}; - -NumpyAllocator g_numpyAllocator; - -bool NDArrayConverter::toMat(PyObject *o, Mat &m) { - bool allowND = true; - if (!o || o == Py_None) { - if (!m.data) m.allocator = &g_numpyAllocator; - return true; - } - - if (PyInt_Check(o)) { - double v[] = {static_cast(PyInt_AsLong((PyObject *)o)), 0., 0., 0.}; - m = Mat(4, 1, CV_64F, v).clone(); - return true; - } - if (PyFloat_Check(o)) { - double v[] = {PyFloat_AsDouble((PyObject *)o), 0., 0., 0.}; - m = Mat(4, 1, CV_64F, v).clone(); - return true; - } - if (PyTuple_Check(o)) { - int i, sz = (int)PyTuple_Size((PyObject *)o); - m = Mat(sz, 1, CV_64F); - for (i = 0; i < sz; i++) { - PyObject *oi = PyTuple_GET_ITEM(o, i); - if (PyInt_Check(oi)) - m.at(i) = (double)PyInt_AsLong(oi); - else if (PyFloat_Check(oi)) - m.at(i) = (double)PyFloat_AsDouble(oi); - else { - failmsg("%s is not a numerical tuple", info.name); - m.release(); - return false; - } - } - return true; - } - - if (!PyArray_Check(o)) { - failmsg("%s is not a numpy array, neither a scalar", info.name); - return false; - } - - PyArrayObject *oarr = (PyArrayObject *)o; - - bool needcopy = false, needcast = false; - int typenum = PyArray_TYPE(oarr), new_typenum = typenum; - int type = typenum == NPY_UBYTE ? CV_8U - : typenum == NPY_BYTE ? CV_8S - : typenum == NPY_USHORT ? CV_16U - : typenum == NPY_SHORT ? CV_16S - : typenum == NPY_INT ? CV_32S - : typenum == NPY_INT32 ? CV_32S - : typenum == NPY_FLOAT ? CV_32F - : typenum == NPY_DOUBLE ? CV_64F - : -1; - - if (type < 0) { - if (typenum == NPY_INT64 || typenum == NPY_UINT64 || typenum == NPY_LONG) { - needcopy = needcast = true; - new_typenum = NPY_INT; - type = CV_32S; - } else { - failmsg("%s data type = %d is not supported", info.name, typenum); - return false; - } - } - -#ifndef CV_MAX_DIM - const int CV_MAX_DIM = 32; -#endif - - int ndims = PyArray_NDIM(oarr); - if (ndims >= CV_MAX_DIM) { - failmsg("%s dimensionality (=%d) is too high", info.name, ndims); - return false; - } - - int size[CV_MAX_DIM + 1]; - size_t step[CV_MAX_DIM + 1]; - size_t elemsize = CV_ELEM_SIZE1(type); - const npy_intp *_sizes = PyArray_DIMS(oarr); - const npy_intp *_strides = PyArray_STRIDES(oarr); - bool ismultichannel = ndims == 3 && _sizes[2] <= CV_CN_MAX; - - for (int i = ndims - 1; i >= 0 && !needcopy; i--) { - // these checks handle cases of - // a) multi-dimensional (ndims > 2) arrays, as well as simpler 1- and - // 2-dimensional cases b) transposed arrays, where _strides[] elements go - // in non-descending order c) flipped arrays, where some of _strides[] - // elements are negative - // the _sizes[i] > 1 is needed to avoid spurious copies when - // NPY_RELAXED_STRIDES is set - if ((i == ndims - 1 && _sizes[i] > 1 && (size_t)_strides[i] != elemsize) || - (i < ndims - 1 && _sizes[i] > 1 && _strides[i] < _strides[i + 1])) - needcopy = true; - } - - if (ismultichannel && _strides[1] != (npy_intp)elemsize * _sizes[2]) - needcopy = true; - - if (needcopy) { - // if (info.outputarg) - //{ - // failmsg("Layout of the output array %s is incompatible with cv::Mat - // (step[ndims-1] != elemsize or step[1] != elemsize*nchannels)", - // info.name); return false; - // } - - if (needcast) { - o = PyArray_Cast(oarr, new_typenum); - oarr = (PyArrayObject *)o; - } else { - oarr = PyArray_GETCONTIGUOUS(oarr); - o = (PyObject *)oarr; - } - - _strides = PyArray_STRIDES(oarr); - } - - // Normalize strides in case NPY_RELAXED_STRIDES is set - size_t default_step = elemsize; - for (int i = ndims - 1; i >= 0; --i) { - size[i] = (int)_sizes[i]; - if (size[i] > 1) { - step[i] = (size_t)_strides[i]; - default_step = step[i] * size[i]; - } else { - step[i] = default_step; - default_step *= size[i]; - } - } - - // handle degenerate case - if (ndims == 0) { - size[ndims] = 1; - step[ndims] = elemsize; - ndims++; - } - - if (ismultichannel) { - ndims--; - type |= CV_MAKETYPE(0, size[2]); - } - - if (ndims > 2 && !allowND) { - failmsg("%s has more than 2 dimensions", info.name); - return false; - } - - m = Mat(ndims, size, type, PyArray_DATA(oarr), step); - m.u = g_numpyAllocator.allocate(o, ndims, size, type, step); - m.addref(); - - if (!needcopy) { - Py_INCREF(o); - } - m.allocator = &g_numpyAllocator; - - return true; -} - -PyObject *NDArrayConverter::toNDArray(const cv::Mat &m) { - if (!m.data) Py_RETURN_NONE; - Mat temp, *p = (Mat *)&m; - if (!p->u || p->allocator != &g_numpyAllocator) { - temp.allocator = &g_numpyAllocator; - ERRWRAP2(m.copyTo(temp)); - p = &temp; - } - PyObject *o = (PyObject *)p->u->userdata; - Py_INCREF(o); - return o; -} diff --git a/python/ndarray_converter.h b/python/ndarray_converter.h deleted file mode 100644 index 3b87f94..0000000 --- a/python/ndarray_converter.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef __NDARRAY_CONVERTER_H__ -#define __NDARRAY_CONVERTER_H__ - -#include - -#include - -class NDArrayConverter { - public: - // must call this first, or the other routines don't work! - static bool init_numpy(); - - static bool toMat(PyObject *o, cv::Mat &m); - static PyObject *toNDArray(const cv::Mat &mat); -}; - -// -// Define the type converter -// - -#include - -namespace pybind11 { -namespace detail { - -template <> -struct type_caster { - public: - PYBIND11_TYPE_CASTER(cv::Mat, _("numpy.ndarray")); - - bool load(handle src, bool) { - return NDArrayConverter::toMat(src.ptr(), value); - } - - static handle cast(const cv::Mat &m, return_value_policy, handle defval) { - return handle(NDArrayConverter::toNDArray(m)); - } -}; - -} // namespace detail -} // namespace pybind11 - -#endif diff --git a/python/old_setup.py b/python/old_setup.py deleted file mode 100644 index 7289d88..0000000 --- a/python/old_setup.py +++ /dev/null @@ -1,120 +0,0 @@ -# -# Much of this copied from https://github.com/edmBernard/pybind11_opencv_numpy -# - -import fnmatch -import os -from os.path import dirname, exists, join -from setuptools import find_packages, setup, Extension -from setuptools.command.build_ext import build_ext -import subprocess -import sys -import setuptools - -# -# pybind-specific compilation stuff -# - -class get_pybind_include(object): - """Helper class to determine the pybind11 include path - - The purpose of this class is to postpone importing pybind11 - until it is actually installed, so that the ``get_include()`` - method can be invoked. """ - - def __init__(self, user=False): - self.user = user - - def __str__(self): - import pybind11 - return pybind11.get_include(self.user) - -# As of Python 3.6, CCompiler has a `has_flag` method. -# cf http://bugs.python.org/issue26689 -def has_flag(compiler, flagname): - """Return a boolean indicating whether a flag name is supported on - the specified compiler. - """ - import tempfile - with tempfile.NamedTemporaryFile('w', suffix='.cpp') as f: - f.write('int main (int argc, char **argv) { return 0; }') - try: - compiler.compile([f.name], extra_postargs=[flagname]) - except setuptools.distutils.errors.CompileError: - return False - return True - - -def cpp_flag(compiler): - """Return the -std=c++[11/14] compiler flag. - - The c++14 is preferred over c++11 (when it is available). - """ - if has_flag(compiler, '-std=c++14'): - return '-std=c++14' - elif has_flag(compiler, '-std=c++11'): - return '-std=c++11' - else: - raise RuntimeError('Unsupported compiler -- at least C++11 support ' - 'is needed!') - - -class BuildExt(build_ext): - """A custom build extension for adding compiler-specific options.""" - c_opts = { - 'msvc': ['/EHsc'], - 'unix': [], - } - - if sys.platform == 'darwin': - c_opts['unix'] += ['-stdlib=libc++', '-mmacosx-version-min=10.7'] - - def build_extensions(self): - ct = self.compiler.compiler_type - opts = self.c_opts.get(ct, []) - - for arg in ext_modules[0].extra_compile_args: - opts.append(arg) - - if ct == 'unix': - opts.append('-s') # strip - opts.append('-g0') # remove debug symbols - opts.append(cpp_flag(self.compiler)) - if has_flag(self.compiler, '-fvisibility=hidden'): - opts.append('-fvisibility=hidden') - for ext in self.extensions: - ext.extra_compile_args = opts - build_ext.build_extensions(self) - -ext_modules = [ - Extension( - 'serdp_common', - [ - 'serdp_common_py.cpp', - 'ndarray_converter.cpp', - '../lib/DrawSonar.cpp', - '../lib/DataStructures.cpp' - ], - include_dirs=[ - # Path to pybind11 headers - get_pybind_include(), - get_pybind_include(user=True), - "../include/" - ], - extra_compile_args=['-DABSTRACT_SONAR_INTERFACE_ONLY'], - libraries=['opencv_core', 'opencv_highgui'], - language='c++', - ), -] - -setup( - name='serdp_common', - version='0.1', - author='Aaron Marburg', - author_email='amarburg@uw.edu', - packages=find_packages(), - ext_modules=ext_modules, - install_requires=None, - cmdclass={'build_ext': BuildExt}, - zip_safe=False, -) diff --git a/python/py_draw_sonar.cpp b/python/py_draw_sonar.cpp deleted file mode 100644 index 7024a5e..0000000 --- a/python/py_draw_sonar.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// #include -// -// #include -// -// // #include "ndarray_converter.h" -// // #include "serdp_common/DrawSonar.h" -// -// namespace py = pybind11; -// -// -// cv::Mat read_image(std::string image_name) { -// #if CV_MAJOR_VERSION < 4 -// cv::Mat image = cv::imread(image_name, CV_LOAD_IMAGE_COLOR); -// #else -// cv::Mat image = cv::imread(image_name, cv::IMREAD_COLOR); -// #endif -// return image; -// } -// -// // class AddClass { -// // public: -// // AddClass(int value) : value(value) {} -// // -// // cv::Mat add(cv::Mat input) { -// // return input + this->value; -// // } -// // -// // private: -// // int value; -// // }; -// - -#include -#include - -#include -#include -#include -#include -#include - -#include "draw_sonar/DataStructures.h" -#include "draw_sonar/DrawSonar.h" -#include "ndarray_converter.h" - -namespace py = pybind11; - -// Sample functions from examples used to bootstrap the project - -cv::Mat read_image(std::string image_name) { -#if CV_MAJOR_VERSION < 4 - cv::Mat image = cv::imread(image_name, CV_LOAD_IMAGE_COLOR); -#else - cv::Mat image = cv::imread(image_name, cv::IMREAD_COLOR); -#endif - return image; -} - -cv::Mat passthru(cv::Mat image) { return image; } - -cv::Mat cloneimg(cv::Mat image) { return image.clone(); } - -int add(int i, int j) { return i + j; } - -class AddClass { - public: - AddClass(int value) : value(value) {} - - cv::Mat add(cv::Mat input) { return input + this->value; } - - private: - int value; -}; - -int calculateImageWidth(int height) { return 100; } - -/// -PYBIND11_MODULE(py_draw_sonar, m) { - NDArrayConverter::init_numpy(); - - m.def("read_image", &read_image, "A function that read an image", - py::arg("image")); - - m.def("passthru", &passthru, "Passthru function", py::arg("image")); - m.def("clone", &cloneimg, "Clone function", py::arg("image")); - - m.def("cppadd", &add, "A function which adds two numbers"); - - m.def("calculateImageWidth", &calculateImageWidth, - "Given a sonar height, calculate width in pixels"); - - py::class_(m, "AddClass") - .def(py::init()) - .def("add", &AddClass::add); -} diff --git a/python/sonar_image_proc/__init__.py b/python/sonar_image_proc/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/python/sonar_image_proc/sonar_msg_metadata.py b/python/sonar_image_proc/sonar_msg_metadata.py deleted file mode 100644 index fd400d4..0000000 --- a/python/sonar_image_proc/sonar_msg_metadata.py +++ /dev/null @@ -1,65 +0,0 @@ -#! /usr/bin/env python3 -""" -Copyright 2023 University of Washington Applied Physics Laboratory -Author: Marc Micatka & Laura Lindzey -""" - -from __future__ import annotations # use type of class in member function annotation. - -import numpy as np -import typing -from marine_acoustic_msgs.msg import ProjectedSonarImage - - -class SonarImageMetadata(object): - def __init__(self, sonar_image_msg: ProjectedSonarImage): - """ - Metadata for a sonar image, containing all information necessary - to compute its geometry. - NOTE(lindzey): excludes beamwidths because those are not used when - deciding what elevation angles to publish. - """ - self.num_angles = len(sonar_image_msg.beam_directions) - self.num_ranges = len(sonar_image_msg.ranges) - self.ranges = np.array(sonar_image_msg.ranges) - self.min_range = np.min(sonar_image_msg.ranges) - self.max_range = np.max(sonar_image_msg.ranges) - - xx = np.array([dir.x for dir in sonar_image_msg.beam_directions]) - yy = np.array([dir.y for dir in sonar_image_msg.beam_directions]) - zz = np.array([dir.z for dir in sonar_image_msg.beam_directions]) - self.azimuths = np.arctan2(-1 * yy, np.sqrt(xx**2 + zz**2)) - self.min_azimuth = np.min(self.azimuths) - self.max_azimuth = np.max(self.azimuths) - elev_beamwidth = np.median(sonar_image_msg.ping_info.tx_beamwidths) - self.min_elevation = -0.5 * elev_beamwidth - self.max_elevation = 0.5 * elev_beamwidth - - def __eq__(self, other: SonarImageMetadata) -> bool: - """ - Overrides the default implementation of == and != (along with is and is not) - Determine whether all fields are "close enough" for the - metadata to be the same. - """ - if self.num_angles != other.num_angles: - return False - if self.num_ranges != other.num_ranges: - return False - return np.allclose( - [self.min_range, self.max_range, self.min_azimuth, self.max_azimuth], - [other.min_range, other.max_range, other.min_azimuth, other.max_azimuth], - ) - - def __str__(self) -> typing.String: - """ - Overrides the default implementation of print(SonarImageMetadata) - """ - ss = "SonarImageMetadata: {} beams, {} ranges, {:0.2f}=>{:0.2f} m, {:0.1f}=>{:0.1f} deg".format( - self.num_angles, - self.num_ranges, - self.min_range, - self.max_range, - np.degrees(self.min_azimuth), - np.degrees(self.max_azimuth), - ) - return ss diff --git a/python/test.py b/python/test.py deleted file mode 100644 index ef044ea..0000000 --- a/python/test.py +++ /dev/null @@ -1,4 +0,0 @@ -import draw_sonar - -print( draw_sonar.pyadd(1,2) ) -print( draw_sonar.cppadd(1,2) ) diff --git a/ros/src/draw_sonar_node.cpp b/ros/src/draw_sonar_node.cpp deleted file mode 100644 index b8ece56..0000000 --- a/ros/src/draw_sonar_node.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#include "nodelet/loader.h" -#include "ros/ros.h" - -int main(int argc, char **argv) { - ros::init(argc, argv, "draw_sonar"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - - nodelet.load(ros::this_node::getName(), "sonar_image_proc/draw_sonar", remap, - nargv); - - ros::spin(); - return 0; -} diff --git a/ros/src/sonar_postprocessor_node.cpp b/ros/src/sonar_postprocessor_node.cpp deleted file mode 100644 index 91fa87f..0000000 --- a/ros/src/sonar_postprocessor_node.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#include "nodelet/loader.h" -#include "ros/ros.h" - -int main(int argc, char **argv) { - ros::init(argc, argv, "sonar_postprocessor"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - - nodelet.load(ros::this_node::getName(), - "sonar_image_proc/sonar_postprocessor", remap, nargv); - - ros::spin(); - return 0; -} diff --git a/sonar_image_proc.rosinstall b/sonar_image_proc.repos similarity index 100% rename from sonar_image_proc.rosinstall rename to sonar_image_proc.repos diff --git a/ros/src/draw_sonar_nodelet.cpp b/src/draw_sonar_nodelet.cpp similarity index 100% rename from ros/src/draw_sonar_nodelet.cpp rename to src/draw_sonar_nodelet.cpp diff --git a/src/draw_sonar_parameters.yaml b/src/draw_sonar_parameters.yaml new file mode 100644 index 0000000..f9c7ac0 --- /dev/null +++ b/src/draw_sonar_parameters.yaml @@ -0,0 +1,72 @@ +draw_sonar: + max_range: + type: double + default_value: 0.0 + description: "" + + publish_old: + type: bool + default_value: false + description: "" + + publish_timing: + type: bool + default_value: true + description: "" + + publish_histogram: + type: bool + default_value: true + description: "" + + color_map: + type: string + default_value: "inferno" + description: "" + + line_alpha: + type: double + default_value: 0.5 + description: "" + validation: + bounds<>: [0.0,1.0] + + line_thickness: + type: int + default_value: 1 + description: "" + validation: + bounds<>: [1,5] + + range_spacing: + type: double + default_value: 0 + description: "" + validation: + bounds<>: [0.0,100.0] + + bearing_spacing: + type: double + default_value: 0 + description: "" + validation: + bounds<>: [20.0,100.0] + + log_scale: + type: bool + default_value: false + description: "" + + min_db: + type: double + default_value: 0 + description: "" + validation: + bounds<>: [-221.0,0.0] + + max_db: + type: double + default_value: 0 + description: "" + validation: + bounds<>: [-221.0,0.0] diff --git a/ros/src/sonar_postprocessor_nodelet.cpp b/src/sonar_postprocessor_nodelet.cpp similarity index 100% rename from ros/src/sonar_postprocessor_nodelet.cpp rename to src/sonar_postprocessor_nodelet.cpp diff --git a/src/sonar_postprocessor_parameters.yaml b/src/sonar_postprocessor_parameters.yaml new file mode 100644 index 0000000..edd9dbd --- /dev/null +++ b/src/sonar_postprocessor_parameters.yaml @@ -0,0 +1,14 @@ +sonar_postprocessor: + gain: + type: double + default_value: 1.0 + description: "" + validation: + bounds<>: [0.0,100.0] + + gamma: + type: double + default_value: 1.0 + description: "" + validation: + bounds<>: [0.0,100.0] \ No newline at end of file diff --git a/ros/tools/bag2sonar.cpp b/tools/bag2sonar.cpp similarity index 100% rename from ros/tools/bag2sonar.cpp rename to tools/bag2sonar.cpp