diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 000000000..91f6cb057 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,47 @@ +--- +name: Bug report +about: Create a report to help improve wavemap +title: '' +labels: bug +assignees: victorreijgwart + +--- + +**Describe the bug** +A brief description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. Run wavemap launch file '...' +2. In rviz, request "Request whole map" '....' +3. See error + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Observed outcome** +A clear and concise description of what is actually happening, instead of what you expected. + +**Screenshots** +If relevant, add screenshots to help explain your problem. +For example, of error messages on the console and/or different outputs of wavemap shown in Rviz. + +**System information (please complete if relevant):** + - CPU: [e.g. Intel i9-9900K] + - GPU: [e.g. Nvidia RTX 2080Ti] # Only for visualization-related issues + - RAM: [e.g. 32GB] + - OS: [e.g. Ubuntu 20.04] + - Wavemap + - install: [e.g., Native (ROS with catkin); or Docker] + - version: [e.g., v1.4.0] + +**Runtime information (please complete if relevant):** + - Launch file: [e.g. Link to the launch file you used] + - Config file: [e.g. Link to the config file you used] + - Dataset name [e.g. Newer College Cloister sequence] # For public datasets + - Custom setup: # For online use or personal datasets + - depth sensor: [e.g. Livox MID360 LiDAR] + - pose source: [e.g. Odometry from FastLIO2] + +**Additional context** +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 000000000..354c2b5e2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,23 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: enhancement +assignees: victorreijgwart + +--- + +**Context** +What are you trying to do, and how would you like to do it differently? +Is it something wavemap currently cannot do? +If your request is related to a problem, briefly describe the problem as well. + +**Requested feature** +A clear and concise description of the feature you're interested in. +How would this feature benefit you and others? + +**(Optional) Suggest a solution** +If you have an idea for a solution, briefly summarize it here. + +**Additional context** +Add any other context or screenshots relevant to the requested feature here. diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 000000000..53cbedf13 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,50 @@ +# Description + +Thank you for opening a PR. Please summarize the changes in 1 or 2 sentences. + +## Type of change + +Delete options that are not relevant. + +- [ ] Bug fix (non-breaking change which fixes an issue) +- [ ] New feature (non-breaking change which adds functionality) +- [ ] Breaking change (fix or feature that would cause existing functionality to not work as expected) + +## Detailed summary + +Please describe the motivation, context and a link to related issues (if appropriate). List any dependencies that are required for this change. + +Feel free to summarize the changes as a list of bullet points. + +Fixes # (issue) + +# Testing + +If possible, verify that the changes produce the desired results by extending the unit tests. If you would like us to help you with this, feel free to open the pull request already and let us know. + +If manual tests were performed to verify these changes, please describe them here and provide instructions to reproduce them. Please also list any relevant details for your test configuration below. + +If the changes are performance related, this is a good place to list the metrics that were used and the improvements that have been achieved. + +**System information (please complete if relevant):** +- CPU: [e.g. Intel i9-9900K] +- GPU: [e.g. Nvidia RTX 2080Ti] # Only for visualization-related issues +- RAM: [e.g. 32GB] +- OS: [e.g. Ubuntu 20.04] +- Installation: [e.g., Native (ROS with catkin); or Docker] + +**Runtime information (please complete if relevant):** +- Launch file: [e.g. Link to the launch file you used] +- Config file: [e.g. Link to the config file you used] +- Dataset name [e.g. Newer College Cloister sequence] # For public datasets +- Custom setup: # For online use or personal datasets + - Depth sensor: [e.g. Livox MID360 LiDAR] + - Pose source: [e.g. Odometry from FastLIO2] + +# Checklist: + +- [ ] My code follows the style guidelines of this project +- [ ] I have performed a self-review of my code +- [ ] I have commented my code, particularly in hard-to-understand areas +- [ ] I have made corresponding changes to the documentation +- [ ] Any required changes in dependencies have been committed and pushed diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4e6c0c914..4a954db6d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -454,7 +454,9 @@ jobs: sanitizer: - { name: UBSAN, detects: 'undefined behavior' } - { name: ASAN, detects: 'addressability and leaks' } - - { name: TSAN, detects: 'data races and deadlocks' } + # - { name: TSAN, detects: 'data races and deadlocks' } + # NOTE: TSAN is disabled until the following bug is resolved: + # https://bugs.launchpad.net/ubuntu/+source/gcc-10/+bug/2029910. # NOTE: MSAN is not used for now since it also requires all deps to be # instrumented (recompiled with clang and the MSan flags, LLVM's # stdlib instead of GCCs,...). We therefore use Valgrind to diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d4380d53c..ef2daf4d3 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -93,13 +93,20 @@ repos: id: hadolint args: [ --config=tooling/git_hook_configs/.hadolint.yaml ] - - repo: https://github.com/sirosen/check-jsonschema - rev: 0.10.1 + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.24.1 hooks: - name: Check GitHub workflow file schema conformance id: check-github-workflows - name: Check GitHub action files schema conformance id: check-github-actions + - name: Check wavemap's custom schema definitions + id: check-metaschema + files: ^tooling/schemas/wavemap/.*\.(json)$ + - name: Check wavemap config files + id: check-jsonschema + files: ^(|.*\/)wavemap(|_\w+)\.yaml$ + args: [ --schemafile=tooling/schemas/wavemap/wavemap_config.json ] - repo: https://github.com/markdownlint/markdownlint rev: v0.11.0 @@ -114,6 +121,7 @@ repos: - name: Check syntax of reStructuredText id: rstcheck additional_dependencies: [ sphinx ] + args: [ --report-level=warning ] - repo: local hooks: @@ -123,25 +131,25 @@ repos: language: system types: [ file ] files: (package.xml)$ - args: [ --schema, tooling/xml_schemas/package_format2.xsd ] + args: [ --schema, tooling/schemas/xml/package_format2.xsd ] - name: Check ROS launch file schema conformance id: xmllint entry: xmllint --noout language: system types: [ file ] files: \.(launch)$ - args: [ --schema, tooling/xml_schemas/roslaunch.xsd ] + args: [ --schema, tooling/schemas/xml/roslaunch.xsd ] - name: Check (ROS) URDF file schema conformance id: xmllint entry: xmllint --noout language: system types: [ file ] files: \.(urdf)$ - args: [ --schema, tooling/xml_schemas/package_format2.xsd ] + args: [ --schema, tooling/schemas/xml/package_format2.xsd ] # - name: Check (ROS) SDF file schema conformance # id: xmllint # entry: xmllint --noout # language: system # types: [ file ] # files: \.(sdf|model|world)$ - # args: [ --schema, tooling/xml_schemas/sdf.xsd ] + # args: [ --schema, tooling/schemas/xml/sdf.xsd ] diff --git a/docs/Doxyfile b/docs/Doxyfile index 2447d1c8c..da86a8f3a 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -32,7 +32,7 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = "project" +PROJECT_NAME = "wavemap" # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version diff --git a/docs/conf.py b/docs/conf.py index 95054ae49..6db777a84 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -80,6 +80,7 @@ html_theme = 'sphinx_rtd_theme' html_theme_path = [sphinx_rtd_theme.get_html_theme_path()] html_logo = "logo.png" +html_title = f"release {version}" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -170,8 +171,8 @@ # -- Extension configuration ------------------------------------------------- # Setup the breathe extension -breathe_projects = {"project": "./_doxygen/xml"} -breathe_default_project = "project" +breathe_projects = {"wavemap": "./_doxygen/xml"} +breathe_default_project = "wavemap" # Setup the exhale extension exhale_args = { diff --git a/docs/index.rst b/docs/index.rst index 95ebe43f5..747f16128 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -1,5 +1,5 @@ -Wavemap -####### +Wavemap documentation +##################### .. image:: https://github.com/ethz-asl/wavemap/assets/6238939/0df66963-3871-4fae-8567-523518c43494 :alt: 3D reconstruction of Newer College's Cloister diff --git a/docs/pages/configuration.rst b/docs/pages/configuration.rst index 98c244a04..af6e9532f 100644 --- a/docs/pages/configuration.rst +++ b/docs/pages/configuration.rst @@ -1,4 +1,204 @@ Configuration ############# +.. highlight:: YAML +.. rstcheck: ignore-directives=doxygenstruct +.. rstcheck: ignore-roles=repo_file -A guide on how to configure and tune wavemap for usage on your own datasets and robots is coming soon. +Wavemap's configuration system is designed to be flexible and expressive. Let's start with an example to illustrate the overall structure: + +.. literalinclude:: ../../ros/wavemap_ros/config/wavemap_ouster_os0.yaml + :language: YAML + :caption: :repo_file:`Example config to use wavemap with an Ouster OS0-128 LiDAR` + +The map's general settings, such as the coordinate frame in which it's represented and the rate at which it's published, are configured under ``map/general``. +The data structure used to store the volumetric map is configured under ``map/data_structure``. +Finally, a list of inputs can be specified. + +You might have noticed two peculiarities in wavemap's configs. +The first is that subsections often feature a **type** tag. This tag determines the type of the object that should be created for the corresponding element. For example, setting ``data_structure/type: hashed_chunked_wavelet_octree`` tells wavemap to create a *HashedChunkedWaveletOctree* data structure to store the map. +The second is that **units** are specified explicitly. This eliminates misunderstandings and enables automatic conversions from derived units, such as ``{ millimeters: 27.67 }`` or ``{ degrees: 180.0 }``, to SI base units, such as ``{ meters: 0.02767 }`` or ``{ radians: 3.14159 }``. Wavemap's code internally always operates in SI base units. + +Note that the wavemap ROS server prints warnings for all unrecognized params at startup. This can be helpful for debugging and to quickly find typos in config param names. + +To get started quickly, we recommend skimming through these :repo_file:`example configs `. +For reference, an overview of all available config options is provided below. + +ROS server +********** +These settings control the general behavior of the ROS server. +They are nested in the top level config under ``map/general``. + +.. doxygenstruct:: wavemap::WavemapServerConfig + :project: wavemap + :members: + +Map data structures +******************* +These settings control the data structure that is used to store the map. +They are nested in the top level config under ``map/data_structure``. + +The following three data structures are fully supported by wavemap's ROS server: ``wavelet_octree``, ``hashed_wavelet_octree`` or ``hashed_chunked_wavelet_octree``. For general use, we recommend the ``hashed_wavelet_octree`` data structure. If you need the best possible performance, the ``hashed_chunked_wavelet_octree`` data structure is faster and uses less RAM. However, it is still under active development. + +Wavelet octree +============== +Selected by setting ``map/data_structure/type: "wavelet_octree"``. + +The ``wavelet_octree`` is the simplest of the wavelet-based data structures and stores the wavelet coefficients in a standard octree. This data structure can be useful in case you need the entire map to be contained in a single tree and don't mind sacrificing performance. + +.. doxygenstruct:: wavemap::WaveletOctreeConfig + :project: wavemap + :members: + +Hashed wavelet octree +===================== +Selected by setting ``map/data_structure/type: "hashed_wavelet_octree"``. + +The ``hashed_wavelet_octree`` combines the strengths of wavelet octrees with block-hashing. At the top level, the map is split into blocks which are accessed through a hash table. Each block in turn contains a small wavelet octree. + +.. doxygenstruct:: wavemap::HashedWaveletOctreeConfig + :project: wavemap + :members: + +Hashed chunked wavelet octree +============================= +Selected by setting ``map/data_structure/type: "hashed_chunked_wavelet_octree"``. + +The ``hashed_chunked_wavelet_octree`` is similar to the ``hashed_wavelet_octree``, but instead of storing all octree nodes separately it groups them into chunks. + +.. doxygenstruct:: wavemap::HashedChunkedWaveletOctreeConfig + :project: wavemap + :members: + + +Measurement inputs +****************** +These settings control the measurement input handlers. +They are nested in the top level config under ``inputs``. Note that ``inputs`` takes a YAML list, and one input handler will be created for each item in the list. + +Each input subscribes to a measurement ROS topic, looks up the pose for each measurement, and then calls its integrators to integrate the measurement into the map. We cover the measurement input handler types and their general settings first, followed by the integrator settings. + +Pointcloud input handler +======================== +Selected by setting ``inputs[i]/type: "pointcloud"``. + +The settings are available under ``inputs[i]/general`` are: + +.. doxygenstruct:: wavemap::PointcloudInputHandlerConfig + :project: wavemap + :members: + +Depth image input handler +========================= +Selected by setting ``inputs[i]/type: "depth_image"``. + +The settings are available under ``inputs[i]/general`` are: + +.. doxygenstruct:: wavemap::DepthImageInputHandlerConfig + :project: wavemap + :members: + +Measurement integrators +======================= +These settings specify which integrators will be applied whenever the input they belong to receives a new measurement. In similar fashion to the ``inputs`` key, ``inputs[i]/integrators`` takes a YAML list, and one integrator will be created for each item in the list. In most cases, a single integrator will suffice. However, the option of specifying multiple integrators can, for example, be used to integrate a given input at a very high resolution close to the robot (e.g. 1 to 3m) with one integrator, and at a lower resolution over a long range (e.g. 3 to 30m) with another integrator. + +For each integrator, you need to specify the ``integration_method``, ``projection_model`` and ``measurement_model``. + +Integration method +------------------ +These settings control the algorithm that is used to apply measurement updates to the map. +They are nested in the top level config under ``inputs[i]/integrators[j]/integration_method``. + +Compatible map data structure and integration method types: + +.. list-table:: + :widths: 50 50 + :header-rows: 1 + + * - Data structure + - Integration method + * - ``wavelet_octree`` + - ``wavelet_integrator`` + * - ``hashed_wavelet_octree`` + - ``hashed_wavelet_integrator`` + * - ``hashed_chunked_wavelet_octree`` + - ``hashed_chunked_wavelet_integrator`` + +They support the following settings: + +.. doxygenstruct:: wavemap::ProjectiveIntegratorConfig + :project: wavemap + :members: + +.. _configuration_projection_models: + +Projection models +----------------- +These settings control the projection model that is used to convert cartesian coordinates to/from sensor coordinates. +They are nested in the top level config under ``inputs[i]/integrators[j]/projection_model``. + +Spherical projection model +^^^^^^^^^^^^^^^^^^^^^^^^^^ +Selected by setting ``inputs[i]/integrators[j]/projection_model/type: 'spherical_projector'``. + +This projection model is appropriate for most LiDARs, including Velodynes. It also works with dome LiDARs such as the Robosense RS-Bpearl. + +The remaining settings available under ``inputs[i]/integrators[j]/projection_model`` are: + +.. doxygenstruct:: wavemap::SphericalProjectorConfig + :project: wavemap + :members: + +Ouster LiDAR projection model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Selected by setting ``inputs[i]/integrators[j]/projection_model/type: 'ouster_projector'``. + +This projection model improves the reconstruction accuracy for Ouster based LiDARs including the OS0 and OS1, by taking their staggered beam pattern into account. + +The remaining settings available under ``inputs[i]/integrators[j]/projection_model`` are: + +.. doxygenstruct:: wavemap::OusterProjectorConfig + :project: wavemap + :members: + +Pinhole camera projection model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Selected by setting ``inputs[i]/integrators[j]/projection_model/type: 'pinhole_camera_projector'``. + +This projection model is appropriate for most depth cameras. + +The remaining settings available under ``inputs[i]/integrators[j]/projection_model`` are: + +.. doxygenstruct:: wavemap::PinholeCameraProjectorConfig + :project: wavemap + :members: + +.. _configuration_measurement_models: + +Measurement models +------------------ +These settings control the measurement model that is used to convert measurements into corresponding occupancy updates. +They are nested in the top level config under ``inputs[i]/integrators[j]/measurement_model``. + +Continuous ray measurement model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Selected by setting ``inputs[i]/integrators[j]/measurement_model/type: 'continuous_ray'``. + +This measurement model accounts for uncertainty along each measured ray, but does not include angular uncertainty. It is appropriate for high density sensors with a limited range, such as most depth cameras. Considering angular uncertainty usually does not significantly improve reconstruction accuracy when the rays densely cover the entire observed volume, in which case the computational overhead is unnecessary. + +The remaining settings available under ``inputs[i]/integrators[j]/measurement_model`` are: + +.. doxygenstruct:: wavemap::ContinuousRayConfig + :project: wavemap + :members: + +Continuous beam measurement model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Selected by setting ``inputs[i]/integrators[j]/measurement_model/type: 'continuous_beam'``. + +This measurement model extends the continuous ray model, by including angular uncertainty. For LiDAR sensors, whose ray density is low at long range, it significantly improves the reconstruction quality and recall on thin objects. + +The remaining settings available under ``inputs[i]/integrators[j]/measurement_model`` are: + +.. doxygenstruct:: wavemap::ContinuousBeamConfig + :project: wavemap + :members: diff --git a/docs/pages/demos.rst b/docs/pages/demos.rst index fc991817f..342548448 100644 --- a/docs/pages/demos.rst +++ b/docs/pages/demos.rst @@ -36,7 +36,11 @@ To experiment with different wavemap settings, modify :repo_file:`this config fi Your own data ************* The only requirements for running wavemap are: -1. an odometry source, and -2. a source of depth camera or 3D LiDAR data, as either depth images or point clouds. -*Additional instructions coming soon.* +1. a source of depth measurements, +2. sensor pose (estimates) for each measurement. + +We usually use depth measurements from depth cameras or 3D LiDARs, but any source would work as long as a sufficiently good :ref:`projection ` and :ref:`measurement ` model is available. Wavemap's ROS interface natively supports depth image and pointcloud inputs, and automatically queries the sensor poses from the TF tree. + +To help you get started quickly, we provide various example :repo_file:`config ` and ROS :repo_file:`launch ` files. +For a brief introduction on how to edit wavemap configs and an overview of all the available settings, see the :doc:`configuration page `. diff --git a/libraries/wavemap/CHANGELOG.rst b/libraries/wavemap/CHANGELOG.rst index 21fb5e60d..058aa7da5 100644 --- a/libraries/wavemap/CHANGELOG.rst +++ b/libraries/wavemap/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package wavemap ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.0 (2023-08-30) +------------------ +* Document how to configure wavemap +* Improve config parameter unit management +* Make warnings/errors that can occur when loading configs more descriptive +* Contributors: Victor Reijgwart + 1.3.2 (2023-08-28) ------------------ diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt index 8fa8440d5..7ddd2d087 100644 --- a/libraries/wavemap/CMakeLists.txt +++ b/libraries/wavemap/CMakeLists.txt @@ -36,7 +36,7 @@ include_directories(include ${catkin_INCLUDE_DIRS}) # Libraries # cmake-lint: disable=C0301 add_library(${PROJECT_NAME} - src/config/unit_conversions.cc + src/config/value_with_unit.cc src/data_structure/volumetric/hashed_blocks.cc src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc diff --git a/libraries/wavemap/include/wavemap/config/config_base.h b/libraries/wavemap/include/wavemap/config/config_base.h index 364cea182..ddf285483 100644 --- a/libraries/wavemap/include/wavemap/config/config_base.h +++ b/libraries/wavemap/include/wavemap/config/config_base.h @@ -1,9 +1,11 @@ #ifndef WAVEMAP_CONFIG_CONFIG_BASE_H_ #define WAVEMAP_CONFIG_CONFIG_BASE_H_ +#include + #include "wavemap/config/param.h" #include "wavemap/config/param_checks.h" -#include "wavemap/config/unit_conversions.h" +#include "wavemap/config/value_with_unit.h" #include "wavemap/utils/type_utils.h" namespace wavemap { @@ -36,14 +38,15 @@ struct ConfigBase { } // Setup the introspective member metadata map - using MemberTypes = param::PrimitiveValueTypes::Append; + using MemberTypes = param::PrimitiveValueTypes::Append< + Meters, Radians, Pixels, + Seconds>::Append; using MemberPointer = inject_type_list_as_member_ptrs_t; struct MemberMetadata { param::Name name; MemberPointer ptr; - std::optional unit = std::nullopt; }; static constexpr size_t kNumMembers = num_members; using MemberMap = const std::array; @@ -56,7 +59,9 @@ struct ConfigBase { } // Load config from param map - static ConfigDerivedT from(const param::Map& params); + static std::optional from(const param::Value& params); + static std::optional from(const param::Value& params, + const std::string& subconfig_name); // Comparison operators friend bool operator==(const ConfigDerivedT& lhs, const ConfigDerivedT& rhs) { diff --git a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h index ae6bbd086..0a04a245a 100644 --- a/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/config_base_inl.h @@ -60,92 +60,77 @@ namespace wavemap { namespace detail { // Loader for PrimitiveValueTypes -// NOTE: We exclude FloatingPoints since these are specialized separately. -template < - typename ConfigDerivedT, typename MemberPtrT, - typename ConfigValueT = member_type_t>, - std::enable_if_t && - !std::is_same_v, - bool> = true> +template >, + std::enable_if_t, + bool> = true> void loadParam(const param::Name& param_name, const param::Value& param_value, - ConfigDerivedT& config, MemberPtrT config_member_ptr, - const std::optional& /*config_member_unit*/) { + ConfigDerivedT& config, MemberPtrT config_member_ptr) { ConfigValueT& config_value = config.*config_member_ptr; if (param_value.holds()) { - config_value = ConfigValueT{param_value.get()}; - } else { - LOG(WARNING) << "Type of param " << param_name - << " does not match type of corresponding config value."; - } -} - -// Loader for floating point types, which support unit conversions -template -void loadParam(const param::Name& param_name, const param::Value& param_value, - ConfigDerivedT& config, - FloatingPoint ConfigDerivedT::*config_member_ptr, - const std::optional& config_member_unit) { - FloatingPoint& config_value = config.*config_member_ptr; - if (config_member_unit.has_value()) { - // TODO(victorr): Extend toUnit to auto-convert ints to floats - config_value = param::convert::toUnit( - param_value, config_member_unit.value(), config_value); - } else { - if (param_value.holds()) { - config_value = param_value.get(); - } else if (param_value.holds()) { - config_value = static_cast(param_value.get()); - } else { - LOG(WARNING) << "Type of param " << param_name - << " does not match type of corresponding config value."; + config_value = ConfigValueT{param_value.get().value()}; + return; + } else if constexpr (std::is_same_v) { + if (const auto param_int = param_value.get(); param_int) { + // If the param_value and config_value's types do not match exactly, we + // still allow automatic conversions from ints to floats + // NOTE: This is avoids pesky, potentially confusing errors when setting + // whole numbers and forgetting the decimal point (e.g. 1 vs 1.0). + config_value = + ConfigValueT{static_cast(param_int.value())}; + return; } } -} -// Loader for types that define a "from" method, such as configs -template >, - decltype(ConfigValueT::from(std::declval()), - bool()) = true> -void loadParam(const param::Name& param_name, const param::Value& param_value, - ConfigDerivedT& config, MemberPtrT config_member_ptr, - const std::optional& /*config_member_unit*/) { - ConfigValueT& config_value = config.*config_member_ptr; - if (param_value.holds()) { - config_value = ConfigValueT::from(param_value.get()); - } else { - LOG(WARNING) << "Type of param " << param_name - << " does not match type of corresponding config value."; - } + LOG(WARNING) << "Type of param " << param_name + << " does not match the type of its corresponding config value. " + "Keeping default value \"" + << config_value << "\"."; } -// Loader for TypeSelector types +// Loader for types that define a "from" method, such as configs derived from +// ConfigBase, type IDs derived from TypeSelector, and config values derived +// from ValueWithUnits template >, - decltype(ConfigValueT::strToTypeId(std::declval()), + decltype(ConfigValueT::from(std::declval()), bool()) = true> void loadParam(const param::Name& param_name, const param::Value& param_value, - ConfigDerivedT& config, MemberPtrT config_member_ptr, - const std::optional& /*config_member_unit*/) { + ConfigDerivedT& config, MemberPtrT config_member_ptr) { ConfigValueT& config_value = config.*config_member_ptr; - if (param_value.holds()) { - config_value = ConfigValueT::strToTypeId(param_value.get()); + if (const auto read_value = ConfigValueT::from(param_value); read_value) { + config_value = read_value.value(); } else { - LOG(WARNING) << "Type of param " << param_name - << " does not match type of corresponding config value."; + // Report the error, and print the fallback (default) value that will be + // used instead if possible + if constexpr (has_to_str_member_fn_v) { + LOG(WARNING) << "Param " << param_name + << " could not be loaded. Keeping default value: " + << config_value.toStr() << "."; + } else { + LOG(WARNING) << "Param " << param_name + << " could not be loaded. Keeping default value."; + } } } } // namespace detail template -ConfigDerivedT +std::optional ConfigBase::from( - const param::Map& params) { + const param::Value& params) { + const auto param_map = params.get(); + if (!param_map) { + LOG(WARNING) << "Tried to load config from a param that is not of type Map " + "(dictionary). Will be ignored."; + return std::nullopt; + } + ConfigDerivedT config; const auto& member_map = ConfigDerivedT::memberMap; - for (const auto& param_kv : params) { + for (const auto& param_kv : param_map.value()) { const auto& param_name = param_kv.first; const auto& param_value = param_kv.second; @@ -165,9 +150,8 @@ ConfigBase::from( // If so, load it if (member_it != member_map.end()) { - const auto& unit = member_it->unit; auto param_loader = [&](auto&& ptr) { - detail::loadParam(param_name, param_value, config, ptr, unit); + detail::loadParam(param_name, param_value, config, ptr); }; std::visit(param_loader, member_it->ptr); continue; @@ -179,6 +163,21 @@ ConfigBase::from( return config; } + +template +std::optional +ConfigBase::from( + const param::Value& params, const std::string& subconfig_name) { + if (const auto subconfig_params = params.getChild(subconfig_name); + subconfig_params) { + return from(subconfig_params.value()); + } + + LOG(WARNING) << "Tried to load subconfig named " << subconfig_name + << ", but params contained no such key. Ignoring request."; + return std::nullopt; +} } // namespace wavemap #endif // WAVEMAP_CONFIG_IMPL_CONFIG_BASE_INL_H_ diff --git a/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h b/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h index 4e198865b..926013bfe 100644 --- a/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h +++ b/libraries/wavemap/include/wavemap/config/impl/type_selector_inl.h @@ -1,7 +1,9 @@ #ifndef WAVEMAP_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ #define WAVEMAP_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ +#include #include +#include namespace wavemap { template @@ -17,28 +19,69 @@ TypeSelector::strToTypeId(const std::string& name) { } template -DerivedNamedTypeSetT TypeSelector::fromParamMap( - const param::Map& params, std::string& error_msg) { - if (!param::map::hasKey(params, param::kTypeSelectorKey)) { - error_msg = "Type param is not set."; - return kInvalidTypeId; - } +std::optional TypeSelector::from( + const param::Value& params) { + // Read the type name from params + std::string type_name; + if (params.holds()) { + // If the param is of type string, read the name directly + type_name = params.get().value(); + } else if (params.holds()) { + // If the param is of type Map, try to read the name from its "type" subkey + const auto type_param = params.getChild(param::kTypeSelectorKey); + if (!type_param) { + LOG(WARNING) << "Nested type name (\"" << param::kTypeSelectorKey + << "\") is not set."; + return std::nullopt; + } - if (!param::map::keyHoldsValue(params, - param::kTypeSelectorKey)) { - error_msg = "Type param is not a string."; - return kInvalidTypeId; + if (!type_param->holds()) { + LOG(WARNING) << "Nested type name (\"" << param::kTypeSelectorKey + << "\") is not a string."; + return std::nullopt; + } + + type_name = type_param->get().value(); + } else { + LOG(WARNING) << "Type names can only be read from params of type string, " + "or from param maps (dictionary) with a subkey named \"" + << param::kTypeSelectorKey << "\"."; + return std::nullopt; } - const auto type_name = - param::map::keyGetValue(params, param::kTypeSelectorKey); + // Match the type name to a type id DerivedNamedTypeSetT type_id(type_name); if (!type_id.isValid()) { - error_msg = "Type param does not match a known type."; + LOG(WARNING) + << "Value of type name param \"" << param::kTypeSelectorKey << "\": \"" + << type_name + << "\" does not match a known type name. Supported type names are [" + << std::accumulate(std::next(DerivedNamedTypeSetT::names.cbegin()), + DerivedNamedTypeSetT::names.cend(), + std::string(DerivedNamedTypeSetT::names[0]), + [](auto str, const auto& el) -> std::string { + return std::move(str) + ", " + std::string(el); + }) + << "]."; + return std::nullopt; } return type_id; } + +template +std::optional TypeSelector::from( + const param::Value& params, const std::string& subconfig_name) { + if (const auto subconfig_params = params.getChild(subconfig_name); + subconfig_params) { + return from(subconfig_params.value()); + } + + LOG(WARNING) << "Tried to load type name from subconfig named " + << subconfig_name + << ", but no such subconfig was found. Ignoring request."; + return std::nullopt; +} } // namespace wavemap #endif // WAVEMAP_CONFIG_IMPL_TYPE_SELECTOR_INL_H_ diff --git a/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h b/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h new file mode 100644 index 000000000..26d7a1101 --- /dev/null +++ b/libraries/wavemap/include/wavemap/config/impl/value_with_unit_inl.h @@ -0,0 +1,64 @@ +#ifndef WAVEMAP_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ +#define WAVEMAP_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ + +#include + +namespace wavemap { +template +std::string ValueWithUnit::toStr() const { + return std::to_string(value) + " [" + SiUnit::typeIdToStr(unit) + "]"; +} + +template +std::optional> ValueWithUnit::from( + const param::Value& params) { + const auto param_map = params.get(); + if (!param_map) { + LOG(WARNING) << "Tried to load a value with annotated units from a param " + "that is not of type Map (dictionary). Cannot perform " + "conversion to SI unit \"" + << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + return std::nullopt; + } + + if (param_map->size() != 1) { + LOG(WARNING) << "Value with unit should have exactly one key and value. " + "Cannot perform conversion to SI unit \"" + << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + return std::nullopt; + } + + const auto [param_unit, param_value] = *param_map->begin(); + + const auto si_unit_and_conversion = param::getUnitToSi(param_unit); + if (!si_unit_and_conversion) { + LOG(WARNING) << "Value has unknown unit \"" << param_unit + << "\". Cannot perform conversion to SI unit \"" + << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + return std::nullopt; + } + + const auto [si_unit, conversion_factor] = si_unit_and_conversion.value(); + if (si_unit != unit) { + LOG(WARNING) << "Value has unit \"" << param_unit + << "\" which cannot be converted to SI unit \"" + << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + return std::nullopt; + } + + if (const auto param_float = param_value.get(); param_float) { + return param_float.value() * conversion_factor; + } + + if (const auto param_int = param_value.get(); param_int) { + return static_cast(param_int.value()) * conversion_factor; + } + + LOG(WARNING) << "Value must be of type float or int. Cannot perform " + "conversion to SI unit \"" + << SiUnit::typeIdToStr(unit) << "\". Will be ignored."; + return std::nullopt; +} +} // namespace wavemap + +#endif // WAVEMAP_CONFIG_IMPL_VALUE_WITH_UNIT_INL_H_ diff --git a/libraries/wavemap/include/wavemap/config/param.h b/libraries/wavemap/include/wavemap/config/param.h index 925bfa8e5..cf802ed12 100644 --- a/libraries/wavemap/include/wavemap/config/param.h +++ b/libraries/wavemap/include/wavemap/config/param.h @@ -29,13 +29,28 @@ class ValueT { return std::holds_alternative(data_); } - // Read the value assuming it is of type T. Throws if this is not the case. - // NOTE: First check if the current value is actually of type T using - // holds(). + // Try to read the value using type T. Returns an empty optional if it fails. template - T get() const { - CHECK(holds()); - return std::get(data_); + std::optional get() const { + if (holds()) { + return std::get(data_); + } else { + return std::nullopt; + } + } + + // Methods to work with nested configs + inline bool hasChild(const Name& key) const { + if (const auto map = get(); map) { + return map.value().count(key); + } + return false; + } + std::optional getChild(const Name& key) const { + if (const auto map = get(); map) { + return map.value().at(key); + } + return std::nullopt; } private: @@ -46,29 +61,6 @@ using PrimitiveValueTypes = TypeList; using Value = inject_type_list_t; using Array = Value::Array; using Map = Value::Map; - -namespace map { -inline bool hasKey(const Map& map, const Name& key) { return map.count(key); } - -template -bool keyHoldsValue(const Map& map, const Name& key) { - return map.count(key) && map.at(key).template holds(); -} - -template -T keyGetValue(const Map& map, const Name& key) { - return map.at(key).template get(); -} - -template -T keyGetValue(const Map& map, const Name& key, T default_value) { - if (keyHoldsValue(map, key)) { - return map.at(key).template get(); - } else { - return default_value; - } -} -} // namespace map } // namespace wavemap::param #endif // WAVEMAP_CONFIG_PARAM_H_ diff --git a/libraries/wavemap/include/wavemap/config/param_checks.h b/libraries/wavemap/include/wavemap/config/param_checks.h index a2b412531..09ec91c53 100644 --- a/libraries/wavemap/include/wavemap/config/param_checks.h +++ b/libraries/wavemap/include/wavemap/config/param_checks.h @@ -25,15 +25,15 @@ namespace wavemap { #define IS_PARAM_GE(value, threshold, verbose) \ is_param>(value, threshold, verbose, NAMEOF(value), ">=") -template -bool is_param(T value, T threshold) { +template +bool is_param(A value, B threshold) { return ComparisonOp{}(value, threshold); } -template -bool is_param(T value, T threshold, bool verbose, const std::string& value_name, +template +bool is_param(A value, B threshold, bool verbose, const std::string& value_name, const std::string& comparison_op_name) { - if (is_param(value, threshold)) { + if (is_param(value, threshold)) { return true; } else { LOG_IF(WARNING, verbose) << "Param \"" << value_name << "\" is not " diff --git a/libraries/wavemap/include/wavemap/config/type_selector.h b/libraries/wavemap/include/wavemap/config/type_selector.h index 40951a490..77ff254ad 100644 --- a/libraries/wavemap/include/wavemap/config/type_selector.h +++ b/libraries/wavemap/include/wavemap/config/type_selector.h @@ -75,9 +75,10 @@ struct TypeSelector { static_cast(type_id) < DerivedTypeSelectorT::names.size(); } - // Convenience method to read the type from a param map - static DerivedTypeSelectorT fromParamMap(const param::Map& params, - std::string& error_msg); + // Convenience method to read the type from params + static std::optional from(const param::Value& params); + static std::optional from( + const param::Value& params, const std::string& subconfig_name); private: TypeId id_ = kInvalidTypeId; diff --git a/libraries/wavemap/include/wavemap/config/unit_conversions.h b/libraries/wavemap/include/wavemap/config/unit_conversions.h deleted file mode 100644 index 64c52648d..000000000 --- a/libraries/wavemap/include/wavemap/config/unit_conversions.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef WAVEMAP_CONFIG_UNIT_CONVERSIONS_H_ -#define WAVEMAP_CONFIG_UNIT_CONVERSIONS_H_ - -#include "wavemap/common.h" -#include "wavemap/config/param.h" -#include "wavemap/config/type_selector.h" - -namespace wavemap { -struct SiUnit : TypeSelector { - using TypeSelector::TypeSelector; - - enum Id : TypeId { kMeters, kRadians, kPixels, kSeconds }; - - static constexpr std::array names = {"meters", "radians", "pixels", - "seconds"}; -}; - -struct ValueWithUnit { - FloatingPoint value = kNaN; - SiUnit unit = SiUnit::kInvalidTypeId; -}; -inline ValueWithUnit operator*(FloatingPoint lhs, const ValueWithUnit& rhs) { - return {lhs * rhs.value, rhs.unit}; -} - -namespace param::convert { -ValueWithUnit toSiUnit(const Map& map); - -FloatingPoint toUnit(const Map& map, SiUnit unit); - -FloatingPoint toUnit(const Value& param, SiUnit unit, - FloatingPoint default_value); - -FloatingPoint toUnit(const Map& map, const Name& key, SiUnit unit, - FloatingPoint default_value); - -template -FloatingPoint toUnit(const Map& map) { - return toUnit(map, unit); -} - -template -FloatingPoint toUnit(const Value& param, FloatingPoint default_value) { - return toUnit(param, unit, default_value); -} - -template -FloatingPoint toUnit(const Map& map, const Name& key, - FloatingPoint default_value) { - return toUnit(map, key, unit, default_value); -} -} // namespace param::convert -} // namespace wavemap - -#endif // WAVEMAP_CONFIG_UNIT_CONVERSIONS_H_ diff --git a/libraries/wavemap/include/wavemap/config/value_with_unit.h b/libraries/wavemap/include/wavemap/config/value_with_unit.h new file mode 100644 index 000000000..f05d33be8 --- /dev/null +++ b/libraries/wavemap/include/wavemap/config/value_with_unit.h @@ -0,0 +1,86 @@ +#ifndef WAVEMAP_CONFIG_VALUE_WITH_UNIT_H_ +#define WAVEMAP_CONFIG_VALUE_WITH_UNIT_H_ + +#include + +#include "wavemap/common.h" +#include "wavemap/config/param.h" +#include "wavemap/config/type_selector.h" + +namespace wavemap { +struct SiUnit : TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kMeters, kRadians, kPixels, kSeconds }; + + static constexpr std::array names = {"meters", "radians", "pixels", + "seconds"}; +}; + +template +struct ValueWithUnit { + static constexpr SiUnit::Id kUnit = unit; + T value{}; + + // Constructors + ValueWithUnit() = default; + ValueWithUnit(T value) : value(value) {} // NOLINT + + // Assignment operator + ValueWithUnit& operator=(T rhs) { + value = rhs; + return *this; + } + + // Allow implicit conversions to the underlying type + operator T&() { return value; } + operator const T&() const { return value; } + + // Method to load from configs + static std::optional from(const param::Value& params); + + // Method to facilitate printing + std::string toStr() const; +}; + +/** + * Value expressed in meters. + * @tparam T The value's underlying scalar type. + */ +template +using Meters = ValueWithUnit; + +/** + * Value expressed in radians. + * @tparam T The value's underlying scalar type. + */ +template +using Radians = ValueWithUnit; + +/** + * Value expressed in pixels. + * @tparam T The value's underlying scalar type. + */ +template +using Pixels = ValueWithUnit; + +/** + * Value expressed in seconds. + * @tparam T The value's underlying scalar type. + */ +template +using Seconds = ValueWithUnit; + +namespace param { +struct ConversionToSi { + SiUnit::Id si_unit; + FloatingPoint conversion_factor; +}; + +std::optional getUnitToSi(const std::string& unit); +} // namespace param +} // namespace wavemap + +#include "wavemap/config/impl/value_with_unit_inl.h" + +#endif // WAVEMAP_CONFIG_VALUE_WITH_UNIT_H_ diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h index ab1492a53..d7d683d34 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h @@ -12,18 +12,30 @@ #include "wavemap/utils/int_math.h" namespace wavemap { +/** + * Config struct for the hashed chunked wavelet octree volumetric data + * structure. + */ struct HashedChunkedWaveletOctreeConfig : ConfigBase { static constexpr IndexElement kMaxSupportedTreeHeight = HashedChunkedWaveletOctreeBlock::kMaxSupportedTreeHeight; - FloatingPoint min_cell_width = 0.1f; + //! Maximum resolution of the map, set as the width of the smallest cell that + //! it can represent. + Meters min_cell_width = 0.1f; + //! Lower threshold for the occupancy values stored in the map, in log-odds. FloatingPoint min_log_odds = -2.f; + //! Upper threshold for the occupancy values stored in the map, in log-odds. FloatingPoint max_log_odds = 4.f; + //! Height of the octree in each hashed block. IndexElement tree_height = 6; - FloatingPoint only_prune_blocks_if_unused_for = 5.f; + //! Only prune blocks if they have not been updated for at least this amount + //! of time. Useful to avoid pruning blocks that are still being updated, + //! whose nodes would most likely directly be reallocated if pruned. + Seconds only_prune_blocks_if_unused_for = 5.f; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h index 5a4907190..ac2216443 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/hashed_wavelet_octree.h @@ -12,14 +12,25 @@ #include "wavemap/utils/int_math.h" namespace wavemap { +/** + * Config struct for the hashed wavelet octree volumetric data structure. + */ struct HashedWaveletOctreeConfig : ConfigBase { - FloatingPoint min_cell_width = 0.1f; + //! Maximum resolution of the map, set as the width of the smallest cell that + //! it can represent. + Meters min_cell_width = 0.1f; + //! Lower threshold for the occupancy values stored in the map, in log-odds. FloatingPoint min_log_odds = -2.f; + //! Upper threshold for the occupancy values stored in the map, in log-odds. FloatingPoint max_log_odds = 4.f; + //! Height of the octree in each hashed block. IndexElement tree_height = 6; - FloatingPoint only_prune_blocks_if_unused_for = 5.f; + //! Only prune blocks if they have not been updated for at least this amount + //! of time. Useful to avoid pruning blocks that are still being updated, + //! whose nodes would most likely directly be reallocated if pruned. + Seconds only_prune_blocks_if_unused_for = 5.f; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h index 99f3796a0..9be7bf247 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_base.h @@ -27,11 +27,18 @@ struct VolumetricDataStructureType : TypeSelector { "hashed_chunked_wavelet_octree"}; }; +/** + * Base config struct for volumetric data structures. + */ struct VolumetricDataStructureConfig : ConfigBase { - FloatingPoint min_cell_width = 0.1f; + //! Maximum resolution of the map, set as the width of the smallest cell that + //! it can represent. + Meters min_cell_width = 0.1f; + //! Lower threshold for the occupancy values stored in the map, in log-odds. FloatingPoint min_log_odds = -2.f; + //! Upper threshold for the occupancy values stored in the map, in log-odds. FloatingPoint max_log_odds = 4.f; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_factory.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_factory.h index cc62edf54..a13b7f1fe 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_factory.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_data_structure_factory.h @@ -9,13 +9,13 @@ namespace wavemap { class VolumetricDataStructureFactory { public: static VolumetricDataStructureBase::Ptr create( - const param::Map& params, + const param::Value& params, std::optional default_data_structure_type = std::nullopt); static VolumetricDataStructureBase::Ptr create( VolumetricDataStructureType data_structure_type, - const param::Map& params); + const param::Value& params); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_octree.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_octree.h index d3b5be89e..c636f7ce7 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_octree.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/volumetric_octree.h @@ -11,8 +11,11 @@ #include "wavemap/indexing/ndtree_index.h" namespace wavemap { +/** + * Config struct for the octree volumetric data structure. + */ struct VolumetricOctreeConfig : ConfigBase { - FloatingPoint min_cell_width = 0.1f; + Meters min_cell_width = 0.1f; FloatingPoint min_log_odds = -2.f; FloatingPoint max_log_odds = 4.f; diff --git a/libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h b/libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h index 40a8a6207..3bf1f860e 100644 --- a/libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h +++ b/libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h @@ -12,12 +12,20 @@ #include "wavemap/indexing/ndtree_index.h" namespace wavemap { +/** + * Config struct for the wavelet octree volumetric data structure. + */ struct WaveletOctreeConfig : ConfigBase { - FloatingPoint min_cell_width = 0.1f; + //! Maximum resolution of the map, set as the width of the smallest cell that + //! it can represent. + Meters min_cell_width = 0.1f; + //! Lower threshold for the occupancy values stored in the map, in log-odds. FloatingPoint min_log_odds = -2.f; + //! Upper threshold for the occupancy values stored in the map, in log-odds. FloatingPoint max_log_odds = 4.f; + //! Height of the octree used to store the map. IndexElement tree_height = 14; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h index 71c1406bc..9be0f08a5 100644 --- a/libraries/wavemap/include/wavemap/integrator/integrator_factory.h +++ b/libraries/wavemap/include/wavemap/integrator/integrator_factory.h @@ -10,11 +10,12 @@ namespace wavemap { class IntegratorFactory { public: static IntegratorBase::Ptr create( - const param::Map& params, VolumetricDataStructureBase::Ptr occupancy_map, + const param::Value& params, + VolumetricDataStructureBase::Ptr occupancy_map, std::optional default_integrator_type = std::nullopt); static IntegratorBase::Ptr create( - IntegratorType integrator_type, const param::Map& params, + IntegratorType integrator_type, const param::Value& params, VolumetricDataStructureBase::Ptr occupancy_map); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/constant_ray.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/constant_ray.h index 30db340f1..fc4f02fa5 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/constant_ray.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/constant_ray.h @@ -6,6 +6,9 @@ #include "wavemap/indexing/index_conversions.h" namespace wavemap { +/** + * Config struct for the constant ray measurement model. + */ struct ConstantRayConfig : ConfigBase { FloatingPoint log_odds_occupied = 0.85f; FloatingPoint log_odds_free = -0.4f; diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h index fcb7ff831..722fd8c26 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_beam.h @@ -13,13 +13,26 @@ #include "wavemap/utils/eigen_format.h" namespace wavemap { +/** + * Config struct for the continuous beam measurement model. + */ struct ContinuousBeamConfig : ConfigBase { - FloatingPoint angle_sigma = 0.f; - FloatingPoint range_sigma = 0.f; + //! Uncertainty along the angle axis. + Radians angle_sigma = 0.f; + //! Uncertainty along the range axis. + Meters range_sigma = 0.f; + //! Scale factor to apply to the continuous Bayesian occupancy model for cells + //! that are observed as free. This can, for example, be used to give a higher + //! weight to occupied updates than free updates. FloatingPoint scaling_free = 0.5f; + //! Scale factor to apply to the continuous Bayesian occupancy model for cells + //! that are observed as occupied. This can, for example, be used to give a + //! higher weight to occupied updates than free updates. FloatingPoint scaling_occupied = 0.5f; + //! Which neighboring beams to consider when computing a cell's measurement + //! update. BeamSelectorType beam_selector_type = BeamSelectorType::kAllNeighbors; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h index bdeabfbf8..5e91cb760 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/continuous_ray.h @@ -10,10 +10,20 @@ #include "wavemap/integrator/projective/update_type.h" namespace wavemap { +/** + * Config struct for the continuous ray measurement model. + */ struct ContinuousRayConfig : ConfigBase { - FloatingPoint range_sigma = 0.f; + //! Uncertainty along the range axis. + Meters range_sigma = 0.f; + //! Scale factor to apply to the continuous Bayesian occupancy model for cells + //! that are observed as free. This can, for example, be used to give a higher + //! weight to occupied updates than free updates. FloatingPoint scaling_free = 0.5f; + //! Scale factor to apply to the continuous Bayesian occupancy model for cells + //! that are observed as occupied. This can, for example, be used to give a + //! higher weight to occupied updates than free updates. FloatingPoint scaling_occupied = 0.5f; BeamSelectorType beam_selector_type = BeamSelectorType::kNearestNeighbor; diff --git a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_factory.h b/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_factory.h index 27250c482..f8040aec5 100644 --- a/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_factory.h +++ b/libraries/wavemap/include/wavemap/integrator/measurement_model/measurement_model_factory.h @@ -9,7 +9,7 @@ namespace wavemap { class MeasurementModelFactory { public: static MeasurementModelBase::Ptr create( - const param::Map& params, ProjectorBase::ConstPtr projection_model, + const param::Value& params, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, Image::ConstPtr beam_offset_image = nullptr, std::optional default_measurement_model_type = @@ -18,7 +18,7 @@ class MeasurementModelFactory { static MeasurementModelBase::Ptr create( MeasurementModelType measurement_model_type, ProjectorBase::ConstPtr projection_model, Image<>::ConstPtr range_image, - Image::ConstPtr beam_offset_image, const param::Map& params); + Image::ConstPtr beam_offset_image, const param::Value& params); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/circular_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/circular_projector.h index cda012978..1fdd4e308 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/circular_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/circular_projector.h @@ -5,9 +5,16 @@ #include "wavemap/config/config_base.h" namespace wavemap { +/** + * Config struct for the circular projection model. + */ struct CircularProjectorConfig : ConfigBase { - FloatingPoint min_angle = 0.f; - FloatingPoint max_angle = 0.f; + //! Minimum angle along this axis. + Radians min_angle = 0.f; + //! Maximum angle along this axis. + Radians max_angle = 0.f; + //! Resolution of the image along this axis, + //! set as the number of cells along the axis. IndexElement num_cells = 0; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h index 322752486..0a10df089 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/ouster_projector.h @@ -10,12 +10,23 @@ #include "wavemap/utils/approximate_trigonometry.h" namespace wavemap { +/** + * Config struct for the ouster LiDAR projection model. + */ struct OusterProjectorConfig : ConfigBase { + //! Properties of the projection model along the elevation axis. CircularProjectorConfig elevation; + //! Properties of the projection model along the azimuth axis. CircularProjectorConfig azimuth; - FloatingPoint lidar_origin_to_beam_origin = 0.02767f; - FloatingPoint lidar_origin_to_sensor_origin_z_offset = 0.03618f; + //! Offset between the Ouster LiDAR frame's origin and the laser beam's start + //! point (radial direction). For illustrations and additional information, + //! please see the Ouster sensor's manual. + Meters lidar_origin_to_beam_origin = 0.02767f; + //! Offset between the Ouster sensor and LiDAR frame's origins (z-direction). + //! For illustrations and additional information, please see the Ouster + //! sensor's manual. + Meters lidar_origin_to_sensor_origin_z_offset = 0.03618f; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h index 63df4a427..93e3c910f 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/pinhole_camera_projector.h @@ -7,13 +7,26 @@ #include "wavemap/integrator/projection_model/projector_base.h" namespace wavemap { +/** + * Config struct for the pinhole camera projection model. + */ struct PinholeCameraProjectorConfig : ConfigBase { + //! The image's width in pixels. IndexElement width = 0; + //! The image's height in pixels. IndexElement height = 0; + //! Fx according to ROS' CameraInfo convention: + //! http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html. FloatingPoint fx = 0.f; + //! Fy according to ROS' CameraInfo convention: + //! http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html. FloatingPoint fy = 0.f; + //! Cx according to ROS' CameraInfo convention: + //! http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html. FloatingPoint cx = 0.f; + //! Cy according to ROS' CameraInfo convention: + //! http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html. FloatingPoint cy = 0.f; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h index bfbba7d71..88a6ae907 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_base.h @@ -6,7 +6,7 @@ #include "wavemap/common.h" #include "wavemap/config/type_selector.h" -#include "wavemap/config/unit_conversions.h" +#include "wavemap/config/value_with_unit.h" #include "wavemap/data_structure/aabb.h" namespace wavemap { diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_factory.h b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_factory.h index 3f3cdeba9..1431c3cbf 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/projector_factory.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/projector_factory.h @@ -7,11 +7,11 @@ namespace wavemap { class ProjectorFactory { public: static ProjectorBase::Ptr create( - const param::Map& params, + const param::Value& params, std::optional default_projector_type = std::nullopt); static ProjectorBase::Ptr create(ProjectorType projector_type, - const param::Map& params); + const param::Value& params); }; } // namespace wavemap diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h index ef720cfa5..5cb5a1470 100644 --- a/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h +++ b/libraries/wavemap/include/wavemap/integrator/projection_model/spherical_projector.h @@ -10,9 +10,14 @@ #include "wavemap/utils/approximate_trigonometry.h" namespace wavemap { +/** + * Config struct for the spherical projection model. + */ struct SphericalProjectorConfig : ConfigBase { + //! Properties of the projection model along the elevation axis. CircularProjectorConfig elevation; + //! Properties of the projection model along the azimuth axis. CircularProjectorConfig azimuth; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h index d10ca6a3a..4e8460ec5 100644 --- a/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/projective/projective_integrator.h @@ -12,11 +12,27 @@ #include "wavemap/integrator/projection_model/projector_base.h" namespace wavemap { +/** + * Config struct for projective integrators. + */ struct ProjectiveIntegratorConfig : ConfigBase { - FloatingPoint min_range = 0.5f; - FloatingPoint max_range = 20.f; - + //! Minimum range measurements should have to be considered. + //! Measurements below this threshold are ignored. + Meters min_range = 0.5f; + //! Maximum range up to which to update the map. + //! Measurements that exceed this range are used as free-space beams, + //! up to the maximum range. + Meters max_range = 20.f; + + //! Maximum resolution to use for this integrator, set as the height at which + //! to terminate the coarse-to-fine measurement updates. Defaults to 0 (max + //! res). Can be set to 1 for 1/2 of the max resolution, to 2 for 1/4 of the + //! max resolution, etc.This can be used to fuse multiple inputs with + //! different maximum resolutions into a single map. NdtreeIndexElement termination_height = 0; + //! The update error threshold at which the coarse-to-fine measurement + //! integrator is allowed to terminate, in log-odds. For more information, + //! please refer to: https://www.roboticsproceedings.org/rss19/p065.pdf. FloatingPoint termination_update_error = 0.1f; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h index 9f3735baa..894010dfd 100644 --- a/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h +++ b/libraries/wavemap/include/wavemap/integrator/ray_tracing/ray_tracing_integrator.h @@ -9,9 +9,12 @@ #include "wavemap/iterator/ray_iterator.h" namespace wavemap { +/** + * Config struct for the ray-tracing integrator. + */ struct RayTracingIntegratorConfig : ConfigBase { - FloatingPoint min_range = 0.5f; - FloatingPoint max_range = 20.f; + Meters min_range = 0.5f; + Meters max_range = 20.f; static MemberMap memberMap; diff --git a/libraries/wavemap/include/wavemap/utils/type_utils.h b/libraries/wavemap/include/wavemap/utils/type_utils.h index b4856f81b..de901f3aa 100644 --- a/libraries/wavemap/include/wavemap/utils/type_utils.h +++ b/libraries/wavemap/include/wavemap/utils/type_utils.h @@ -64,6 +64,16 @@ struct inject_type_list_as_member_ptrs> { template