From 9a81b1d63314894896e9ee0ca6801dc7751e1d43 Mon Sep 17 00:00:00 2001 From: kairenw <44489146+kairenw@users.noreply.github.com> Date: Wed, 29 Mar 2023 09:08:28 -0700 Subject: [PATCH] Concurrent FW 2.5 release (#500) Includes: * Introduction of IndexedPcapReader, seek metehod to PcapReader * Moving port guessing logic from Python into C++ * `SensorHTTP` class made public * experimental mtp_init_client to init client for multicast support * Support of future DF sensor line metadata via introduction of fps and changes to metadata parsing * Numerous viz changes including introducing palettes, ImageMode and CloudMode * bugfix on application of autoleveling for viz * FindPcap updates * FW 2.5 test metadata assets * Change default metadata output to nonlegacy --- CHANGELOG.rst | 38 +- CMakeLists.txt | 5 +- cmake/FindPcap.cmake | 24 + conan/test_package/conanfile.py | 1 + conanfile.py | 5 +- docs/Doxyfile | 1 - docs/conf.py | 8 +- docs/cpp/ouster_client/lidar_scan.rst | 2 +- docs/cpp/ouster_pcap/os_pcap.rst | 6 +- docs/index.rst | 6 + .../migration/migration-20220927-20230114.rst | 101 +++ .../migration/migration-20230114-20230403.rst | 130 +++ docs/overview.rst | 41 +- docs/python/devel.rst | 2 +- docs/python/quickstart.rst | 2 +- docs/python/viz/viz-api-tutorial.rst | 2 +- docs/python/viz/viz-run.rst | 9 +- ouster_client/include/ouster/client.h | 6 +- ouster_client/include/ouster/lidar_scan.h | 125 ++- .../{src => include/ouster}/sensor_http.h | 2 - ouster_client/include/ouster/types.h | 11 +- ouster_client/src/client.cpp | 7 +- ouster_client/src/http_client.h | 4 +- ouster_client/src/image_processing.cpp | 5 + ouster_client/src/lidar_scan.cpp | 142 ++-- ouster_client/src/parsing.cpp | 21 +- ouster_client/src/sensor_http.cpp | 2 +- ouster_client/src/sensor_http_imp.h | 2 +- ouster_client/src/sensor_tcp_imp.h | 7 +- ouster_client/src/types.cpp | 154 +++- ouster_pcap/CMakeLists.txt | 10 +- .../include/ouster/indexed_pcap_reader.h | 88 ++ ouster_pcap/include/ouster/os_pcap.h | 200 ++++- ouster_pcap/include/ouster/pcap.h | 204 +++++ ouster_pcap/src/indexed_pcap_reader.cpp | 92 +++ ouster_pcap/src/os_pcap.cpp | 562 +++++++------ ouster_pcap/src/pcap.cpp | 356 ++++++++ ouster_viz/include/ouster/point_viz.h | 148 +++- ouster_viz/src/cloud.cpp | 30 +- ouster_viz/src/cloud.h | 1 + ouster_viz/src/colormaps.h | 779 ++++++++++++++++++ ouster_viz/src/common.h | 54 +- ouster_viz/src/glfw.cpp | 25 +- ouster_viz/src/image.cpp | 37 +- ouster_viz/src/image.h | 4 + ouster_viz/src/misc.cpp | 1 + ouster_viz/src/point_viz.cpp | 144 +++- python/Dockerfile | 5 + python/setup.py | 7 +- python/src/cpp/_client.cpp | 140 ++-- python/src/cpp/_pcap.cpp | 163 +++- python/src/cpp/_viz.cpp | 190 ++++- python/src/ouster/client/__init__.py | 4 + python/src/ouster/client/_client.pyi | 35 +- python/src/ouster/client/_digest.py | 9 +- python/src/ouster/client/core.py | 32 +- python/src/ouster/client/data.py | 45 +- python/src/ouster/pcap/__init__.py | 1 - python/src/ouster/pcap/_pcap.pyi | 66 +- python/src/ouster/pcap/pcap.py | 160 +--- python/src/ouster/sdk/_viz.pyi | 26 +- python/src/ouster/sdk/convert_to_legacy.py | 29 + python/src/ouster/sdk/simple_viz.py | 18 +- python/src/ouster/sdk/util.py | 80 ++ python/src/ouster/sdk/viz.py | 482 ++++++++--- python/tests/__init__.py | 0 python/tests/test_core.py | 11 +- python/tests/test_data.py | 8 +- python/tests/test_metadata.py | 3 +- python/tests/test_pcap.py | 272 +++++- python/tests/test_viz.py | 338 +++++++- tests/CMakeLists.txt | 12 + tests/bcompat_meta_json_test.cpp | 10 +- tests/bcompat_sensor_info_data.h | 155 +++- tests/lidar_scan_test.cpp | 2 - tests/metadata/2_5_0_os-992146000760-128.json | 525 ++++++++++++ .../2_5_0_os-992146000760-128_legacy.json | 480 +++++++++++ tests/metadata/3_0_1_os-122246000293-128.json | 525 ++++++++++++ .../3_0_1_os-122246000293-128_legacy.json | 480 +++++++++++ tests/pcap_test.cpp | 160 ++++ ...-0-32-U1_v2.2.0_1024x10-single-packet.pcap | Bin 0 -> 8530 bytes 81 files changed, 7033 insertions(+), 1046 deletions(-) create mode 100644 cmake/FindPcap.cmake create mode 100644 docs/migration/migration-20220927-20230114.rst create mode 100644 docs/migration/migration-20230114-20230403.rst rename ouster_client/{src => include/ouster}/sensor_http.h (97%) create mode 100644 ouster_pcap/include/ouster/indexed_pcap_reader.h create mode 100644 ouster_pcap/include/ouster/pcap.h create mode 100644 ouster_pcap/src/indexed_pcap_reader.cpp create mode 100644 ouster_pcap/src/pcap.cpp create mode 100644 python/src/ouster/sdk/convert_to_legacy.py create mode 100644 python/src/ouster/sdk/util.py create mode 100644 python/tests/__init__.py create mode 100644 tests/metadata/2_5_0_os-992146000760-128.json create mode 100644 tests/metadata/2_5_0_os-992146000760-128_legacy.json create mode 100644 tests/metadata/3_0_1_os-122246000293-128.json create mode 100644 tests/metadata/3_0_1_os-122246000293-128_legacy.json create mode 100644 tests/pcap_test.cpp create mode 100644 tests/pcaps/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap diff --git a/CHANGELOG.rst b/CHANGELOG.rst index c2de8ba8..c37ae89b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,12 +2,40 @@ Changelog ========= -[unreleased] -============ +[20230403] +========== + +* Default metadata output across all functionality has been switched to the non-legacy format ouster_client ------------- -- Added a new method ``mtp_init_client`` to init the client with multicast support (experimental). +* Added a new method ``mtp_init_client`` to init the client with multicast support (experimental). +* the class ``SensorHttp`` which provides easy access to REST APIs of the sensor has been made public + under the ``ouster::sensor::util`` namespace. +* breaking change: get_metadata defaults to outputting non-legacy metadata +* add debug five_word profile which will be removed later +* breaking change: remove deprecations on LidarScan + +ouster_viz +---------- +* update viz camera with other objects in draw + +ouster_pcap +----------- +* add seek method to PcapReader +* add port guessing logic + +python +------ +* introduce utility to convert nonlegacy metadata to legacy +* use resolve_metadata to find unspecified metadata for simple-viz +* remove port guessing logic in favor of using new C++ ouster_pcap port guessing functionality +* add soft-id-check to skip the init_id/sn check for lidar_packets with metadata + +Numerous changes to SimpleViz and LidarScanViz including: +* expose visible in viz to Python +* introduce ImageMode and CloudMode +* bugfix: remove spurious sqrt application to autoleveled images [20230114] @@ -90,10 +118,6 @@ ouster_client * allow vcpkg configuration via environment variables * fix a bug in sensor_config struct equality comparison operator -ouster_pcap ------------ -* fix incorrect encapsulation protocol being reported in ``packet_info`` - ouster_viz ---------- * clean up GL context logic to avoid errors on window/intel UHD graphics diff --git a/CMakeLists.txt b/CMakeLists.txt index 93b9a310..76a051a8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,10 +7,10 @@ include(DefaultBuildType) include(VcpkgEnv) # ==== Project Name ==== -project(ouster_example VERSION 20230114) +project(ouster_example VERSION 20230403) # generate version header -set(OusterSDK_VERSION_STRING 0.7.1) +set(OusterSDK_VERSION_STRING 0.8.1) include(VersionGen) # ==== Options ==== @@ -89,7 +89,6 @@ set(CPACK_DEBIAN_PACKAGE_NAME ouster-sdk) set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT) set(CPACK_DEBIAN_PACKAGE_DEPENDS "libjsoncpp-dev, libeigen3-dev, libtins-dev, libglfw3-dev, libglew-dev, libspdlog-dev") - include(CPack) # ==== Install ==== diff --git a/cmake/FindPcap.cmake b/cmake/FindPcap.cmake new file mode 100644 index 00000000..ec325f38 --- /dev/null +++ b/cmake/FindPcap.cmake @@ -0,0 +1,24 @@ +find_package(PkgConfig QUIET) +if(PKG_CONFIG_FOUND) + pkg_check_modules(PC_PCAP QUIET libpcap) + if(PC_PCAP_FOUND) + set(PCAP_VERSION_STRING ${PC_PCAP_VERSION}) + endif() +endif() + +find_path(PCAP_INCLUDE_DIR + NAMES pcap.h + HINTS ${PC_PCAP_INCLUDE_DIRS}) + +find_library(PCAP_LIBRARY + NAMES pcap pcap_static wpcap + HINTS ${PC_PCAP_LIBRARY_DIRS}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Pcap + REQUIRED_VARS PCAP_LIBRARY PCAP_INCLUDE_DIR + VERSION_VAR PCAP_VERSION_STRING) + +mark_as_advanced( + PCAP_INCLUDE_DIR + PCAP_LIBRARY) diff --git a/conan/test_package/conanfile.py b/conan/test_package/conanfile.py index 304bd30d..a52cf82b 100644 --- a/conan/test_package/conanfile.py +++ b/conan/test_package/conanfile.py @@ -2,6 +2,7 @@ from conans import ConanFile, CMake, tools + class OusterSDKTestConan(ConanFile): settings = "os", "compiler", "build_type", "arch" generators = "cmake_paths", "cmake_find_package" diff --git a/conanfile.py b/conanfile.py index 5e36fd81..26fd9018 100644 --- a/conanfile.py +++ b/conanfile.py @@ -4,6 +4,7 @@ from pprint import pformat + class OusterSDKConan(ConanFile): name = "ouster_sdk" license = "BSD 3-Clause License" @@ -48,7 +49,7 @@ class OusterSDKConan(ConanFile): # https://docs.conan.io/en/1.51/howtos/capture_version.html#how-to-capture-package-version-from-text-or-build-files def set_version(self): content = tools.load(os.path.join(self.recipe_folder, "CMakeLists.txt")) - version = re.search("set\(OusterSDK_VERSION_STRING (.*)\)", content).group(1) + version = re.search(r"set\(OusterSDK_VERSION_STRING (.*)\)", content).group(1) self.version = version.strip() def config_options(self): @@ -65,7 +66,7 @@ def requirements(self): self.requires("eigen/3.4.0") self.requires("jsoncpp/1.9.5") self.requires("spdlog/1.10.0") - self.requires("libcurl/7.82.0") + self.requires("libcurl/7.84.0") if self.options.build_pcap: self.requires("libtins/4.3") diff --git a/docs/Doxyfile b/docs/Doxyfile index 23288f7a..d5e03aa1 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -845,7 +845,6 @@ WARN_LOGFILE = INPUT = ../ouster_client \ ../ouster_pcap \ ../ouster_viz \ - ../ouster_ros # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses diff --git a/docs/conf.py b/docs/conf.py index 9352b51c..da693219 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -22,7 +22,6 @@ from string import Template import shutil import os -import sys import re project = 'Ouster Sensor SDK' @@ -40,13 +39,15 @@ raise RuntimeError("Could not guess OUSTER_SDK_PATH") print(OUSTER_SDK_PATH) + # https://packaging.python.org/en/latest/guides/single-sourcing-package-version/ def parse_version(): with open(os.path.join(OUSTER_SDK_PATH, 'CMakeLists.txt')) as listfile: content = listfile.read() - groups = re.search("set\(OusterSDK_VERSION_STRING ([^-\)]+)(-(.*))?\)", content) + groups = re.search(r"set\(OusterSDK_VERSION_STRING ([^-\)]+)(-(.*))?\)", content) return groups.group(1) + (groups.group(3) or "") + # The full version, including alpha/beta/rc tags version = release = parse_version() @@ -203,13 +204,14 @@ def do_doxygen_generate_xml(app): app.config["breathe_projects"].update( {name: os.path.join(doxygen_output_dir, path)}) + def do_doxygen_temp_cleanup(app, exception): if "temp_doxy_file_dir" in app.config: shutil.rmtree(app.config["temp_doxy_file_dir"]) + def setup(app): # Add a hook for generating doxygen xml and cleaning up app.connect("builder-inited", do_doxygen_generate_xml) app.connect("build-finished", do_doxygen_temp_cleanup) - diff --git a/docs/cpp/ouster_client/lidar_scan.rst b/docs/cpp/ouster_client/lidar_scan.rst index 66648c23..a046ae12 100644 --- a/docs/cpp/ouster_client/lidar_scan.rst +++ b/docs/cpp/ouster_client/lidar_scan.rst @@ -29,7 +29,7 @@ XYZLut .. doxygenstruct:: ouster::XYZLut :members: -.. doxygenfunction:: ouster::make_xyz_lut(size_t w, size_t h, double range_unit, double lidar_origin_to_beam_origin_mm, const mat4d& transform, const std::vector& azimuth_angles_deg, const std::vector& altitude_angles_deg) +.. doxygenfunction:: ouster::make_xyz_lut(size_t w, size_t h, double range_unit, const mat4d& beam_to_lidar_transform, const mat4d& transform, const std::vector& azimuth_angles_deg, const std::vector& altitude_angles_deg) .. doxygenfunction:: ouster::make_xyz_lut(const sensor::sensor_info& sensor) diff --git a/docs/cpp/ouster_pcap/os_pcap.rst b/docs/cpp/ouster_pcap/os_pcap.rst index 2af04aaf..43cbb2b6 100644 --- a/docs/cpp/ouster_pcap/os_pcap.rst +++ b/docs/cpp/ouster_pcap/os_pcap.rst @@ -33,10 +33,12 @@ Functions .. doxygenfunction:: ouster::sensor_utils::read_packet -.. doxygenfunction:: ouster::sensor_utils::record_initialize +.. doxygenfunction:: ouster::sensor_utils::record_initialize( const std::string& file, const std::string& src_ip, const std::string& dst_ip, int frag_size, bool use_sll_encapsulation = false) + +.. doxygenfunction:: ouster::sensor_utils::record_initialize(const std::string& file, int frag_size, bool use_sll_encapsulation = false); .. doxygenfunction:: ouster::sensor_utils::record_uninitialize -.. doxygenfunction:: ouster::sensor_utils::record_packet(record_handle& handle, const packet_info& info, const uint8_t* buf, size_t buffer_size) +.. doxygenfunction:: ouster::sensor_utils::record_packet(record_handle& handle, const std::string& src_ip, const std::string& dst_ip, int src_port, int dst_port, const uint8_t* buf, size_t buffer_size, uint64_t microsecond_timestamp) diff --git a/docs/index.rst b/docs/index.rst index 94a75662..c7606d5b 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -38,6 +38,12 @@ C++ API Reference Changelog +.. toctree:: + :caption: Migration Guides + :hidden: + + Migrating from 20220927/0.5.1 to 20230114/0.7.1 + Migrating from 20230114/0.7.1 to 20230403/0.8.1 .. FAQ diff --git a/docs/migration/migration-20220927-20230114.rst b/docs/migration/migration-20220927-20230114.rst new file mode 100644 index 00000000..89a418a4 --- /dev/null +++ b/docs/migration/migration-20220927-20230114.rst @@ -0,0 +1,101 @@ +=============================================== +Migration from 20220927/0.5.1 to 20230114/0.7.1 +=============================================== + +The 20230114 release, which corresponds to Python SDK 0.7.1, brings a few breaking changes +introduced to support the newest FW of the sensor, FW 3.0. The changes and how to mitigate them are +summarized here. + +Signal Multiplier +----------------- + +FW 3.0 allows setting the signal multiplier to non-int values 0.25 and 0.5. As such, we have changed +the type of the ``sensor_config`` struct member ``signal_multiplier`` to double. + +Old saved config jsons will continue to load properly. + +XYZLut and ``sensor_info`` +-------------------------- + +The ``sensor_info`` struct now contains the ``beam_to_lidar_transform`` specifying the relationship +between the beam coordinate frame and lidar coordinate frame. The double +``lidar_origin_to_beam_origin_mm`` which corresponds to the (0, 3) element of the +``beam_to_lidar_transform`` has not been removed. + +The ``make_xyz_lut`` function now takes a ``mat4d`` transformation specifying the relation between +the beam and lidar coordinate frames, as opposed to the previous double which assumed an Identity +rotation. + +Users using pre-FW3.0 sensors with the latest SDK ``make_xyz_lut`` overload which accepts a +``sensor_info`` need not change anything, as the ``sensor_info`` struct automatically derives and +self-populates with the appropriate ``beam_to_lidar_transform``. + +Default Parameters in init_client +--------------------------------- + +The shortform C++ ``init_client`` and Python ``client.Sensor()`` no longer have default parameters. +Since they do not configure the sensor, it didn't make sense to default to any value for any +parameters. Users must provide the ports and hostname explicitly now. + +Timeout Improvements +-------------------- + +The Python Scans interface timeout parameter has been changed from None to 1 second by default to +avoid confusing hanging behavior when timing out is appropriate. The default timeout has also been +changed to 2 seconds across the board. + +Notes for the future +-------------------- + +For customers who know they will continue to upgrade their version of the SDK, we also wish to +highlight some upcoming breaking changes. + +Default Metadata Format ++++++++++++++++++++++++ +First, the next release will default writing the non-legacy metadata format as opposed to the +current format, also known as the legacy metadata format. The SDK will continue to read the legacy +format, i.e., it will continue to read old recorded data), and it will also be able to produce the +legacy format if the parameter ``legacy=true`` is specified to the ``get_metadata`` function. + +Deprecations +++++++++++++ +We are going to start removing a number of deprecations in the ``ouster_client`` code with the next +release. We will document any such removals and how to migrate from them in this or the following +migration guide (whichever is relevant). + + - ``LidarScan::N_FIELDS``: The number of fields in a profile has varied since the introduction of + eUDP profiles. To find the number of fields in your scan, we suggest using the iterator to loop + over and count the number of fields. + + + - ``LidarScan::Field``: use ``sensor::ChanField`` instead. + + - ``LidarScan::BlockHeader``: ``BlockHeaders``, structs consisting of the ``timestamp``, + ``encoder`` ticks, and ``status`` which corresponded to a single column of measurements + (identifiable by``measurement_id``), have been deprecated in favor of ``Headers``, which are + Eigen Arrays of length equivalent to the # of columns in a scan, with the ith element being the + value from the ith measurement. As such, where once you might write: + + .. code:: + + auto n_invalid = std::count_if( scans[i].headers.begin(), scans[i].headers.end(), [](const + LidarScan::BlockHeader& h) { return h.status != 0xffffffff; }); + + you should now use the ``status()`` function: + + .. code:: + + auto status = scan.status(); auto n_invalid = std::count_if(status.data(), status.data() + + status.size(), [](const uint32_t s) { return !(s & 0x01); }); + + Timestamps are now available through ``timestamp()``, also returning a ``Header``; and the information + contained in ``encoder`` is available through ``measurement_id()`` (see the next line item for the + conversion). + +- ``encoder``: Encoder counts were part of the LEGACY UDP profile, representing the azimuth angle as + a raw encoder count starting from 0 to a max value of 90111. It thus incremented 44 ticks per + azimuth angle in 2048 mode, 88 ticks in 1024 mode and 176 in 512 mode. To recover encoder + infcount, you can multiply the ``measurement_id`` by the number dictated by your lidar mode. We + would suggest, however, migrating to use simply ``measurement_id`` and multiplying by ``360 + degrees/ N`` where ``N`` is the number of columns in your mode (512, 1024, 2048) when you need the + azimuth, thus untying any sense of azimuth from the internal mechanicals of the Ouster sensor. diff --git a/docs/migration/migration-20230114-20230403.rst b/docs/migration/migration-20230114-20230403.rst new file mode 100644 index 00000000..dc2a7055 --- /dev/null +++ b/docs/migration/migration-20230114-20230403.rst @@ -0,0 +1,130 @@ +=============================================== +Migration from 20230114/0.7.1 to 20230403/0.8.1 +=============================================== + +The 20230403 release, which corresponds to Python SDK 0.8.1, brings a few breaking changes. The +changes and how to mitigate them are summarized here. + +Default Metadata Format ++++++++++++++++++++++++ +The default output format for the metadata has changed from the legacy to the non-legacy format. +However, the SDK continues to read the legacy format, and will therefore continue to able to read +old recorded data. + +To continue to output the legacy format with C++, set the parameter ``legacy`` to ``true`` when +calling the ``get_metadata`` function. + +To continue to output the legacy format with Python, set the parameter ``_legacy_format`` to +``True`` when instantiating a ``Sensor``. + +If you find that you have written data out in the non-legacy format and you wish to use the legacy +format instead, use the provided Python utility ``convert-meta-to-legacy``:: + + convert-meta-to-legacy + + +LidarScan deprecations +++++++++++++++++++++++ +A number of long-deprecated members of the LidarScan interface have been removed. These are: + + - ``LidarScan::N_FIELDS``: The number of fields in a profile has varied since the introduction of + eUDP profiles in FW 2.2. To find the number of fields in your scan, we suggest using the iterator + to loop over and count the number of fields. + + - ``LidarScan::Field``: use ``sensor::ChanField`` instead. + + - ``LidarScan::BlockHeader``: ``BlockHeaders``, structs consisting of the ``timestamp``, + ``encoder`` ticks, and ``status`` which corresponded to a single column of measurements + (identifiable by``measurement_id``), were deprecated in favor of ``Headers``, which are Eigen + Arrays of length equivalent to the # of columns in a scan, with the ith element being the value + from the ith measurement. As such, where once you might write:: + + auto n_invalid = std::count_if( scans[i].headers.begin(), scans[i].headers.end(), [](const + LidarScan::BlockHeader& h) { return h.status != 0xffffffff; }); + + you should now use the ``status()`` function:: + + auto status = scan.status(); + auto n_invalid = std::count_if(status.data(), status.data() + status.size(), [](const uint32_t s) { return !(s & 0x01); }); + + Timestamps are now available through ``timestamp()``, also returning a ``Header``; and the + information contained in ``encoder`` is available through ``measurement_id()`` (see the next + line item for the conversion). + + - ``encoder``: Encoder counts were part of the LEGACY UDP profile, representing the azimuth angle + as a raw encoder count starting from 0 to a max value of 90111. It thus incremented 44 ticks per + azimuth angle in 2048 mode, 88 ticks in 1024 mode and 176 in 512 mode. To recover encoder + infcount, you can multiply the ``measurement_id`` by the number dictated by your lidar mode. We + suggest, however, migrating to use simply ``measurement_id`` and multiplying by ``360 degrees/ + N`` where ``N`` is the number of columns in your mode (512, 1024, 2048) when you need the + azimuth, thus untying any sense of azimuth from the internal mechanicals of the Ouster sensor. + + +Notes for the future +-------------------- + +For customers who know they will continue to upgrade their version of the SDK, we also wish to +highlight some upcoming breaking changes. + +Dropped Support ++++++++++++++++ + +Python +~~~~~~ +Python 3.7 reaches its end of life on June 27th, 2023. We will likely stop producing Python 3.7 +``ouster-sdk`` wheels by November 2023. + +FW versions +~~~~~~~~~~~ +FW 2.0 will be 3 years old in November 2023, with numerous upgrades introduces in subsequent FWs +2.1-2.5, and FW 3.0. New versions of the Python SDK and new commits of ouster_example will likely +stop supporting direct communication with sensors running FW 2.0 by November 2023, although recorded +data in the pcap+json format will continue to be read and supported. + +Ubuntu 18.04 +~~~~~~~~~~~~ +Ubuntu 18.04 reaches its end of standard support in May 2023. We will likely stop supporting the C++ +and Python builds of ``ouster_example`` on 18.04 by November 2023. You will still be able to run +available Python wheels on your system if you so choose. + + +ROS +~~~ +As already announced in the ouster-ros repo, there will no longer be ROS melodic support past its +End of Life in May 2023. + +Future Dropped Support +~~~~~~~~~~~~~~~~~~~~~~ +Looking forward, we will be dropping support for Python versions, Ubuntu versions, and ROS distros +as they reach their end of standard support instead of supporting them past with a grace period. +This means, for instance, that we will stop supporting Ubuntu 20.04 in April 2025 and Python 3.8 +in October 2024. + + +FW deprecations, current and upcoming +------------------------------------- + +FW 2.5 +~~~~~~ +There is no need to upgrade to this latest release of the SDK in order to work with sensors running +the new FW 2.5 if you already updated to 20230114/0.7.1. However, please note that the default value +of ``udp_profile_lidar`` has been updated to the Single Return profile (``RNG19_RFL8_SIG15_NIR16``) +from the legacy profile (``LEGACY``). The legacy profile is still supported, but you will have to +change your sensor default via its configuration webpage or the HTTP or TCP APIs. + + +Future +~~~~~~ + +We will discuss future FW removals and deprecations and their impact if you are a user of the SDK. + +TCP API +....... +The planned removal of the TCP API in future firmwares should not affect SDK users who communciate +with the sensor only through SDK APIs, as we have already updated our code to use the HTTP API where +it is available. + +LEGACY Profile +.............. +The planned removal of the LEGACY profile in future firmwares also should not affect SDK users, as +it will just be a code pathway not used. diff --git a/docs/overview.rst b/docs/overview.rst index 909f1d4d..a4c9a03e 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -11,13 +11,46 @@ Quick links * :doc:`installation` * :doc:`python/quickstart` * :doc:`cpp/building` -* :doc:`ros/index` +* `Ouster ROS 1 driver`_ .. figure:: /images/simple-viz.png :align: center * :doc:`python/viz/index` +Compatibility with FW +--------------------- + +The Ouster SDK currently provides backwards compatibility with any FW 2.0 and later for all +releases. Older SDK versions are not, however, generally forward-compatible with later FW relesaes, +e.g., the SDK version 20210608 (ouster-sdk 0.2.0) is not compatible with FW 3.0. + +.. note:: + + Please note that compatibility does not indicate that upgrading between SDK versions can happen + seamlessly. For information on breaking changes between releases, please the `Changelog`_. + +The following table indicates the compatibility of each released SDK version and its FW compatibility: + +==================================== ======= ======= ======= ======= ======= ======= ======= ======= +SDK Tag (Release) / Python SDK FW 1.13 FW 2.0 FW 2.1 FW 2.2 FW 2.3 FW 2.4 FW 3.0 FW 2.5 +==================================== ======= ======= ======= ======= ======= ======= ======= ======= +C++ SDK 20230403 / Python SDK 0.8.1 no **yes** **yes** **yes** **yes** **yes** **yes** **yes** +C++ SDK 20230114 / Python SDK 0.7.1 no **yes** **yes** **yes** **yes** **yes** **yes** **yes** +C++ SDK 20220927 / Python SDK 0.5.2 no **yes** **yes** **yes** **yes** **yes** no no +C++ SDK 20220826 / Python SDK 0.5.1 no **yes** **yes** **yes** **yes** **yes** no no +C++ SDK 20220608 / Python SDK 0.4.1 **yes** **yes** **yes** **yes** **yes** no no no +C++ SDK 20220504 / Python SDK 0.4.0 **yes** **yes** **yes** **yes** **yes** no no no +C++ SDK 20220107 / Python SDK 0.3.0 **yes** **yes** **yes** **yes** no no no no +C++ SDK 20210608 / Python SDK 0.2.1 **yes** **yes** **yes** no no no no no +C++ SDK 20201209 / n/a **yes** **yes** **yes** no no no no no +C++ SDK v1.13.0 / n/a **yes** no no no no no no no +==================================== ======= ======= ======= ======= ======= ======= ======= ======= + +If you are a C++ SDK user who has upgraded to the latest FW but requires an older SDK version, +please contact our customer support or the Field Applications Engineer who works with you. + + Status and Contact Info ----------------------- @@ -26,8 +59,12 @@ every release. For questions about using your Ouster hardware, you may find it useful to reference the `Ouster sensor documentation`_ and/or contact `Ouster support`_. For issues specific to the SDK please use -the `GitHub issue tracker`_. +the `GitHub issue tracker`_. Announcements for the Ouster SDK are posted in the `Ouster Github +announcements`_ .. _Ouster sensor documentation: https://static.ouster.dev/sensor-docs/index.html .. _Ouster support: https://ouster.atlassian.net/servicedesk/customer/portal/8 .. _Github issue tracker: https://github.com/ouster-lidar/ouster_example/issues +.. _Ouster Github announcements: https://github.com/ouster-lidar/ouster_example/discussions/categories/announcements +.. _Changelog: https://github.com/ouster-lidar/ouster_example/blob/master/CHANGELOG.rst +.. _Ouster ROS 1 driver: https://github.com/ouster-lidar/ouster-ros diff --git a/docs/python/devel.rst b/docs/python/devel.rst index f758c461..beebec34 100644 --- a/docs/python/devel.rst +++ b/docs/python/devel.rst @@ -170,7 +170,7 @@ image, run: -t ouster-sdk-tox the ``BASE`` argument will default to ``ubuntu:18.04``, but can also be set to other docker tags, -e.g. ``ubuntu:20.04``, ``ubuntu:22.04`` or ``debian:10``. Then, run the container to invoke tox: +e.g. ``ubuntu:20.04``, ``ubuntu:22.04`` or ``debian:11``. Then, run the container to invoke tox: .. code:: console diff --git a/docs/python/quickstart.rst b/docs/python/quickstart.rst index 99960909..23e4d423 100644 --- a/docs/python/quickstart.rst +++ b/docs/python/quickstart.rst @@ -134,7 +134,7 @@ Just like with the sample data, you can create a :py:class:`.PacketSource` from .. code:: python - >>> source = client.Sensor(hostname) + >>> source = client.Sensor(hostname, 7502, 7503) >>> info = source.metadata Now we have a ``source`` from our sensor! You're ready to record, visualize to visualize data from your sensor, proceed to diff --git a/docs/python/viz/viz-api-tutorial.rst b/docs/python/viz/viz-api-tutorial.rst index b23bea5c..5dccc26d 100644 --- a/docs/python/viz/viz-api-tutorial.rst +++ b/docs/python/viz/viz-api-tutorial.rst @@ -283,7 +283,7 @@ unstructured point clouds to draw 3D axes at the origin: .. literalinclude:: /../python/src/ouster/sdk/examples/viz.py :start-after: [doc-stag-axes-helper] :end-before: [doc-etag-axes-helper] - :emphasize-lines: 18-23 + :emphasize-lines: 17-22 :linenos: :dedent: diff --git a/docs/python/viz/viz-run.rst b/docs/python/viz/viz-run.rst index 43b4c1ed..03f240be 100644 --- a/docs/python/viz/viz-run.rst +++ b/docs/python/viz/viz-run.rst @@ -63,7 +63,14 @@ Keyboard controls: .. [end-simple-viz-keymap] -For usage and other options, run ``simple-viz -h`` +The visualizer also includes an option to control the orientation of the point cloud in space when +loaded. If you possess, say, an OS-DOME mounted an upside down, you can start the visualizer with +the option ``--extrinsics``:: + + $ simple-viz --sensor 10.0.0.13 --extrinsics -1 0 0 0 0 1 0 0 0 0 -1 0 0 0 0 1 +The input is a row-major homogenous matrix. + +For other options, run ``simple-viz -h`` .. note:: diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index f2d66dd7..81320e90 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -168,6 +168,10 @@ bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf); * * Will attempt to fetch from the network if not already populated. * + * @throw runtime_error if the sensor is in ERROR state, the firmware version + * used to initialize the HTTP or TCP client is invalid, the metadata could + * not be retrieved from the sensor, or the response could not be parsed. + * * @param[in] cli client returned by init_client associated with the connection. * @param[in] timeout_sec how long to wait for the sensor to initialize. * @param[in] legacy_format whether to use legacy format of metadata output. @@ -175,7 +179,7 @@ bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf); * @return a text blob of metadata parseable into a sensor_info struct. */ std::string get_metadata(client& cli, int timeout_sec = 60, - bool legacy_format = true); + bool legacy_format = false); /** * Get sensor config from the sensor. diff --git a/ouster_client/include/ouster/lidar_scan.h b/ouster_client/include/ouster/lidar_scan.h index 684b62ac..3abc9150 100644 --- a/ouster_client/include/ouster/lidar_scan.h +++ b/ouster_client/include/ouster/lidar_scan.h @@ -42,44 +42,12 @@ using LidarScanFieldTypes = */ class LidarScan { public: - [[deprecated]] static constexpr int N_FIELDS = - 4; ///< @deprecated Number of fields now varies by lidar profile or - ///< constructor provided arguments. Use N_FIELDS with caution even - ///< when working with legacy lidar profile data, and do not use for - ///< all non-legacy lidar profile formats. - - using raw_t [[deprecated]] = uint32_t; ///< @deprecated - using ts_t [[deprecated]] = std::chrono::nanoseconds; ///< @deprecated - template using Header = Eigen::Array; ///< Header typedef /** XYZ coordinates with dimensions arranged contiguously in columns. */ using Points = Eigen::Array; - /** Old names provided for compatibility, see sensor::ChanField. */ - using Field [[deprecated]] = sensor::ChanField; ///< @deprecated - [[deprecated]] static constexpr Field RANGE = - sensor::RANGE; ///< @deprecated - [[deprecated]] static constexpr Field INTENSITY = - sensor::SIGNAL; ///< @deprecated - [[deprecated]] static constexpr Field AMBIENT = - sensor::NEAR_IR; ///< @deprecated - [[deprecated]] static constexpr Field REFLECTIVITY = - sensor::REFLECTIVITY; ///< @deprecated - - /** - * Measurement block information, other than the channel data. - * - * @deprecated BlockHeaders are deprecated in favor of Header. See - * ``timestamp()``, ``measurement_id()``, and ``status()`` - */ - struct [[deprecated]] BlockHeader { - ts_t timestamp; - uint32_t encoder; - uint32_t status; - }; - private: Header timestamp_; Header measurement_id_; @@ -106,16 +74,6 @@ class LidarScan { */ std::ptrdiff_t h{0}; - /** - * Vector containing the header definitions. - * - * @deprecated BlockHeader is deprecated in favor of Header - * - * @warning Members variables: use with caution, some of these will become - * private. - */ - [[deprecated]] std::vector headers{}; - /** * Frame status - information from the packet header which corresponds to a * frame @@ -209,28 +167,6 @@ class LidarScan { */ sensor::ThermalShutdownStatus thermal_shutdown() const; - /** - * Access timestamps as a vector. - * - * @deprecated See `timestamp()` instead - * - * @returns copy of the measurement timestamps as a vector. - */ - [[deprecated]] std::vector timestamps() const; - - /** - * Access measurement block header fields. - * - * @deprecated Please see `status()`, `measurement_id()`, and `timestamp()` - * instead - * - * @return the header values for the specified measurement id. - */ - [[deprecated]] BlockHeader& header(size_t m_id); - - /** @copydoc header(size_t m_id) */ - [[deprecated]] const BlockHeader& header(size_t m_id) const; - /** * Access a lidar data field. * @@ -351,19 +287,6 @@ std::string to_string(const LidarScan& ls); * @{ */ -/** - * Equality for column headers. - * - * @deprecated BlockHeaders are deprecated - * - * @param[in] a The first column header to compare. - * @param[in] b The second column header to compare. - * - * @return if a == b. - */ -[[deprecated]] bool operator==(const LidarScan::BlockHeader& a, - const LidarScan::BlockHeader& b); - /** * Equality for scans. * @@ -405,11 +328,14 @@ struct XYZLut { * Each table is an n x 3 array of doubles stored in column-major order where * each row corresponds to the nth point in a lidar scan, with 0 <= n < h*w. * + * Projections to XYZ made with this XYZLut will be in the coordinate frame + * defined by transform*beam_to_lidar_transform. + * * @param[in] w number of columns in the lidar scan. e.g. 512, 1024, or 2048. * @param[in] h number of rows in the lidar scan. * @param[in] range_unit the unit, in meters, of the range, e.g. * sensor::range_unit. - * @param[in] beam_to_lidar_transform, signifying transform between beams and + * @param[in] beam_to_lidar_transform transform between beams and * lidar origin. Translation portion is in millimeters. * @param[in] transform additional transformation to apply to resulting points. * @param[in] azimuth_angles_deg azimuth offsets in degrees for each of h beams. @@ -425,6 +351,8 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, /** * Convenient overload that uses parameters from the supplied sensor_info. + * Projections to XYZ made with this XYZLut will be in the sensor coordinate + * frame defined in the sensor documentation. * * @param[in] sensor metadata returned from the client. * @@ -554,6 +482,47 @@ class ScanBatcher { bool operator()(const uint8_t* packet_buf, LidarScan& ls); }; +/** + * Imu Data + */ +struct Imu { + union { + std::array angular_vel; + struct { + double wx, wy, wz; + }; + }; + union { + std::array linear_accel; + struct { + double ax, ay, az; + }; + }; + union { + std::array ts; + struct { + uint64_t sys_ts, accel_ts, gyro_ts; + }; + }; +}; + +/** Equality for Imu */ +inline bool operator==(const Imu& a, const Imu& b) { + return a.angular_vel == b.angular_vel && a.linear_accel == b.linear_accel && + a.ts == b.ts; +}; + +/** Not Equality for Imu */ +inline bool operator!=(const Imu& a, const Imu& b) { + return !(a == b); +}; + +std::string to_string(const Imu& imu); + +/// Reconstructs buf with UDP imu_packet to osf::Imu object +void packet_to_imu(const uint8_t* buf, const ouster::sensor::packet_format& pf, + Imu& imu); + } // namespace ouster #include "ouster/impl/cartesian.h" diff --git a/ouster_client/src/sensor_http.h b/ouster_client/include/ouster/sensor_http.h similarity index 97% rename from ouster_client/src/sensor_http.h rename to ouster_client/include/ouster/sensor_http.h index ad2c0aee..2b764a06 100644 --- a/ouster_client/src/sensor_http.h +++ b/ouster_client/include/ouster/sensor_http.h @@ -25,8 +25,6 @@ class SensorHttp { protected: /** * Constructs an http interface to communicate with the sensor. - * - * @param[in] hostname hostname of the sensor to communicate with. */ SensorHttp() = default; diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index eaa6f49b..3cae90e5 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -173,6 +173,9 @@ enum UDPProfileLidar { /** Single Returns Low Data Rate */ PROFILE_RNG15_RFL8_NIR8, + + /** Five Word Profile */ + PROFILE_FIVE_WORD_PIXEL, }; /** Profile indicating packet format of IMU data. */ @@ -364,6 +367,7 @@ struct data_format { ColumnWindow column_window; ///< window of columns over which sensor fires UDPProfileLidar udp_profile_lidar; ///< profile of lidar packet UDPProfileIMU udp_profile_imu; ///< profile of imu packet + uint16_t fps; ///< frames per second }; /** Stores necessary information from sensor to parse and project sensor data. @@ -755,6 +759,11 @@ enum ChanField { FLAGS = 8, ///< 1st return flags FLAGS2 = 9, ///< 2nd return flags RAW_HEADERS = 40, ///< raw headers for packet/footer/column for dev use + RAW32_WORD5 = 45, ///< raw word access to packet for dev use + RAW32_WORD6 = 46, ///< raw word access to packet for dev use + RAW32_WORD7 = 47, ///< raw word access to packet for dev use + RAW32_WORD8 = 48, ///< raw word access to packet for dev use + RAW32_WORD9 = 49, ///< raw word access to packet for dev use CUSTOM0 = 50, ///< custom user field CUSTOM1 = 51, ///< custom user field CUSTOM2 = 52, ///< custom user field @@ -898,7 +907,7 @@ class packet_format final { /** * Read the packet shot limiting countdown * - * @param[in] liar_buf the lidar buf. + * @param[in] lidar_buf the lidar buf. * * @return the shot limiting countdown. */ diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index 7cc85a4e..091b7d5d 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -26,7 +26,7 @@ #include "logging.h" #include "netcompat.h" #include "ouster/types.h" -#include "sensor_http.h" +#include "ouster/sensor_http.h" using namespace std::chrono_literals; namespace chrono = std::chrono; @@ -360,8 +360,9 @@ bool set_config(const std::string& hostname, const sensor_config& config, std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { try { cli.meta = collect_metadata(cli.hostname, timeout_sec); - } catch (const std::exception&) { - return ""; + } catch (const std::exception& e) { + logger().warn(std::string("Unable to retrieve sensor metadata: ") + e.what()); + throw; } Json::StreamWriterBuilder builder; diff --git a/ouster_client/src/http_client.h b/ouster_client/src/http_client.h index be6cb2ff..7f900711 100644 --- a/ouster_client/src/http_client.h +++ b/ouster_client/src/http_client.h @@ -23,7 +23,7 @@ class HttpClient { /** * Constructs an HttpClient object to communicate with an http server. * - * @param[in] base_url url to the http server. + * @param[in] base_url_ url to the http server. */ HttpClient(const std::string& base_url_) : base_url(base_url_) {} @@ -53,4 +53,4 @@ class HttpClient { }; } // namespace util -} // namespace ouster \ No newline at end of file +} // namespace ouster diff --git a/ouster_client/src/image_processing.cpp b/ouster_client/src/image_processing.cpp index 8f3739cc..8eaa2e03 100644 --- a/ouster_client/src/image_processing.cpp +++ b/ouster_client/src/image_processing.cpp @@ -189,6 +189,11 @@ static Eigen::Array compute_dark_count( image.template cast().colwise().any(); const size_t n_cols = col_mask.count(); + if (n_cols == 0) { + new_dark_count.setZero(image_h); + return new_dark_count; + } + img_t row_diffs_nonzero{image_h - 1, n_cols}; for (size_t i = 0, j = 0; i < image_w && j < n_cols; i++) { if (col_mask[i]) { diff --git a/ouster_client/src/lidar_scan.cpp b/ouster_client/src/lidar_scan.cpp index cfb3ce92..40481996 100644 --- a/ouster_client/src/lidar_scan.cpp +++ b/ouster_client/src/lidar_scan.cpp @@ -27,18 +27,18 @@ enum frame_status_masks : uint64_t { FRAME_STATUS_SHOT_LIMITING_MASK = 0xf0 ///< Mask to get shot limting status }; +//! @cond Doxygen_Suppress enum frame_status_shifts: uint64_t { FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT = 0, ///< No shift for thermal shutdown FRAME_STATUS_SHOT_LIMITING_SHIFT = 4 /// shift 4 for shot limiting }; +//! @endcond // clang-format on using sensor::ChanField; using sensor::ChanFieldType; using sensor::UDPProfileLidar; -constexpr int LidarScan::N_FIELDS; - LidarScan::LidarScan() = default; LidarScan::LidarScan(const LidarScan&) = default; LidarScan::LidarScan(LidarScan&&) = default; @@ -80,6 +80,14 @@ static const Table lb_field_slots{{ {ChanField::NEAR_IR, ChanFieldType::UINT16}, }}; +static const Table five_word_slots{{ + {ChanField::RAW32_WORD1, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD2, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD3, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD4, ChanFieldType::UINT32}, + {ChanField::RAW32_WORD5, ChanFieldType::UINT32}, +}}; + struct DefaultFieldsEntry { const std::pair* fields; size_t n_fields; @@ -93,7 +101,9 @@ Table default_scan_fields{ {UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16, {single_field_slots.data(), single_field_slots.size()}}, {UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8, - {lb_field_slots.data(), lb_field_slots.size()}}}}; + {lb_field_slots.data(), lb_field_slots.size()}}, + {UDPProfileLidar::PROFILE_FIVE_WORD_PIXEL, + {five_word_slots.data(), five_word_slots.size()}}}}; static std::vector> lookup_scan_fields( UDPProfileLidar profile) { @@ -112,17 +122,13 @@ static std::vector> lookup_scan_fields( } // namespace impl // specify sensor:: namespace for doxygen matching -LidarScan::LidarScan( - size_t w, size_t h, - std::vector> - field_types) +LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types) : timestamp_{Header::Zero(w)}, measurement_id_{Header::Zero(w)}, status_{Header::Zero(w)}, field_types_{std::move(field_types)}, w{static_cast(w)}, - h{static_cast(h)}, - headers{w, BlockHeader{ts_t{0}, 0, 0}} { + h{static_cast(h)} { // TODO: error on duplicate fields for (const auto& ft : field_types_) { if (fields_.count(ft.first) > 0) @@ -150,21 +156,6 @@ sensor::ThermalShutdownStatus LidarScan::thermal_shutdown() const { frame_status_shifts::FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT); } -std::vector LidarScan::timestamps() const { - std::vector res; - res.reserve(headers.size()); - for (const auto& h : headers) res.push_back(h.timestamp); - return res; -} - -LidarScan::BlockHeader& LidarScan::header(size_t m_id) { - return headers.at(m_id); -} - -const LidarScan::BlockHeader& LidarScan::header(size_t m_id) const { - return headers.at(m_id); -} - template ::value, T>::type> Eigen::Ref> LidarScan::field(ChanField f) { @@ -234,12 +225,6 @@ bool LidarScan::complete(sensor::ColumnWindow window) const { } } -bool operator==(const LidarScan::BlockHeader& a, - const LidarScan::BlockHeader& b) { - return a.timestamp == b.timestamp && a.encoder == b.encoder && - a.status == b.status; -} - bool operator==(const LidarScan& a, const LidarScan& b) { return a.frame_id == b.frame_id && a.w == b.w && a.h == b.h && a.frame_status == b.frame_status && a.fields_ == b.fields_ && @@ -316,8 +301,12 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, const std::vector& altitude_angles_deg) { if (w <= 0 || h <= 0) throw std::invalid_argument("lut dimensions must be greater than zero"); - if (azimuth_angles_deg.size() != h || altitude_angles_deg.size() != h) + + if ((azimuth_angles_deg.size() != h || altitude_angles_deg.size() != h) && + (azimuth_angles_deg.size() != w * h || + altitude_angles_deg.size() != w * h)) { throw std::invalid_argument("unexpected scan dimensions"); + } double beam_to_lidar_euclidean_distance_mm = beam_to_lidar_transform(0, 3); if (beam_to_lidar_transform(2, 3) != 0) { @@ -326,24 +315,40 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, std::pow(beam_to_lidar_transform(2, 3), 2)); } + XYZLut lut; + Eigen::ArrayXd encoder(w * h); // theta_e Eigen::ArrayXd azimuth(w * h); // theta_a Eigen::ArrayXd altitude(w * h); // phi - const double azimuth_radians = M_PI * 2.0 / w; + if (azimuth_angles_deg.size() == h && altitude_angles_deg.size() == h) { + // OS sensor + const double azimuth_radians = M_PI * 2.0 / w; + + // populate angles for each pixel + for (size_t v = 0; v < w; v++) { + for (size_t u = 0; u < h; u++) { + size_t i = u * w + v; + encoder(i) = 2.0 * M_PI - (v * azimuth_radians); + azimuth(i) = -azimuth_angles_deg[u] * M_PI / 180.0; + altitude(i) = altitude_angles_deg[u] * M_PI / 180.0; + } + } - // populate angles for each pixel - for (size_t v = 0; v < w; v++) { - for (size_t u = 0; u < h; u++) { - size_t i = u * w + v; - encoder(i) = 2.0 * M_PI - (v * azimuth_radians); - azimuth(i) = -azimuth_angles_deg[u] * M_PI / 180.0; - altitude(i) = altitude_angles_deg[u] * M_PI / 180.0; + } else if (azimuth_angles_deg.size() == w * h && + altitude_angles_deg.size() == w * h) { + // DF sensor + // populate angles for each pixel + for (size_t v = 0; v < w; v++) { + for (size_t u = 0; u < h; u++) { + size_t i = u * w + v; + encoder(i) = 0; + azimuth(i) = azimuth_angles_deg[i] * M_PI / 180.0; + altitude(i) = altitude_angles_deg[i] * M_PI / 180.0; + } } } - XYZLut lut; - // unit vectors for each pixel lut.direction = LidarScan::Points{w * h, 3}; lut.direction.col(0) = (encoder + azimuth).cos() * altitude.cos(); @@ -421,9 +426,6 @@ void zero_header_cols(LidarScan& ls, std::ptrdiff_t start, std::ptrdiff_t end) { ls.timestamp().segment(start, end - start).setZero(); ls.measurement_id().segment(start, end - start).setZero(); ls.status().segment(start, end - start).setZero(); - - // zero deprecated header blocks - for (auto m_id = start; m_id < end; m_id++) ls.header(m_id) = {}; } /* @@ -585,8 +587,7 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { for (int icol = 0; icol < pf.columns_per_packet; icol++) { const uint8_t* col_buf = pf.nth_col(icol, packet_buf); const uint16_t m_id = pf.col_measurement_id(col_buf); - const std::chrono::nanoseconds ts(pf.col_timestamp(col_buf)); - const uint32_t encoder = pf.col_encoder(col_buf); + const uint64_t ts = pf.col_timestamp(col_buf); const uint32_t status = pf.col_status(col_buf); const bool valid = (status & 0x01); @@ -621,11 +622,8 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { next_valid_m_id = m_id + 1; } - // old header API; will be removed in a future release - ls.header(m_id) = {ts, encoder, status}; - // write new header values - ls.timestamp()[m_id] = ts.count(); + ls.timestamp()[m_id] = ts; ls.measurement_id()[m_id] = m_id; ls.status()[m_id] = status; @@ -633,6 +631,48 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { } return false; -} // namespace ouster +} + +std::string to_string(const Imu& imu) { + std::stringstream ss; + ss << "Imu: "; + ss << "linear_accel: ["; + for (size_t i = 0; i < imu.linear_accel.size(); ++i) { + if (i > 0) ss << ", "; + ss << imu.linear_accel[i]; + } + ss << "]"; + ss << ", angular_vel = ["; + for (size_t i = 0; i < imu.angular_vel.size(); ++i) { + if (i > 0) ss << ", "; + ss << imu.angular_vel[i]; + } + ss << "]"; + ss << ", ts: ["; + std::array labels{"sys_ts", "accel_ts", "gyro_ts"}; + for (size_t i = 0; i < imu.ts.size(); ++i) { + if (i > 0) ss << ", "; + ss << labels[i] << " = "; + ss << imu.ts[i]; + } + ss << "]"; + return ss.str(); +} + +void packet_to_imu(const uint8_t* buf, const ouster::sensor::packet_format& pf, + Imu& imu) { + // Storing all available timestamps + imu.sys_ts = pf.imu_sys_ts(buf); + imu.accel_ts = pf.imu_accel_ts(buf); + imu.gyro_ts = pf.imu_gyro_ts(buf); + + imu.linear_accel[0] = pf.imu_la_x(buf); + imu.linear_accel[1] = pf.imu_la_y(buf); + imu.linear_accel[2] = pf.imu_la_z(buf); + + imu.angular_vel[0] = pf.imu_av_x(buf); + imu.angular_vel[1] = pf.imu_av_y(buf); + imu.angular_vel[2] = pf.imu_av_z(buf); +} } // namespace ouster diff --git a/ouster_client/src/parsing.cpp b/ouster_client/src/parsing.cpp index f3aaf73e..72ee3d8a 100644 --- a/ouster_client/src/parsing.cpp +++ b/ouster_client/src/parsing.cpp @@ -86,6 +86,23 @@ static const Table single_field_info{{ {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, }}; +static const Table five_word_pixel_info{{ + {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, + {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, + {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, + {ChanField::RANGE2, {UINT32, 4, 0x0007ffff, 0}}, + {ChanField::FLAGS2, {UINT8, 6, 0b11111000, 3}}, + {ChanField::REFLECTIVITY2, {UINT8, 7, 0, 0}}, + {ChanField::SIGNAL, {UINT16, 8, 0, 0}}, + {ChanField::SIGNAL2, {UINT16, 10, 0, 0}}, + {ChanField::NEAR_IR, {UINT16, 12, 0, 0}}, + {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, + {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, + {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, + {ChanField::RAW32_WORD4, {UINT32, 12, 0, 0}}, + {ChanField::RAW32_WORD5, {UINT32, 16, 0, 0}}, +}}; + Table profiles{{ {UDPProfileLidar::PROFILE_LIDAR_LEGACY, {legacy_field_info.data(), legacy_field_info.size(), 12}}, @@ -95,6 +112,8 @@ Table profiles{{ {single_field_info.data(), single_field_info.size(), 12}}, {UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8, {lb_field_info.data(), lb_field_info.size(), 4}}, + {UDPProfileLidar::PROFILE_FIVE_WORD_PIXEL, + {five_word_pixel_info.data(), five_word_pixel_info.size(), 20}}, }}; static const ProfileEntry& lookup_profile_entry(UDPProfileLidar profile) { @@ -423,7 +442,7 @@ uint16_t packet_format::px_signal(const uint8_t* px_buf) const { } uint16_t packet_format::px_ambient(const uint8_t* px_buf) const { - return px_field(px_buf, ChanField::AMBIENT); + return px_field(px_buf, ChanField::NEAR_IR); } /* IMU packet parsing */ diff --git a/ouster_client/src/sensor_http.cpp b/ouster_client/src/sensor_http.cpp index 45f9cadb..964c4133 100644 --- a/ouster_client/src/sensor_http.cpp +++ b/ouster_client/src/sensor_http.cpp @@ -1,4 +1,4 @@ -#include "sensor_http.h" +#include "ouster/sensor_http.h" #include diff --git a/ouster_client/src/sensor_http_imp.h b/ouster_client/src/sensor_http_imp.h index 0298c056..f8827e23 100644 --- a/ouster_client/src/sensor_http_imp.h +++ b/ouster_client/src/sensor_http_imp.h @@ -10,7 +10,7 @@ #pragma once #include "http_client.h" -#include "sensor_http.h" +#include "ouster/sensor_http.h" namespace ouster { namespace sensor { diff --git a/ouster_client/src/sensor_tcp_imp.h b/ouster_client/src/sensor_tcp_imp.h index 7e5bc816..c2b2afa6 100644 --- a/ouster_client/src/sensor_tcp_imp.h +++ b/ouster_client/src/sensor_tcp_imp.h @@ -2,7 +2,7 @@ * Copyright (c) 2022, Ouster, Inc. * All rights reserved. * - * @file sensor_tcp.h + * @file sensor_tcp_imp.h * @brief A high level TCP interface for Ouster sensors. * */ @@ -10,8 +10,7 @@ #pragma once #include "netcompat.h" - -#include "sensor_http.h" +#include "ouster/sensor_http.h" namespace ouster { namespace sensor { @@ -139,4 +138,4 @@ class SensorTcpImp : public util::SensorHttp { } // namespace impl } // namespace sensor -} // namespace ouster \ No newline at end of file +} // namespace ouster diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index 824911c8..947b7c07 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -20,6 +20,7 @@ #include "logging.h" #include "ouster/impl/build.h" #include "ouster/version.h" + namespace ouster { using nonstd::make_optional; @@ -68,7 +69,7 @@ extern const Table polarity_strings{ extern const Table nmea_baud_rate_strings{ {{BAUD_9600, "BAUD_9600"}, {BAUD_115200, "BAUD_115200"}}}; -Table chanfield_strings{{ +Table chanfield_strings{{ {ChanField::RANGE, "RANGE"}, {ChanField::RANGE2, "RANGE2"}, {ChanField::SIGNAL, "SIGNAL"}, @@ -93,13 +94,19 @@ Table chanfield_strings{{ {ChanField::RAW32_WORD2, "RAW32_WORD2"}, {ChanField::RAW32_WORD3, "RAW32_WORD3"}, {ChanField::RAW32_WORD4, "RAW32_WORD4"}, + {ChanField::RAW32_WORD5, "RAW32_WORD5"}, + {ChanField::RAW32_WORD6, "RAW32_WORD6"}, + {ChanField::RAW32_WORD7, "RAW32_WORD7"}, + {ChanField::RAW32_WORD8, "RAW32_WORD8"}, + {ChanField::RAW32_WORD9, "RAW32_WORD9"}, }}; -Table udp_profile_lidar_strings{{ +Table udp_profile_lidar_strings{{ {PROFILE_LIDAR_LEGACY, "LEGACY"}, {PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, "RNG19_RFL8_SIG16_NIR16_DUAL"}, {PROFILE_RNG19_RFL8_SIG16_NIR16, "RNG19_RFL8_SIG16_NIR16"}, {PROFILE_RNG15_RFL8_NIR8, "RNG15_RFL8_NIR8"}, + {PROFILE_FIVE_WORD_PIXEL, "FIVE_WORD_PIXEL"}, }}; Table udp_profile_imu_strings{{ @@ -138,7 +145,7 @@ bool operator==(const data_format& lhs, const data_format& rhs) { lhs.pixel_shift_by_row == rhs.pixel_shift_by_row && lhs.column_window == rhs.column_window && lhs.udp_profile_lidar == rhs.udp_profile_lidar && - lhs.udp_profile_imu == rhs.udp_profile_imu); + lhs.udp_profile_imu == rhs.udp_profile_imu && lhs.fps == rhs.fps); } bool operator!=(const data_format& lhs, const data_format& rhs) { @@ -233,7 +240,8 @@ data_format default_data_format(lidar_mode mode) { offset, column_window, UDPProfileLidar::PROFILE_LIDAR_LEGACY, - UDPProfileIMU::PROFILE_IMU_LEGACY}; + UDPProfileIMU::PROFILE_IMU_LEGACY, + static_cast(frequency_of_lidar_mode(mode))}; } static double default_lidar_origin_to_beam_origin(std::string prod_line) { @@ -496,23 +504,13 @@ std::string to_string(ThermalShutdownStatus thermal_shutdown_status) { } void check_signal_multiplier(const double signal_multiplier) { - int signal_multiplier_int = int(signal_multiplier); std::string signal_multiplier_error = "Provided signal multiplier is invalid: " + std::to_string(signal_multiplier) + " cannot be converted to one of [0.25, 0.5, 1, 2, 3]"; - // get the doubles out of the way - if (signal_multiplier == 0.25 || signal_multiplier == 0.5) return; - - // everything else has to be essentially an int - if (std::fabs(signal_multiplier - double(signal_multiplier_int)) > - signal_multiplier_int * std::numeric_limits::epsilon()) { - throw std::runtime_error(signal_multiplier_error); - } - - // the int has to 1, 2, or 3 - if (signal_multiplier_int < 1 || signal_multiplier_int > 3) { + std::set valid_values = {0.25, 0.5, 1, 2, 3}; + if (!valid_values.count(signal_multiplier)) { throw std::runtime_error(signal_multiplier_error); } } @@ -684,7 +682,8 @@ static bool is_new_format(const std::string& metadata) { if (metadata.size()) { if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{errors}; + throw std::runtime_error{ + "Error parsing metadata when checking format: " + errors}; } size_t nonlegacy_fields_present = 0; @@ -714,8 +713,7 @@ static bool is_new_format(const std::string& metadata) { static data_format parse_data_format(const Json::Value& root) { const std::vector data_format_required_fields{ - "pixels_per_column", "columns_per_packet", "columns_per_frame", - "pixel_shift_by_row"}; + "pixels_per_column", "columns_per_packet", "columns_per_frame"}; for (const auto& field : data_format_required_fields) { if (!root.isMember(field)) { @@ -729,12 +727,17 @@ static data_format parse_data_format(const Json::Value& root) { format.columns_per_packet = root["columns_per_packet"].asInt(); format.columns_per_frame = root["columns_per_frame"].asInt(); - if (root["pixel_shift_by_row"].size() != format.pixels_per_column) { - throw std::runtime_error{"Unexpected number of pixel_shift_by_row"}; - } + if (root.isMember("pixel_shift_by_row")) { + if (root["pixel_shift_by_row"].size() != format.pixels_per_column) { + throw std::runtime_error{"Unexpected number of pixel_shift_by_row"}; + } - for (const auto& v : root["pixel_shift_by_row"]) - format.pixel_shift_by_row.push_back(v.asInt()); + for (const auto& v : root["pixel_shift_by_row"]) + format.pixel_shift_by_row.push_back(v.asInt()); + } else { + // DF path + format.pixel_shift_by_row.assign(format.pixels_per_column, 0); + } if (root.isMember("column_window")) { if (root["column_window"].size() != 2) { @@ -777,6 +780,13 @@ static data_format parse_data_format(const Json::Value& root) { format.udp_profile_imu = PROFILE_IMU_LEGACY; } + if (root.isMember("fps")) { + format.fps = root["fps"].asInt(); + } else { + logger().warn("No fps found. Trying to use one from lidar mode (or 0)"); + format.fps = 0; + } + return format; } // namespace sensor @@ -791,8 +801,17 @@ static sensor_info parse_legacy(const std::string& meta) { throw std::runtime_error{errors}; } - const std::vector minimum_legacy_metadata_fields{ - "beam_altitude_angles", "beam_azimuth_angles", "lidar_mode"}; + // NOTE[pb]: DF development metadata.json doesn't have beam_altitude_angles + // and beam_azimuth_angles and instead provides beam_xyz. However + // final implementation should have azimuth/altitude angles and + // we may uncomment the validation back closer to the release. + // const std::vector minimum_legacy_metadata_fields{ + // "beam_altitude_angles", "beam_azimuth_angles", "lidar_mode"}; + + // NOTE[pb]: DF metadata doesn't have lidar_mode, but because it's + // going through convert_to_legacy() function it get's the empty + // string lidar_mode ... hmmm, fine for now... + const std::vector minimum_legacy_metadata_fields{"lidar_mode"}; for (auto field : minimum_legacy_metadata_fields) { if (!root.isMember(field)) { @@ -845,6 +864,11 @@ static sensor_info parse_legacy(const std::string& meta) { // "data_format" introduced in fw 2.0. Fall back to 1.13 if (root.isMember("data_format")) { info.format = parse_data_format(root["data_format"]); + // data_format.fps was added for DF sensors, so we are backfilling + // fps value for OS sensors here if it's not present in metadata + if (info.format.fps == 0) { + info.format.fps = frequency_of_lidar_mode(info.mode); + } } else { logger().warn("No data_format found. Using default legacy data format"); info.format = default_data_format(info.mode); @@ -888,19 +912,77 @@ static sensor_info parse_legacy(const std::string& meta) { info.lidar_origin_to_beam_origin_mm; } - if (root["beam_altitude_angles"].size() != info.format.pixels_per_column) { + if (root["beam_altitude_angles"].size() != 0 && + root["beam_altitude_angles"].size() != info.format.pixels_per_column) { throw std::runtime_error{"Unexpected number of beam_altitude_angles"}; } - if (root["beam_azimuth_angles"].size() != info.format.pixels_per_column) { + if (root["beam_azimuth_angles"].size() != 0 && + root["beam_azimuth_angles"].size() != info.format.pixels_per_column) { throw std::runtime_error{"Unexpected number of beam_azimuth_angles"}; } - for (const auto& v : root["beam_altitude_angles"]) - info.beam_altitude_angles.push_back(v.asDouble()); + if (root["beam_altitude_angles"].size() == info.format.pixels_per_column) { + if (root["beam_altitude_angles"][0].isArray()) { + // DF sensor path + for (const auto& row : root["beam_altitude_angles"]) + for (const auto& v : row) + info.beam_altitude_angles.push_back(v.asDouble()); - for (const auto& v : root["beam_azimuth_angles"]) - info.beam_azimuth_angles.push_back(v.asDouble()); + if (info.beam_altitude_angles.size() != + info.format.pixels_per_column * info.format.columns_per_frame) { + throw std::runtime_error{ + "Unexpected number of total beam_altitude_angles"}; + } + } else { + // OS sensor path + for (const auto& v : root["beam_altitude_angles"]) + info.beam_altitude_angles.push_back(v.asDouble()); + } + } + + if (root["beam_azimuth_angles"].size() == info.format.pixels_per_column) { + if (root["beam_azimuth_angles"][0].isArray()) { + // DF sensor path + for (const auto& row : root["beam_azimuth_angles"]) { + for (const auto& v : row) + info.beam_azimuth_angles.push_back(v.asDouble()); + } + + if (info.beam_azimuth_angles.size() != + info.format.pixels_per_column * info.format.columns_per_frame) { + throw std::runtime_error{ + "Unexpected number of total beam_azimuth_angles"}; + } + } else { + // OS sensor path + for (const auto& v : root["beam_azimuth_angles"]) + info.beam_azimuth_angles.push_back(v.asDouble()); + } + } + + // NOTE[pb]: this block that handles beam_xyz shouldn't survive past + // the DF development phase and we need to swith to azimuth/altitude + // angles in the metadata, because they take less space and they + // are less redundant configuration of intrinsics than unit xyz vectors + if (info.beam_altitude_angles.empty() && info.beam_azimuth_angles.empty()) { + if (root["beam_xyz"].size() != + 3 * info.format.pixels_per_column * info.format.columns_per_frame) { + throw std::runtime_error{"Unexpected number of beam_xyz"}; + } + + // DF sensor path + auto& xyz = root["beam_xyz"]; + for (Json::Value::ArrayIndex idx = 0; idx < xyz.size(); idx += 3) { + auto x = xyz[idx + 0].asDouble(); + auto y = xyz[idx + 1].asDouble(); + auto z = xyz[idx + 2].asDouble(); + auto al = std::atan2(z, sqrt(x * x + y * y)) * 180.0 / M_PI; + auto az = std::atan2(y, x) * 180.0 / M_PI; + info.beam_altitude_angles.push_back(al); + info.beam_azimuth_angles.push_back(az); + } + } // "imu_to_sensor_transform" may be absent in sensor config // produced by Ouster Studio, so we backfill it with default value @@ -938,6 +1020,7 @@ static sensor_info parse_legacy(const std::string& meta) { } auto zero_check = [](auto el, std::string name) { + if (el.size() == 0) return; bool all_zeros = std::all_of(el.cbegin(), el.cend(), [](double k) { return k == 0.0; }); if (all_zeros) { @@ -981,7 +1064,8 @@ std::string convert_to_legacy(const std::string& metadata) { if (metadata.size()) { if (!Json::parseFromStream(read_builder, ss, &root, &errors)) { - throw std::runtime_error{errors}; + throw std::runtime_error{ + "Errors parsing metadata for convert_to_legacy: " + errors}; } } Json::Value result{}; @@ -1024,7 +1108,8 @@ sensor_info parse_metadata(const std::string& metadata) { if (metadata.size()) { if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{errors}; + throw std::runtime_error{ + "Errors parsing metadata for parse_metadata: " + errors}; } sensor_info info{}; @@ -1067,6 +1152,7 @@ std::string to_string(const sensor_info& info) { root["data_format"]["pixels_per_column"] = info.format.pixels_per_column; root["data_format"]["columns_per_packet"] = info.format.columns_per_packet; root["data_format"]["columns_per_frame"] = info.format.columns_per_frame; + root["data_format"]["fps"] = info.format.fps; for (auto i : info.format.pixel_shift_by_row) root["data_format"]["pixel_shift_by_row"].append(i); diff --git a/ouster_pcap/CMakeLists.txt b/ouster_pcap/CMakeLists.txt index 3bd6b203..adb39eef 100644 --- a/ouster_pcap/CMakeLists.txt +++ b/ouster_pcap/CMakeLists.txt @@ -1,10 +1,11 @@ # ==== Requirements ==== +find_package(Pcap REQUIRED) find_package(libtins REQUIRED) # ==== Libraries ==== -add_library(ouster_pcap src/os_pcap.cpp) +add_library(ouster_pcap src/pcap.cpp src/os_pcap.cpp src/indexed_pcap_reader.cpp) target_include_directories(ouster_pcap SYSTEM PRIVATE - ${PCAP_INCLUDE_DIR} ${libtins_INCLUDE_DIRS}) + ${PCAP_INCLUDE_DIR}) target_include_directories(ouster_pcap PUBLIC $ $) @@ -13,7 +14,10 @@ if(WIN32) target_compile_options(ouster_pcap PRIVATE /wd4200) target_link_libraries(ouster_pcap PUBLIC ws2_32) endif() -target_link_libraries(ouster_pcap PRIVATE libtins) +target_link_libraries(ouster_pcap + PUBLIC + OusterSDK::ouster_client + PRIVATE ${PCAP_LIBRARY} libtins) add_library(OusterSDK::ouster_pcap ALIAS ouster_pcap) # ==== Install ==== diff --git a/ouster_pcap/include/ouster/indexed_pcap_reader.h b/ouster_pcap/include/ouster/indexed_pcap_reader.h new file mode 100644 index 00000000..5b6016da --- /dev/null +++ b/ouster_pcap/include/ouster/indexed_pcap_reader.h @@ -0,0 +1,88 @@ +/** + * Copyright (c) 2023, Ouster Inc. + */ + +#pragma once + +#include "ouster/pcap.h" +#include "ouster/os_pcap.h" +#include "ouster/types.h" + +namespace ouster { +namespace sensor_utils { + +/** + * A PcapReader that allows seeking to the start of a lidar frame. + * To do this, the constructor calls `get_stream_info`, which in turn calls + * IndexedPcapReader::update_index_for_current_packet for each packet. + * This allows us to compute the index and obtain the stream_info while reading + * the PCAP file only once. + */ +struct IndexedPcapReader : public PcapReader { + using frame_index = std::vector; ///< Maps a frame number to a file offset + + /** + * @param pcap_filename[in] A file path of the pcap to read + * @param metadata_filenames[in] A vector of sensor metadata file paths + */ + IndexedPcapReader( + const std::string& pcap_filename, + const std::vector& metadata_filenames, + std::function progress_callback + ); + + /** + * Attempts to match the current packet to one of the sensor info objects + * and returns the appropriate packet format if there is one + * + * @return An optional packet format for the current packet + */ + nonstd::optional sensor_idx_for_current_packet() const; + + /** + * @return the current packet's frame_id + * if the packet is associated with a sensor (and its corresponding packet format) + */ + nonstd::optional current_frame_id() const; + + /** + * Updates the frame index for the current packet + * + * Important: this method is only meant to be invoked from `get_stream_info`! + */ + void update_index_for_current_packet(); + + /** + * @return The stream_info associated with this PcapReader + */ + std::shared_ptr get_stream_info() const; // TODO move to parent class + + /** + * Seeks to the given frame number for the given sensor index + */ + void seek_to_frame(size_t sensor_index, unsigned int frame_number); + + /** + * Returns the number of frames in the frame index for the given sensor index. + * + * @param sensor_index[in] The position of the sensor for which to retrieve the desired frame count. + * @return The number of frames in the sensor's frame index. + */ + size_t frame_count(size_t sensor_index) const; + + /** + * Return true if the frame_id from the packet stream has rolled over, + * hopefully avoiding spurious result that could occur from out of order or dropped packets. + * + * @return true if the frame id has rolled over. + */ + static bool frame_id_rolled_over(uint16_t previous, uint16_t current); + + std::vector sensor_infos_; ///< A vector of sensor_info that correspond to the provided metadata files + std::shared_ptr stream_info_; ///< TODO: move to parent class + std::vector frame_indices_; ///< frame index for each sensor + std::vector> previous_frame_ids_; ///< previous frame id for each sensor +}; + +} // namespace sensor_utils +} // namespace ouster diff --git a/ouster_pcap/include/ouster/os_pcap.h b/ouster_pcap/include/ouster/os_pcap.h index 8f240d34..62edef51 100644 --- a/ouster_pcap/include/ouster/os_pcap.h +++ b/ouster_pcap/include/ouster/os_pcap.h @@ -12,36 +12,108 @@ #include #include #include +#include +#include +#include + +#include "ouster/types.h" +#include "ouster/pcap.h" namespace ouster { namespace sensor_utils { -struct packet_info { - using ts = std::chrono::microseconds; ///< Microsecond timestamp +using ts = std::chrono::microseconds; ///< Microsecond timestamp + +/** + * To string method for packet info structs. + * + * @param[inout] stream_in The pre-existing ostream to concat with data. + * @param[in] data The packet_info to output. + * + * @return The new output stream containing concatted stream_in and data. + */ +std::ostream& operator<<(std::ostream& stream_in, const packet_info& data); - // TODO: use numerical IPs for efficient filtering +/** + * Structure representing a hash key/sorting key for a udp stream + */ +struct stream_key { std::string dst_ip; ///< The destination IP std::string src_ip; ///< The source IP + int src_port; ///< The src port int dst_port; ///< The destination port - int src_port; ///< The source port - size_t payload_size; ///< The size of the packet payload - ts timestamp; ///< The packet capture timestamp - int fragments_in_packet; ///< Number of fragments in the packet - int ip_version; ///< The ip version, 4 or 6 - int encapsulation_protocol; ///< PCAP encapsulation type - // TODO: remove, library ignores non-UDP packes - int network_protocol; ///< IANA protocol number. Always 17 (UDP) + + bool operator==(const struct stream_key &other) const; + bool operator!=(const struct stream_key &other) const; + bool operator<(const struct stream_key &other) const; + bool operator>(const struct stream_key &other) const; + bool operator<=(const struct stream_key &other) const; + bool operator>=(const struct stream_key &other) const; }; /** - * To string method for packet info structs. + * Structure representing a hash key/sorting key for a udp stream + */ +struct guessed_ports { + int lidar; ///< Guessed lidar port + int imu; ///< Guessed imu port +}; + +/** + * To string method for stream_key structs. * * @param[inout] stream_in The pre-existing ostream to concat with data. - * @param[in] data The packet_info to output. + * @param[in] data The stream_key to output. * * @return The new output stream containing concatted stream_in and data. */ -std::ostream& operator<<(std::ostream& stream_in, const packet_info& data); +std::ostream& operator<<(std::ostream& stream_in, const stream_key& data); + +struct stream_data { + uint64_t count; ///< Number of packets in a specified stream + std::map payload_size_counts; ///< Packet sizes detected in a specified stream + ///< Key: Packet Size + ///< Value: Count of a specific packet size + std::map fragment_counts; ///< Fragments detected in a specified stream + ///< Key: Number of fragments + ///< Value: Count of a specific number of packets + std::map ip_version_counts; ///< IP version detected in a specified stream + ///< Key: IP Version + ///< Value: Count of the specific ip version +}; + +/** + * To string method for stream_data structs. + * + * @param[inout] stream_in The pre-existing ostream to concat with data. + * @param[in] data The stream_data to output. + * + * @return The new output stream containing concatted stream_in and data. + */ +std::ostream& operator<<(std::ostream& stream_in, const stream_data& data); + +/** + * Structure representing the information about network streams in a pcap file + */ +struct stream_info { + uint64_t total_packets; ///< The total number of packets detected + uint32_t encapsulation_protocol; ///< The encapsulation protocol for the pcap file + + ts timestamp_max; ///< The latest timestamp detected + ts timestamp_min; ///< The earliest timestamp detected + + std::map udp_streams; ///< Datastructure containing info on all of the different streams +}; + +/** + * To string method for stream info structs. + * + * @param[inout] stream_in The pre-existing ostream to concat with data. + * @param[in] data The stream_info to output. + * + * @return The new output stream containing concatted stream_info and data. + */ +std::ostream& operator<<(std::ostream& stream_in, const stream_info& data); /** Struct to hide the stepwise playback details. */ struct playback_handle; @@ -96,21 +168,6 @@ bool next_packet_info(playback_handle& handle, packet_info& info); */ size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size); -/** - * Initialize the record handle for recording single sensor pcap files. Will be - * removed - * - * @param[in] file The file path to the target pcap to record to. - * @param[in] src_ip The source address to label the packets with. - * @param[in] dst_ip The destination address to label the packets with. - * @param[in] frag_size The size of the fragments for packet fragmentation. - * @param[in] use_sll_encapsulation Whether to use sll encapsulation. - */ -[[deprecated]] std::shared_ptr record_initialize( - const std::string& file, const std::string& src_ip, - const std::string& dst_ip, int frag_size, - bool use_sll_encapsulation = false); - /** * Initialize the record handle for recording multi sensor pcap files. Source * and destination IPs must be provided with each packet. @@ -130,10 +187,11 @@ std::shared_ptr record_initialize( void record_uninitialize(record_handle& handle); /** - * Record a buffer to a single sensor record_handle pcap file. Source - * and destination IPs must be provided during initialization. Will be removed. + * Record a buffer to a multi sensor record_handle pcap file. * * @param[in] handle The record handle that record_initialize has initted. + * @param[in] src_ip The source address to label the packets with. + * @param[in] dst_ip The destination address to label the packets with. * @param[in] src_port The source port to label the packets with. * @param[in] dst_port The destination port to label the packets with. * @param[in] buf The buffer to record to the pcap file. @@ -141,28 +199,80 @@ void record_uninitialize(record_handle& handle); * @param[in] microsecond_timestamp The timestamp to record the packet as * microseconds. */ -[[deprecated]] void record_packet(record_handle& handle, int src_port, - int dst_port, const uint8_t* buf, - size_t buffer_size, - uint64_t microsecond_timestamp); +void record_packet(record_handle& handle, const std::string& src_ip, + const std::string& dst_ip, int src_port, int dst_port, + const uint8_t* buf, size_t buffer_size, + uint64_t microsecond_timestamp); /** * Record a buffer to a multi sensor record_handle pcap file. * * @param[in] handle The record handle that record_initialize has initted. - * @param[in] src_ip The source address to label the packets with. - * @param[in] dst_ip The destination address to label the packets with. - * @param[in] src_port The source port to label the packets with. - * @param[in] dst_port The destination port to label the packets with. + * @param[in] info The packet_info object to use for the packet. * @param[in] buf The buffer to record to the pcap file. * @param[in] buffer_size The size of the buffer to record to the pcap file. - * @param[in] microsecond_timestamp The timestamp to record the packet as - * microseconds. */ -void record_packet(record_handle& handle, const std::string& src_ip, - const std::string& dst_ip, int src_port, int dst_port, - const uint8_t* buf, size_t buffer_size, - uint64_t microsecond_timestamp); +void record_packet(record_handle& handle, const packet_info& info, + const uint8_t* buf, size_t buffer_size); +/** + * Return the information about network streams in a pcap file. + * + * @param[in] file The pcap file to read. + * @param[in] packets_to_process Number of packets to process < 0 for all of them + * + * @return A pointer to the resulting stream_info + */ +std::shared_ptr get_stream_info(const std::string& file, int packets_to_process=-1); + +/** + * Return the information about network streams in a pcap file. + * + * @param[in] file The pcap file to read. + * @param[in] progress_callback A callback to invoke after each packet is scanned + * current: The current file offset + * delta: The delta in file offset + * total: The total size of the file + * @param[in] packets_per_callback Callback every n packets + * @param[in] packets_to_process Number of packets to process < 0 for all of them + * + * @return A pointer to the resulting stream_info + */ +std::shared_ptr get_stream_info(const std::string& file, + std::function progress_callback, + int packets_per_callback, + int packets_to_process=-1); + +/** + * Return the information about network streams in a PcapReader and generate indicies (if the PcapReader is an IndexedPcapReader). + * + * @param[in] pcap_reader The PcapReader + * @param[in] sensor_info a set of sensor_info used to parse packets contained in the file + * @param[in] progress_callback A callback to invoke after each packet is scanned + * current: The current file offset + * delta: The delta in file offset + * total: The total size of the file + * @param[in] packets_per_callback Callback every n packets + * @param[in] packets_to_process Number of packets to process < 0 for all of them + * + * @return A pointer to the resulting stream_info + */ +std::shared_ptr get_stream_info(PcapReader& pcap_reader, + std::function progress_callback, + int packets_per_callback, + int packets_to_process=-1); +/** + * Return a guess of the correct ports located in a pcap file. + * + * @param[in] info The stream_info structure generated from a specific pcap file + * @param[in] lidar_packet_sizes The size of the lidar packets + * @param[in] imu_packet_sizes The size of the imu packets + * @param[in] lidar_spec The expected lidar port from the metadata(pass 0 for unknown) + * @param[in] imu_spec The expected imu port from the metadata(pass 0 for unknown) + * + * @return A vector (sorted by most likely to least likely) of the guessed ports + */ +std::vector guess_ports(stream_info &info, int lidar_packet_size, int imu_packet_size, + int expected_lidar_port, int expected_imu_port); } // namespace sensor_utils } // namespace ouster diff --git a/ouster_pcap/include/ouster/pcap.h b/ouster_pcap/include/ouster/pcap.h new file mode 100644 index 00000000..85fd2348 --- /dev/null +++ b/ouster_pcap/include/ouster/pcap.h @@ -0,0 +1,204 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace ouster { +namespace sensor_utils { + +struct pcap_impl; +struct pcap_writer_impl; + +static constexpr int IANA_UDP = 17; + +struct packet_info { + using ts = std::chrono::microseconds; + + // TODO: use numerical IPs for efficient filtering + std::string dst_ip; ///< The destination IP + std::string src_ip; ///< The source IP + int dst_port; ///< The destination port + int src_port; ///< The source port + size_t payload_size; ///< The size of the packet payload + size_t packet_size; ///< The size of the full packet + ts timestamp; ///< The packet capture timestamp + int fragments_in_packet; ///< Number of fragments in the packet + int ip_version; ///< The ip version, 4 or 6 + int encapsulation_protocol; ///< PCAP encapsulation type + uint64_t file_offset; ///< Where the packet is in the pcap + // TODO: remove, library ignores non-UDP packes + int network_protocol; ///< IANA protocol number. Always 17 (UDP) +}; + +/** + * Class for dealing with reading pcap files + */ +class PcapReader { + std::unique_ptr impl; ///< Private implementation pointer + packet_info info; ///< Cached packet info + std::map fragment_count; ///< Map to count fragments per packet + uint8_t* data; ///< Cached packet data + + public: + /** + * @param file[in] A filepath of the pcap to read + */ + PcapReader(const std::string& file); + virtual ~PcapReader(); + + /** + * Advances to the next packet and returns the size of that packet. + * Will also populate data and info for next_packet(), current_data(), + * current_length(), and current_info() + * + * @return The size of the packet payload + */ + size_t next_packet(); + + /** + * Return the current packets data. + * To advance to a new packet please use next_packet() + * To get the size of the data use current_length() + * + * @return A pointer to a byte array containing the packet data + */ + const uint8_t* current_data() const; + + /** + * Return the current packets data size. + * To advance to a new packet please use next_packet() + * + * @return The size of the byte array + */ + size_t current_length() const; + + /** + * Return the current packets info. + * To advance to a new packet please use next_packet() + * + * @return A packet_info object on the current packet + */ + const packet_info& current_info() const; + + /** + * @return The size of the PCAP file in bytes + */ + uint64_t file_size() const; + + /** + * Return the read position to the start of the PCAP file + */ + void reset(); + + /** + * Seek to the position in the file represented by the + * number of bytes from the beginning of the file. + * + * @param offset[in] The position to seek to in bytes, + * starting from the beginning of the file. + * + * @pre \paramname{offset} must be the offset of a PCAP + * record header. If any other value is provided, + * subsequent packet reads from this PcapReader will be + * invalid until \functionname{reset} is called. + */ + void seek(uint64_t offset); + + private: + uint64_t file_size_{}; + uint64_t file_start_{}; +}; + +/** + * Class for dealing with writing udp pcap files + */ +class PcapWriter { + public: + /** + * Enum to describe the current encapsulation for a pcap file + */ + enum PacketEncapsulation { + NULL_LOOPBACK = 0x0, ///< Null Loopback Encapsulation + ETHERNET = 0x1, ///< Ethernet II Encapsulation + SLL = 0x71, ///< Linux Cooked Capture Encapsulation + }; + + /** + * @param file[in] The file path to write the pcap to + * @param encap[in] The encapsulation to use for the pcap + * @param frag_size[in] The fragmentation size to use (Currently broken) + */ + PcapWriter(const std::string& file, PacketEncapsulation encap, + uint16_t frag_size); + virtual ~PcapWriter(); + + /** + * Write a packet using a buffer to the pcap + * + * @param buf[in] The buffer to write + * @param buf_size[in] The size of the buffer to write + * @param src_ip[in] The source ip address to use for the packet + * @param dst_ip[in] The destination ip address to use for the packet + * @param src_port[in] The source port number to use for the packet + * @param dst_port[in] The destination port number to use for the packet + * @param timestamp[in] The timestamp of the packet to record + * @note The timestamp parameter does not affect the order of packets being + * recorded, it is strictly recorded FIFO. + */ + void write_packet(const uint8_t* buf, size_t buf_size, + const std::string& src_ip, const std::string& dst_ip, + uint16_t src_port, uint16_t dst_port, + packet_info::ts timestamp); + + /** + * Write a packet using a buffer to the pcap + * + * @param buf[in] The buffer to write + * @param buf_size[in] The size of the buffer to write + * @param info[in] The packet info object to use for the recording + * parameters + * @note The timestamp parameter in info does not affect the order of + * packets being recorded, it is strictly recorded FIFO. + */ + void write_packet(const uint8_t* buf, size_t buf_size, + const packet_info& info); + + /** + * Write all pending data to the pcap file + */ + void flush(); + + /** + * Flushes and cleans up all memory in use by the pap writer + */ + void close(); + + protected: + std::unique_ptr impl; ///< Internal data + + uint16_t id; ///< An incrementing id to record packets with + PacketEncapsulation encap; ///< Encapsulation to record with + uint16_t frag_size; ///< Fragmentation size(not currently used) + bool closed; +}; + +/** + * To string method for packet info structs. + * + * @param[inout] stream_in The pre-existing ostream to concat with data. + * @param[in] data The packet_info to output. + * + * @return The new output stream containing concatted stream_in and data. + */ +std::ostream& operator<<(std::ostream& stream_in, const packet_info& data); +} // namespace sensor_utils +} // namespace ouster diff --git a/ouster_pcap/src/indexed_pcap_reader.cpp b/ouster_pcap/src/indexed_pcap_reader.cpp new file mode 100644 index 00000000..1c1eba65 --- /dev/null +++ b/ouster_pcap/src/indexed_pcap_reader.cpp @@ -0,0 +1,92 @@ +#include "ouster/indexed_pcap_reader.h" + +namespace ouster { +namespace sensor_utils { + +IndexedPcapReader::IndexedPcapReader( + const std::string& pcap_filename, + const std::vector& metadata_filenames, + std::function progress_callback) + : PcapReader(pcap_filename) + , frame_indices_(metadata_filenames.size()) + , previous_frame_ids_(metadata_filenames.size()) +{ + for(const std::string& metadata_filename : metadata_filenames) { + sensor_infos_.push_back( + ouster::sensor::metadata_from_json(metadata_filename) + ); + } + stream_info_ = ouster::sensor_utils::get_stream_info(*this, progress_callback, 256, -1); + if(sensor_infos_.size() == 1 && sensor_infos_[0].udp_port_lidar == 0) { + // guess lidar port for a single sensor if it is unspecified in sensor info + const ouster::sensor::packet_format& pf = ouster::sensor::packet_format(sensor_infos_[0]); + std::vector ports = guess_ports( + *stream_info_, + pf.lidar_packet_size, pf.imu_packet_size, + sensor_infos_[0].udp_port_lidar, sensor_infos_[0].udp_port_imu + ); + if(ports.empty()) { + throw std::runtime_error("IndexedPcapReader: unable to determine lidar UDP port"); + } + sensor_infos_[0].udp_port_lidar = ports[0].lidar; + sensor_infos_[0].udp_port_imu = ports[0].imu; + } +} + +nonstd::optional IndexedPcapReader::sensor_idx_for_current_packet() const { + const auto& pkt_info = current_info(); + for(size_t i = 0; i < sensor_infos_.size(); i++) { + if(pkt_info.dst_port == sensor_infos_[i].udp_port_lidar) { + // TODO use the packet format and match serial number if it's available + // this will allow us to have multiple sensors on the same port + return i; + } + } + return nonstd::nullopt; +} + +nonstd::optional IndexedPcapReader::current_frame_id() const { + if(nonstd::optional sensor_idx = sensor_idx_for_current_packet()) { + const ouster::sensor::packet_format& pf = ouster::sensor::packet_format( + sensor_infos_[*sensor_idx] + ); + return pf.frame_id(current_data()); + } + return nonstd::nullopt; +} + +bool IndexedPcapReader::frame_id_rolled_over(uint16_t previous, uint16_t current) { + static_assert(sizeof(uint16_t) == 2, "expected frame_id to be two bytes"); + return previous > 0xff00 && current < 0x00ff; +} + +void IndexedPcapReader::update_index_for_current_packet() { + if(nonstd::optional sensor_info_idx = sensor_idx_for_current_packet()) { + if(nonstd::optional frame_id = current_frame_id()) { + if(!previous_frame_ids_[*sensor_info_idx] + || *previous_frame_ids_[*sensor_info_idx] < *frame_id // frame_id is greater than previous + || frame_id_rolled_over(*previous_frame_ids_[*sensor_info_idx], *frame_id) + ) { + frame_indices_[*sensor_info_idx].push_back(current_info().file_offset); + previous_frame_ids_[*sensor_info_idx] = *frame_id; + } + } + } +} + +void IndexedPcapReader::seek_to_frame(size_t sensor_index, unsigned int frame_number) { + seek(frame_indices_.at(sensor_index).at(frame_number)); +} + + +size_t IndexedPcapReader::frame_count(size_t sensor_index) const { + return frame_indices_.at(sensor_index).size(); +} + +std::shared_ptr IndexedPcapReader::get_stream_info() const { + return stream_info_; +} + +} // namespace sensor_utils +} // namespace ouster + diff --git a/ouster_pcap/src/os_pcap.cpp b/ouster_pcap/src/os_pcap.cpp index 687c178e..602abf68 100644 --- a/ouster_pcap/src/os_pcap.cpp +++ b/ouster_pcap/src/os_pcap.cpp @@ -10,45 +10,35 @@ #include #include #include -#include #include +#include +#include +#include -#include +#include -using namespace Tins; +#include "ouster/pcap.h" +#include "ouster/indexed_pcap_reader.h" namespace ouster { namespace sensor_utils { -static constexpr int PROTOCOL_UDP = 17; - struct record_handle { - std::string dst_ip; ///< The destination IP - std::string src_ip; ///< The source IP - std::string file_name; ///< The filename of the output pcap file - size_t frag_size; ///< The size of the udp data fragmentation - std::unique_ptr - pcap_file_writer; ///< Object that holds the pcap writer - bool use_sll_encapsulation; - record_handle() {} - - ~record_handle() {} + record_handle(const std::string& path, + PcapWriter::PacketEncapsulation encap, uint16_t frag_size) + : writer{std::make_unique(path, encap, frag_size)} {} + + std::unique_ptr writer; }; struct playback_handle { - std::string file_name; ///< The filename of the pcap file - - std::unique_ptr - pcap_reader; ///< Object that holds the unified pcap reader - Tins::Packet packet_cache; - bool have_new_packet; - - Tins::IPv4Reassembler - reassembler; ///< The reassembler mainly for lidar packets + std::string path; + std::unique_ptr pcap; - int encap_proto; + playback_handle(const std::string& path) + : path{path}, pcap{std::make_unique(path)} {} - playback_handle() {} + playback_handle& operator=(playback_handle&& other) = default; ~playback_handle() {} }; @@ -72,290 +62,336 @@ std::ostream& operator<<(std::ostream& stream_in, const packet_info& data) { return stream_in; } -std::shared_ptr replay_initialize( - const std::string& file_name) { - std::shared_ptr result = - std::make_shared(); +bool stream_key::operator==(const struct stream_key &other) const { + return dst_ip == other.dst_ip && + src_ip == other.src_ip && + src_port == other.src_port && + dst_port == other.dst_port; +} - result->file_name = file_name; - result->pcap_reader.reset(new FileSniffer(file_name)); - result->encap_proto = result->pcap_reader->link_type(); +bool stream_key::operator!=(const struct stream_key &other) const { + return !(*this == other); +} - return result; +bool stream_key::operator<=(const struct stream_key &other) const { + return dst_ip <= other.dst_ip && + src_ip <= other.src_ip && + dst_port <= other.dst_port && + src_port <= other.src_port; } -void replay_uninitialize(playback_handle& handle) { - handle.pcap_reader.reset(); +bool stream_key::operator>=(const struct stream_key &other) const { + return dst_ip >= other.dst_ip && + src_ip >= other.src_ip && + dst_port >= other.dst_port && + src_port >= other.src_port; } -void replay_reset(playback_handle& handle) { - handle.pcap_reader.reset(new FileSniffer(handle.file_name)); +bool stream_key::operator<(const struct stream_key &other) const { + return *this <= other && *this != other; } -bool next_packet_info(playback_handle& handle, packet_info& info) { - bool result = false; - - bool reassm = false; - int reassm_packets = 0; - while (!reassm) { - reassm_packets++; - handle.packet_cache = handle.pcap_reader->next_packet(); - if (handle.packet_cache) { - auto pdu = handle.packet_cache.pdu(); - if (pdu) { - IP* ip = pdu->find_pdu(); - IPv6* ipv6 = pdu->find_pdu(); - // Using short circuiting here - if ((ip && ip->protocol() == PROTOCOL_UDP) || - (ipv6 && ipv6->next_header() == PROTOCOL_UDP)) { - // reassm is also used in the while loop - reassm = (handle.reassembler.process(*pdu) != - IPv4Reassembler::FRAGMENTED); - if (reassm) { - info.fragments_in_packet = reassm_packets; - reassm_packets = 0; - - info.encapsulation_protocol = handle.encap_proto; - result = true; - UDP* udp = pdu->find_pdu(); - auto raw = pdu->find_pdu(); - if (ip) { - info.dst_ip = ip->dst_addr().to_string(); - info.src_ip = ip->src_addr().to_string(); - info.ip_version = 4; - info.network_protocol = ip->protocol(); - } else if (ipv6) { - info.dst_ip = ipv6->dst_addr().to_string(); - info.src_ip = ipv6->src_addr().to_string(); - info.ip_version = 6; - info.network_protocol = ipv6->next_header(); - } else { - throw std::runtime_error( - "Malformed packet: no IP headers"); - } - // find_pdu will only return NULL when the ipv4 - // reassembly succeeds on an ipv6 packet, leading to a - // malformed packet - if (udp != NULL) { - info.dst_port = udp->dport(); - info.src_port = udp->sport(); - info.payload_size = raw->payload_size(); - info.timestamp = handle.packet_cache.timestamp(); - handle.have_new_packet = true; - } else { - throw std::runtime_error( - "Malformed Packet: No UDP Detected"); - } - } - } - } - } else { - reassm = true; - } +bool stream_key::operator>(const struct stream_key &other) const { + return *this >= other && *this != other; +} + +std::ostream& operator<<(std::ostream& stream_in, const stream_key& data) { + stream_in << "Source IP: \"" << data.src_ip << "\" " << std::endl; + stream_in << "Destination IP: \"" << data.dst_ip << "\" " << std::endl; + stream_in << "Source Port: " << data.src_port << std::endl; + stream_in << "Destination Port: " << data.dst_port << std::endl; + return stream_in; +} + +std::ostream& operator<<(std::ostream& stream_in, const stream_data& data) { + stream_in << "Count: " << data.count << " "; + stream_in << "Payload Sizes: " << std::endl; + for(auto const& it : data.payload_size_counts) { + stream_in << "Size: " << it.first << " Count: " << it.second << std::endl; } - return result; + stream_in << "Fragments In Packets: " << std::endl; + for(auto const& it : data.fragment_counts) { + stream_in << "Number of Fragments: "<< it.first << " Count: " << it.second << std::endl; + } + + stream_in << "IP Versions: " << std::endl; + for(auto const& it : data.ip_version_counts) { + stream_in << "IP Version: " << it.first << " Count: " << it.second << std::endl; + } + + return stream_in; } -size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size) { - size_t result = 0; - if (handle.have_new_packet) { - result = true; - auto pdu = handle.packet_cache.pdu(); - auto raw = pdu->find_pdu(); - if (raw) { - auto temp = (uint32_t*)&(raw->payload()[0]); - auto size = raw->payload_size(); - if (size > buffer_size) { - throw std::invalid_argument( - "Incompatible argument: expected a bytearray of " - "size > " + - std::to_string(size)); - } else { - handle.have_new_packet = false; - result = size; - memcpy(buf, temp, size); - } - } +std::ostream& operator<<(std::ostream& stream_in, const stream_info& data) +{ + stream_in << "Total Packets: " << data.total_packets << std::endl; + stream_in << "Encapsultion Protocol: " << data.encapsulation_protocol << std::endl; + stream_in << "Max Timestamp: " << data.timestamp_max.count() << std::endl; + stream_in << "Min Timestamp: " << data.timestamp_min.count() << std::endl; + + for(auto it : data.udp_streams) + { + stream_in << "Key: " << std::endl << it.first << std::endl; + stream_in << "Data: " << std::endl << it.second << std::endl; + stream_in << std::endl << std::endl << std::endl; } + return stream_in; +} - return result; +std::shared_ptr replay_initialize( + const std::string& file_path) { + return std::make_shared(file_path); } -std::shared_ptr record_initialize(const std::string& file_name, - const std::string& src_ip, - const std::string& dst_ip, - int frag_size, - bool use_sll_encapsulation) { - std::shared_ptr result = std::make_shared(); - - result->file_name = file_name; - result->frag_size = frag_size; - result->src_ip = src_ip; - result->dst_ip = dst_ip; - result->use_sll_encapsulation = use_sll_encapsulation; - if (use_sll_encapsulation) { - result->pcap_file_writer.reset( - new PacketWriter(file_name, DataLinkType())); +void replay_uninitialize(playback_handle& handle) { handle.pcap.reset(); } + +void replay_reset(playback_handle& handle) { + handle = playback_handle{handle.path}; +} + +bool next_packet_info(playback_handle& handle, packet_info& info) { + if (handle.pcap && handle.pcap->next_packet()) { + info = handle.pcap->current_info(); + return true; + } + return false; +} + +size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size) { + size_t len = handle.pcap->current_length(); + if (len <= buffer_size) { + std::memcpy(buf, handle.pcap->current_data(), len); } else { - result->pcap_file_writer.reset( - new PacketWriter(file_name, DataLinkType())); + len = 0; } - return result; + return len; } std::shared_ptr record_initialize(const std::string& file_name, int frag_size, bool use_sll_encapsulation) { - return record_initialize(file_name, "", "", frag_size, - use_sll_encapsulation); + PcapWriter::PacketEncapsulation encap = + (use_sll_encapsulation) ? PcapWriter::PacketEncapsulation::SLL + : PcapWriter::PacketEncapsulation::ETHERNET; + return std::make_shared(file_name, encap, frag_size); } -void record_uninitialize(record_handle& handle) { - if (handle.pcap_file_writer) handle.pcap_file_writer.reset(); -} +void record_uninitialize(record_handle& handle) { handle.writer.reset(); } -/* - * This was a tricky problem, due to how the ip stack is set up. - * - * SLL Container -> ipv4 container -> udp container -> raw data - * - * The ipv4 container is what does the packet fragmentation and reassembly. - * With each full packet we need the ipv4 reassembly to contain only one udp -header. -* Due to this, we have to only create one official Tins::UDP packet. -* We grab the packet id from this packet -* Every packet after the first Tins::UDP packet needs to just be a ipv4 -* container with a manually set packet type of UDP(dec 17) -* Every packet but the final packet needs to have the current flag set: -* -* pkt.flags(IP::MORE_FRAGMENTS); -* -*/ +void record_packet(record_handle& handle, const packet_info& info, + const uint8_t* buf, size_t buffer_size) { + handle.writer->write_packet(buf, buffer_size, info); +} -// SLL is the linux pcap capture container -std::vector buffer_to_frag_packets(record_handle& handle, - const std::string& src_ip, - const std::string& dst_ip, int src_port, - int dst_port, const uint8_t* buf, - size_t buf_size) { - std::vector result; - - int id = -1; ///< This variable is used to track the packet id, - ///< if -1 then create a packet and grab its id - size_t i = 0; ///< Loop variable that contains current bytes processed - size_t offset_modifier = - 0; ///< This variable contains the offset to account - ///< for the udp packet with the fragment_offset - - while (i < buf_size) { - // First create the ipv4 packet - IP pkt = IP(dst_ip, src_ip); - - // Now figure out the size of the packet payload - size_t size = std::min(handle.frag_size, (buf_size - i)); - - // Correctly set this packets fragment offset - // NOTE: for some reason this has to be divided by 8 - // NOTE: Reference here - // http://libtins.github.io/docs/latest/dd/d3f/classTins_1_1IP.html#a32a6bf84af274748317ef61ce1a91ce5 - pkt.fragment_offset((i + offset_modifier) / 8); - - // If this is the first packet - if (i == 0) { - // Fully create the libtins udp structure - auto udp = UDP(dst_port, src_port); - - if ((size + udp.header_size()) > handle.frag_size) { - // Due to the udp header being included in the payload, - // we need to subtract its size from the payload - size -= udp.header_size(); - // Set the "There is more data to follow" flag - pkt.flags(IP::MORE_FRAGMENTS); - } +void record_packet(record_handle& handle, const std::string& src_ip, + const std::string& dst_ip, int src_port, int dst_port, + const uint8_t* buf, size_t buffer_size, + uint64_t microsecond_timestamp) { + if (!handle.writer) return; - // Pack what we can minus the udp header size into the payload - pkt /= udp / RawPDU((uint8_t*)(buf + i), size); + auto time = packet_info::ts{microsecond_timestamp}; + handle.writer->write_packet(buf, buffer_size, src_ip, dst_ip, src_port, + dst_port, time); +} - // Manually set the ipv4 protocol to UDP - pkt.protocol(PROTOCOL_UDP); +// TODO: make a member of `PcapReader` ? +std::shared_ptr get_stream_info(PcapReader& pcap_reader, + std::function progress_callback, + int packets_per_callback, + int packets_to_process) { + uint64_t fileSize = pcap_reader.file_size(); + std::shared_ptr result = std::make_shared(); + + int callback_count = 0; + uint64_t last_current = 0; + uint64_t diff_acc = 0; + + int i = 0; + packet_info info; + bool first = true; + uint64_t prev_location = 0; + while(((packets_to_process <= 0) || (i < packets_to_process)) + && pcap_reader.next_packet()) + { + info = pcap_reader.current_info(); + + // TODO: if `pcap_reader` is an IndexedPcapReader, + // get the `sensor_info` that matches the packet + // and the `packet_format` from the `sensor_info` + // and possibly use a `ScanBatcher` (one for each sensor stream) + // to determine if a scan boundary has been found + if(IndexedPcapReader* indexed_pcap_reader_ptr = dynamic_cast(&pcap_reader)) { + indexed_pcap_reader_ptr->update_index_for_current_packet(); + } - // Set the fragment_offset offset with the size of the udp header - offset_modifier = udp.header_size(); + callback_count++; + if(first) + { + first = false; + result->encapsulation_protocol = info.encapsulation_protocol; + result->timestamp_max = info.timestamp; + result->timestamp_min = info.timestamp; } - // This is a packet in the middle or end - else { - // Set the "There is more data to follow" flag - if (i + size < buf_size) pkt.flags(IP::MORE_FRAGMENTS); + result->total_packets++; - // Manually set the ipv4 protocol to UDP - pkt.protocol(PROTOCOL_UDP); + if(info.timestamp < result->timestamp_min) result->timestamp_min = info.timestamp; + if(info.timestamp > result->timestamp_max) result->timestamp_max = info.timestamp; - // Pack what we can into the payload - pkt /= RawPDU((uint8_t*)(buf + i), size); - } + stream_key key; - // Here is where we correctly set the packet id - if (id < 0) { - // If this is the first packet, set id to the generated packet id - id = pkt.id(); - } else { - // If this is a following packet, use the first packets id - pkt.id(id); - } + key.dst_ip = info.dst_ip; + key.src_ip = info.src_ip; + key.dst_port = info.dst_port; + key.src_port = info.src_port; - // Add the resulting packet to the vector - result.push_back(pkt); + auto& stream = result->udp_streams[key]; + stream.count++; + stream.payload_size_counts[info.payload_size]++; + stream.fragment_counts[info.fragments_in_packet]++; + stream.ip_version_counts[info.ip_version]++; - // Increment the current byte being processed - i += size; + diff_acc += (info.file_offset - prev_location); + last_current = info.file_offset; + + if( callback_count > packets_per_callback + && packets_per_callback >= 0) + { + progress_callback(info.file_offset, diff_acc, fileSize); + diff_acc = 0; + callback_count = 0; + } + prev_location = info.file_offset; + i++; } + if( diff_acc > 0 && packets_per_callback >= 0) + { + progress_callback(last_current, diff_acc, fileSize); + } + pcap_reader.reset(); return result; } -void record_packet(record_handle& handle, int src_port, int dst_port, - const uint8_t* buf, size_t buffer_size, - uint64_t microsecond_timestamp) { - record_packet(handle, handle.src_ip, handle.dst_ip, src_port, dst_port, buf, - buffer_size, microsecond_timestamp); +std::shared_ptr get_stream_info(const std::string& file, + std::function progress_callback, + int packets_per_callback, + int packets_to_process) +{ + auto handle = replay_initialize(file); + if(handle) { + return get_stream_info( + *(handle->pcap), + progress_callback, + packets_per_callback, + packets_to_process + ); + } + return std::make_shared(); } -void record_packet(record_handle& handle, const std::string& src_ip, - const std::string& dst_ip, int src_port, int dst_port, - const uint8_t* buf, size_t buffer_size, - uint64_t microsecond_timestamp) { - // ensure IPs were provided - if (dst_ip.empty() || src_ip.empty()) { - throw std::invalid_argument("Invalid addresses provided for packet"); +std::shared_ptr get_stream_info(const std::string& file, int packets_to_process) +{ + return get_stream_info(file, + [] (uint64_t, uint64_t, uint64_t) {}, + -1, + packets_to_process); +} +/* + The current approach is roughly: 1) treat each unique source / destination + port and IP as a single logical 'stream' of data, 2) filter out streams that + don't match the expected packet sizes specified by the metadata, 3) pair up + any potential lidar/imu streams that appear to be coming from the same + sensor (have matching source IPs) 4) and finally, filter out the pairs that + contradict any ports specified in the metadata. +*/ +std::vector guess_ports(stream_info &info, + int lidar_packet_sizes, int imu_packet_sizes, + int lidar_spec, int imu_spec) +{ + std::vector temp_result; + std::vector lidar_result; + std::vector imu_result; + std::vector result; + + std::vector lidar_keys; + std::vector imu_keys; + std::vector lidar_src_ips; + std::vector imu_src_ips; + + for(auto it : info.udp_streams) + { + if(it.second.payload_size_counts.count(lidar_packet_sizes) > 0) + { + lidar_keys.push_back(it.first); + lidar_src_ips.push_back(it.first.src_ip); + } + if(it.second.payload_size_counts.count(imu_packet_sizes) > 0) + { + imu_keys.push_back(it.first); + imu_src_ips.push_back(it.first.src_ip); + } + } + + // Full join on lidar and IMU + for(auto lidar_it : lidar_keys) + { + bool imu_processed = false; + for(auto imu_it : imu_keys) + { + // This case runs for when we have both Lidar and IMU data + if(lidar_it.src_ip == imu_it.src_ip) + { + guessed_ports ports; + ports.lidar = lidar_it.dst_port; + ports.imu = imu_it.dst_port; + imu_processed = true; + temp_result.push_back(ports); + } + // This case runs if we just have an IMU unmatched with the lidar + else if(std::find(lidar_src_ips.begin(), lidar_src_ips.end(), + imu_it.src_ip) == lidar_src_ips.end()) + { + guessed_ports ports; + ports.lidar = 0; + ports.imu = imu_it.dst_port; + imu_result.push_back(ports); + } + } + // This case runs if we just have Lidar data + if(!imu_processed && std::find(imu_src_ips.begin(), imu_src_ips.end(), + lidar_it.src_ip) == imu_src_ips.end()) + { + guessed_ports ports; + ports.lidar = lidar_it.dst_port; + ports.imu = 0; + lidar_result.push_back(ports); + } } - // For each of the packets write it to the pcap file - for (auto item : buffer_to_frag_packets(handle, src_ip, dst_ip, src_port, - dst_port, buf, buffer_size)) { - Packet packet; - PDU* pdu; - if (handle.use_sll_encapsulation) { - pdu = new SLL(); - } else { - pdu = new EthernetII(); + // This case runs if we just have IMU data + if(lidar_keys.empty()) + { + for(auto imu_it : imu_keys) + { + guessed_ports ports; + ports.lidar = 0; + ports.imu = imu_it.dst_port; + imu_result.push_back(ports); } - *pdu /= item; - // Nasty libtins bug that causes write to fail - // https://www.gitmemory.com/issue/mfontanini/libtins/348/488141933 - auto _ = pdu->serialize(); - /* - * The next block is due to the fact that the previous serialize does - * not treat the udp packet as if it were decodable. Manually tell - * libtins to go in and serialize the udp packet as well - */ - if (pdu->inner_pdu()->inner_pdu()->inner_pdu() != NULL) { - _ = pdu->inner_pdu()->inner_pdu()->inner_pdu()->serialize(); + } + + temp_result.insert(temp_result.end(), lidar_result.begin(), lidar_result.end()); + temp_result.insert(temp_result.end(), imu_result.begin(), imu_result.end()); + + for(auto it : temp_result) + { + if(((it.lidar == lidar_spec) || (lidar_spec == 0) || (it.lidar == 0)) && + ((it.imu == imu_spec) || (imu_spec == 0) || (it.imu == 0))) + { + result.push_back(it); } - packet = Packet( - *pdu, Timestamp(std::chrono::microseconds{microsecond_timestamp})); - handle.pcap_file_writer->write(packet); - delete pdu; } + return result; } } // namespace sensor_utils diff --git a/ouster_pcap/src/pcap.cpp b/ouster_pcap/src/pcap.cpp new file mode 100644 index 00000000..8f5130dd --- /dev/null +++ b/ouster_pcap/src/pcap.cpp @@ -0,0 +1,356 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @TODO check that the header casting is idiomatic libpcap + * @TODO warn on dropped packets when pcap contains garbage, when fragments + * missing, buffer reused before sending + * @TODO split up reading / playback + * @TODO improve error reporting + */ + +#include "ouster/pcap.h" + +#if defined _WIN32 +#include +#else +#include // inet_ntop +#include // timeval +#endif + +#include +#if defined _WIN32 +#include +#else +#include +#endif +#include +#include +#include + +#include +#include +#include +#include +#include + +using us = std::chrono::microseconds; +using timepoint = std::chrono::system_clock::time_point; +using namespace Tins; + +static constexpr size_t UDP_BUF_SIZE = 65535; +static constexpr int PROTOCOL_UDP = 17; + +namespace ouster { +namespace sensor_utils { + +struct pcap_impl { + pcap_t* handle; + std::unique_ptr + pcap_reader; ///< Object that holds the unified pcap reader + FILE* pcap_reader_internals; + Tins::Packet packet_cache; + Tins::IPv4Reassembler + reassembler; ///< The reassembler mainly for lidar packets + bool have_new_packet; + int encap_proto; +}; + +struct pcap_writer_impl { + pcap_t* handle; + pcap_dumper* dumper; + std::unique_ptr + pcap_file_writer; ///< Object that holds the pcap writer +}; + +PcapReader::PcapReader(const std::string& file) : impl(new pcap_impl) { + std::ifstream fileSizeStream(file, std::ios::binary); + if (fileSizeStream) { + fileSizeStream.seekg(0, std::ios::end); + file_size_ = fileSizeStream.tellg(); + } + impl->pcap_reader = std::make_unique(file); + impl->encap_proto = impl->pcap_reader->link_type(); + impl->pcap_reader_internals = + pcap_file(impl->pcap_reader->get_pcap_handle()); + file_start_ = ftell(impl->pcap_reader_internals); +} + +PcapReader::~PcapReader() {} + +const uint8_t* PcapReader::current_data() const { return data; } + +size_t PcapReader::current_length() const { return info.payload_size; } + +const packet_info& PcapReader::current_info() const { return info; } + +void PcapReader::seek(uint64_t offset) { + if(offset < sizeof(struct pcap_file_header)) { + offset = sizeof(struct pcap_file_header); + } + if(fseek(impl->pcap_reader_internals, offset, SEEK_SET)) { + throw std::runtime_error("pcap seek failed"); + } +} + +uint64_t PcapReader::file_size() const { + return file_size_; +} + +void PcapReader::reset() { + seek(file_start_); +} + +size_t PcapReader::next_packet() { + size_t result = 0; + + bool reassm = false; + int reassm_packets = 0; + while (!reassm) { + reassm_packets++; + info.file_offset = ftell(impl->pcap_reader_internals); + impl->packet_cache = impl->pcap_reader->next_packet(); + if (impl->packet_cache) { + auto pdu = impl->packet_cache.pdu(); + if (pdu) { + info.packet_size = pdu->size(); + IP* ip = pdu->find_pdu(); + IPv6* ipv6 = pdu->find_pdu(); + // Using short circuiting here + if ((ip && ip->protocol() == PROTOCOL_UDP) || + (ipv6 && ipv6->next_header() == PROTOCOL_UDP)) { + // reassm is also used in the while loop + reassm = (impl->reassembler.process(*pdu) != + IPv4Reassembler::FRAGMENTED); + if (reassm) { + info.fragments_in_packet = reassm_packets; + info.encapsulation_protocol = impl->encap_proto; + + if (ip) { + info.dst_ip = ip->dst_addr().to_string(); + info.src_ip = ip->src_addr().to_string(); + info.ip_version = 4; + info.network_protocol = ip->protocol(); + } else if (ipv6) { + info.dst_ip = ipv6->dst_addr().to_string(); + info.src_ip = ipv6->src_addr().to_string(); + info.ip_version = 6; + info.network_protocol = ipv6->next_header(); + } + + // find_pdu will only return NULL when the ipv4 + // reassembly succeeds on an ipv6 packet, leading to a + // malformed packet + UDP* udp = pdu->find_pdu(); + if (udp != nullptr) { + auto raw = pdu->find_pdu(); + info.dst_port = udp->dport(); + info.src_port = udp->sport(); + info.payload_size = raw->payload_size(); + result = info.payload_size; + data = (uint8_t*)&(raw->payload()[0]); + info.timestamp = impl->packet_cache.timestamp(); + impl->have_new_packet = true; + } else { + throw std::runtime_error( + "Malformed Packet: No UDP Detected"); + } + } + } + } + } else { + reassm = true; + } + } + + return result; +} + +PcapWriter::PcapWriter( + const std::string& file, + PcapWriter::PacketEncapsulation encap = PcapWriter::ETHERNET, + uint16_t frag_size = 1500) + : impl(new pcap_writer_impl), + id{0}, + encap(encap), + frag_size(frag_size), + closed(false) { + if (encap != PcapWriter::ETHERNET) { + impl->pcap_file_writer.reset( + new Tins::PacketWriter((file), Tins::DataLinkType())); + } else { + impl->pcap_file_writer.reset(new Tins::PacketWriter( + (file), Tins::DataLinkType())); + } +} + +void PcapWriter::flush() {} + +void PcapWriter::close() { + flush(); + closed = true; +} + +PcapWriter::~PcapWriter() { close(); } + +/* + * This was a tricky problem, due to how the ip stack is set up. + * + * SLL Container -> ipv4 container -> udp container -> raw data + * + * The ipv4 container is what does the packet fragmentation and reassembly. + * With each full packet we need the ipv4 reassembly to contain only one udp +header. +* Due to this, we have to only create one official Tins::UDP packet. +* We grab the packet id from this packet +* Every packet after the first Tins::UDP packet needs to just be a ipv4 +* container with a manually set packet type of UDP(dec 17) +* Every packet but the final packet needs to have the current flag set: +* +* pkt.flags(IP::MORE_FRAGMENTS); +* +*/ +size_t global_id = 1; +// SLL is the linux pcap capture container +std::vector buffer_to_frag_packets(size_t frag_size, + const std::string& src_ip, + const std::string& dst_ip, int src_port, + int dst_port, const uint8_t* buf, + size_t buf_size) { + /// @todo check fragsize to make sure it is in acceptable bounds + frag_size = UDP_BUF_SIZE; + + std::vector result; + + int id = -1; ///< This variable is used to track the packet id, + ///< if -1 then create a packet and grab its id + size_t i = 0; ///< Loop variable that contains current bytes processed + size_t offset_modifier = + 0; ///< This variable contains the offset to account + ///< for the udp packet with the fragment_offset + + while (i < buf_size) { + // First create the ipv4 packet + IP pkt = IP(dst_ip, src_ip); + + // Now figure out the size of the packet payload + size_t size = std::min(frag_size, (buf_size - i)); + + // Correctly set this packets fragment offset + // NOTE: for some reason this has to be divided by 8 + // NOTE: Reference here + // http://libtins.github.io/docs/latest/dd/d3f/classTins_1_1IP.html#a32a6bf84af274748317ef61ce1a91ce5 + pkt.fragment_offset((i + offset_modifier) / 8); + + // If this is the first packet + if (i == 0) { + // Fully create the libtins udp structure + auto udp = UDP(dst_port, src_port); + + if ((size + udp.header_size()) > frag_size) { + // Due to the udp header being included in the payload, + // we need to subtract its size from the payload + size -= udp.header_size(); + // Set the "There is more data to follow" flag + pkt.flags(IP::MORE_FRAGMENTS); + } + + // Pack what we can minus the udp header size into the payload + pkt /= udp / RawPDU((uint8_t*)(buf + i), size); + + // Manually set the ipv4 protocol to UDP + pkt.protocol(PROTOCOL_UDP); + + // Set the fragment_offset offset with the size of the udp header + offset_modifier = udp.header_size(); + } + // This is a packet in the middle or end + else { + // Set the "There is more data to follow" flag + if (i + size < buf_size) pkt.flags(IP::MORE_FRAGMENTS); + + // Manually set the ipv4 protocol to UDP + pkt.protocol(PROTOCOL_UDP); + + // Pack what we can into the payload + pkt /= RawPDU((uint8_t*)(buf + i), size); + } + + // Here is where we correctly set the packet id + if (id < 0) { + // If this is the first packet, set id to the generated packet id + id = global_id; + pkt.id(id); + global_id++; + } else { + // If this is a following packet, use the first packets id + pkt.id(id); + } + + // Add the resulting packet to the vector + result.push_back(pkt); + + // Increment the current byte being processed + i += size; + } + + return result; +} + +void PcapWriter::write_packet(const uint8_t* buf, size_t buf_size, + const std::string& src_ip, + const std::string& dst_ip, uint16_t src_port, + uint16_t dst_port, packet_info::ts timestamp) { + // ensure IPs were provided + if (dst_ip.empty() || src_ip.empty()) { + throw std::invalid_argument( + "PcapWriter: dst_ip and/or src_ip arguments to write_packet cannot " + "be empty."); + } + // For each of the packets write it to the pcap file + for (auto item : buffer_to_frag_packets(frag_size, src_ip, dst_ip, src_port, + dst_port, buf, buf_size)) { + Packet packet; + PDU* pdu; + switch (encap) { + case PcapWriter::PacketEncapsulation::ETHERNET: + pdu = new Tins::EthernetII(); + break; + case PcapWriter::PacketEncapsulation::SLL: + pdu = new Tins::SLL(); + break; + case PcapWriter::PacketEncapsulation::NULL_LOOPBACK: + throw std::runtime_error( + "PcapWriter: NULL_LOOPBACK packet encapsulation not " + "supported"); + break; + default: + throw std::runtime_error( + "PcapWriter: packet encapsulation not supported"); + } + *pdu /= item; + // Nasty libtins bug that causes write to fail + // https://www.gitmemory.com/issue/mfontanini/libtins/348/488141933 + auto _ = pdu->serialize(); + /* + * The next block is due to the fact that the previous serialize does + * not treat the udp packet as if it were decodable. Manually tell + * libtins to go in and serialize the udp packet as well + */ + if (pdu->inner_pdu()->inner_pdu()->inner_pdu() != NULL) { + _ = pdu->inner_pdu()->inner_pdu()->inner_pdu()->serialize(); + } + packet = Packet(*pdu, timestamp); + impl->pcap_file_writer->write(packet); + delete pdu; + } +} + +void PcapWriter::write_packet(const uint8_t* buf, size_t buf_size, + const packet_info& info) { + write_packet(buf, buf_size, info.src_ip, info.dst_ip, info.src_port, + info.dst_port, info.timestamp); +} + +} // namespace sensor_utils +} // namespace ouster diff --git a/ouster_viz/include/ouster/point_viz.h b/ouster_viz/include/ouster/point_viz.h index c001ddb8..c998b4b8 100644 --- a/ouster_viz/include/ouster/point_viz.h +++ b/ouster_viz/include/ouster/point_viz.h @@ -282,14 +282,34 @@ class PointViz { * * @return viewport width reported by glfw */ - int viewport_width(); + int viewport_width() const; /** * Get a viewport height in pixels. * * @return viewport height reported by glfw */ - int viewport_height(); + int viewport_height() const; + + /** + * Get a window width in screen coordinates. + * + * NOTE: this value maybe different from the viewport size on retina + * displays + * + * @return window width reported by glfw + */ + int window_width() const; + + /** + * Get a window height in screen coordinates. + * + * @note this value maybe different from the viewport size on retina + * displays + * + * @return window height reported by glfw + */ + int window_height() const; private: std::unique_ptr pimpl; @@ -317,6 +337,8 @@ struct WindowCtx { double mouse_y{0}; ///< Current mouse y position int viewport_width{0}; ///< Current viewport width in pixels int viewport_height{0}; ///< Current viewport height in pixels + int window_width{0}; ///< Current window width in screen coordinates + int window_height{0}; ///< Current window height in screen coordinates }; /** @@ -447,7 +469,7 @@ class Camera { /** * Get view offset. * - * @preturn view offset of the camera + * @return view offset of the camera */ vec3d get_view_offset() const; @@ -473,7 +495,7 @@ class Camera { void set_orthographic(bool state); /** - * Get orthographic state. + * Get orthographic state. * * @return true if orthographic, false if perspective */ @@ -496,14 +518,14 @@ class Camera { /** * Directly set camera target object pose - * + * * @param[in] target target where camera is looking at */ void set_target(const mat4d& target); /** * Get the pose of a camera target. - * + * * @return target camera pose */ mat4d get_target() const; @@ -576,6 +598,7 @@ class Cloud { std::vector palette_data_{}; mat4d pose_{}; float point_size_{2}; + bool mono_{true}; Cloud(size_t w, size_t h, const mat4d& extrinsic); @@ -612,14 +635,14 @@ class Cloud { * Resets any changes since the last call to PointViz::update() */ void clear(); - + /** * Set all dirty flags. * * Re-sets everything so the object is always redrawn. */ void dirty(); - + /** * Get the size of the point cloud. * @@ -629,7 +652,7 @@ class Cloud { /** * Get the columns of the point cloud. - * + * * @return number of columns in point cloud. (1 - for unstructured) */ size_t get_cols() const { return w_; } @@ -643,13 +666,37 @@ class Cloud { void set_range(const uint32_t* range); /** - * Set the key values, used for colouring. + * Set the key values, used for coloring. * * @param[in] key pointer to array of at least as many elements as there are * points, preferably normalized between 0 and 1 */ void set_key(const float* key); + /** + * Set the key alpha values, leaving the color the same. + * + * @param[in] key pointer to array of at least as many elements as there are + * points, normalized between 0 and 1 + */ + void set_key_alpha(const float* key_alpha); + + /** + * Set the key values in RGB format, used for coloring. + * + * @param[in] key_rgb pointer to array of at least 3x as many elements as + * there are points, normalized between 0 and 1 + */ + void set_key_rgb(const float* key_rgb); + + /** + * Set the key values in RGBA format, used for coloring. + * + * @param[in] key_rgb pointer to array of at least 4x as many elements as + * there are points, normalized between 0 and 1 + */ + void set_key_rgba(const float* key_rgba); + /** * Set the RGBA mask values, used as an overlay on top of the key. * @@ -722,6 +769,7 @@ class Image { bool position_changed_{false}; bool image_changed_{false}; bool mask_changed_{false}; + bool palette_changed_{false}; vec4f position_{}; size_t image_width_{0}; @@ -730,7 +778,10 @@ class Image { size_t mask_width_{0}; size_t mask_height_{0}; std::vector mask_data_{}; - float hshift_{0}; // in normalized screen coordinates [-1. 1] + std::vector palette_data_{}; + float hshift_{0}; // in normalized screen coordinates [-1. 1] + bool mono_{true}; + bool use_palette_{false}; public: /** @@ -755,6 +806,26 @@ class Image { */ void set_image(size_t width, size_t height, const float* image_data); + /** + * Set the image data (RGB). + * + * @param[in] width width of the image data in pixels + * @param[in] height height of the image data in pixels + * @param[in] image_data pointer to an array of width * height elements + * interpreted as a row-major RGB image + */ + void set_image_rgb(size_t width, size_t height, const float* image_data_rgb); + + /** + * Set the image data (RGBA). + * + * @param[in] width width of the image data in pixels + * @param[in] height height of the image data in pixels + * @param[in] image_data pointer to an array of width * height elements + * interpreted as a row-major RGBA image + */ + void set_image_rgba(size_t width, size_t height, const float* image_data_rgba); + /** * Set the RGBA mask. * @@ -795,13 +866,27 @@ class Image { * -1 - image moved to the left for the 1/2 of a horizontal viewport * +1 - image moved to the right for the 1/2 of a horizontal viewport * +0.5 - image moved to the right for the 1/4 of a horizontal viewport - * + * * @param[in] hshift shift in normalized by width coordinates from 0 at * the center [-1.0..1.0] - * + * */ void set_hshift(float hshift); + /** + * Set the image color palette. + * + * @param[in] palette the new palette to use, must have size 3*palette_size + * @param[in] palette_size the number of colors in the new palette + */ + void set_palette(const float* palette, size_t palette_size); + + /** + * Removes the image color palette. + * + */ + void clear_palette(); + friend class impl::GLImage; }; @@ -917,7 +1002,7 @@ class Label { /** * Set the color of the label. - * + * * @param[in] rgba color in RGBA format */ void set_rgba(const vec4f& rgba); @@ -925,22 +1010,49 @@ class Label { friend class impl::GLLabel; }; /** - * @todo document me + * Spezia palette size in number of colors. */ extern const size_t spezia_n; /** - * @todo document me + * Spezia palette, RGB values per element. */ extern const float spezia_palette[][3]; /** - * @todo document me + * Calibrated reflectifiy palette size in number of colors. */ extern const size_t calref_n; /** - * @todo document me + * Calibrated reflectifiy, RGB values per element. */ extern const float calref_palette[][3]; +/** + * Greyscale palette size in number of colors. + */ +extern const size_t grey_n; +/** + * Greyscale palette, RGB values per element. + */ +extern const float grey_palette[][3]; + +/** + * Viridis palette size in number of colors. + */ +extern const size_t viridis_n; +/** + * Viridis palette, RGB values per element. + */ +extern const float viridis_palette[][3]; + +/** + * Magma palette size in number of colors. + */ +extern const size_t magma_n; +/** + * Magma palette, RGB values per element. + */ +extern const float magma_palette[][3]; + } // namespace viz } // namespace ouster diff --git a/ouster_viz/src/cloud.cpp b/ouster_viz/src/cloud.cpp index f4710ce3..f2a6c804 100644 --- a/ouster_viz/src/cloud.cpp +++ b/ouster_viz/src/cloud.cpp @@ -22,8 +22,8 @@ namespace viz { namespace impl { struct CloudIds { - GLuint xyz_id, off_id, range_id, key_id, mask_id, model_id, proj_view_id, - palette_id, transformation_id, trans_index_id; + GLuint xyz_id, off_id, range_id, key_id, mask_id, model_id, + proj_view_id, mono_id, palette_id, transformation_id, trans_index_id; CloudIds() {} /** @@ -35,10 +35,11 @@ struct CloudIds { : xyz_id(glGetAttribLocation(point_program_id, "xyz")), off_id(glGetAttribLocation(point_program_id, "offset")), range_id(glGetAttribLocation(point_program_id, "range")), - key_id(glGetAttribLocation(point_program_id, "key")), - mask_id(glGetAttribLocation(point_program_id, "mask")), + key_id(glGetAttribLocation(point_program_id, "vkey")), + mask_id(glGetAttribLocation(point_program_id, "vmask")), model_id(glGetUniformLocation(point_program_id, "model")), proj_view_id(glGetUniformLocation(point_program_id, "proj_view")), + mono_id(glGetUniformLocation(point_program_id, "mono")), palette_id(glGetUniformLocation(point_program_id, "palette")), transformation_id( glGetUniformLocation(point_program_id, "transformation")), @@ -181,14 +182,18 @@ void GLCloud::draw(const WindowCtx&, const CameraData& camera, Cloud& cloud) { cloud.range_data_.data(), GL_DYNAMIC_DRAW); cloud.range_changed_ = false; } - + if (cloud.key_changed_) { + mono = cloud.mono_; glBindBuffer(GL_ARRAY_BUFFER, key_buffer); glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * cloud.key_data_.size(), cloud.key_data_.data(), GL_DYNAMIC_DRAW); cloud.key_changed_ = false; } + // put the shader into mono or rgb mode + glUniform1i(GLCloud::cloud_ids.mono_id, mono ? 1 : 0); + glEnableVertexAttribArray(GLCloud::cloud_ids.mask_id); glBindBuffer(GL_ARRAY_BUFFER, mask_buffer); glVertexAttribPointer(GLCloud::cloud_ids.mask_id, @@ -238,12 +243,13 @@ void GLCloud::draw(const WindowCtx&, const CameraData& camera, Cloud& cloud) { 0, // stride (void*)0 // array buffer offset ); + glEnableVertexAttribArray(GLCloud::cloud_ids.key_id); glBindBuffer(GL_ARRAY_BUFFER, key_buffer); glVertexAttribPointer(GLCloud::cloud_ids.key_id, - 1, // size + 4, // size GL_FLOAT, // type - GL_FALSE, // normalized? + GL_FALSE, // normalize 0, // stride (void*)0 // array buffer offset ); @@ -269,9 +275,15 @@ void GLCloud::uninitialize() { glDeleteProgram(GLCloud::program_id); } -void GLCloud::beginDraw() { glUseProgram(GLCloud::program_id); } +void GLCloud::beginDraw() { + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ZERO); + glUseProgram(GLCloud::program_id); +} -void GLCloud::endDraw() {} +void GLCloud::endDraw() { + glDisable(GL_BLEND); +} } // namespace impl } // namespace viz diff --git a/ouster_viz/src/cloud.h b/ouster_viz/src/cloud.h index d079ceb2..0ccf97be 100644 --- a/ouster_viz/src/cloud.h +++ b/ouster_viz/src/cloud.h @@ -41,6 +41,7 @@ class GLCloud { GLuint transform_texture; GLuint palette_texture; GLfloat point_size; + bool mono; Eigen::Matrix4d map_pose; Eigen::Matrix4f extrinsic; diff --git a/ouster_viz/src/colormaps.h b/ouster_viz/src/colormaps.h index 8210dc59..9dee00e8 100644 --- a/ouster_viz/src/colormaps.h +++ b/ouster_viz/src/colormaps.h @@ -541,5 +541,784 @@ const float calref_palette[calref_n][3] = { {0.6365244136870435f, 0.02268358323721642f, 0.2628219915417146f}, {0.6280661284121491f, 0.013302575932333749f, 0.26082276047673913f}}; +const size_t grey_n = 256; +const float grey_palette[grey_n][3] = { + {0.0f, 0.0f, 0.0f}, + {0.00392157f, 0.00392157f, 0.00392157f}, + {0.00784314f, 0.00784314f, 0.00784314f}, + {0.0117647f, 0.0117647f, 0.0117647f}, + {0.0156863f, 0.0156863f, 0.0156863f}, + {0.0196078f, 0.0196078f, 0.0196078f}, + {0.0235294f, 0.0235294f, 0.0235294f}, + {0.027451f, 0.027451f, 0.027451f}, + {0.0313726f, 0.0313726f, 0.0313726f}, + {0.0352941f, 0.0352941f, 0.0352941f}, + {0.0392157f, 0.0392157f, 0.0392157f}, + {0.0431373f, 0.0431373f, 0.0431373f}, + {0.0470588f, 0.0470588f, 0.0470588f}, + {0.0509804f, 0.0509804f, 0.0509804f}, + {0.054902f, 0.054902f, 0.054902f}, + {0.0588235f, 0.0588235f, 0.0588235f}, + {0.0627451f, 0.0627451f, 0.0627451f}, + {0.0666667f, 0.0666667f, 0.0666667f}, + {0.0705882f, 0.0705882f, 0.0705882f}, + {0.0745098f, 0.0745098f, 0.0745098f}, + {0.0784314f, 0.0784314f, 0.0784314f}, + {0.0823529f, 0.0823529f, 0.0823529f}, + {0.0862745f, 0.0862745f, 0.0862745f}, + {0.0901961f, 0.0901961f, 0.0901961f}, + {0.0941176f, 0.0941176f, 0.0941176f}, + {0.0980392f, 0.0980392f, 0.0980392f}, + {0.101961f, 0.101961f, 0.101961f}, + {0.105882f, 0.105882f, 0.105882f}, + {0.109804f, 0.109804f, 0.109804f}, + {0.113725f, 0.113725f, 0.113725f}, + {0.117647f, 0.117647f, 0.117647f}, + {0.121569f, 0.121569f, 0.121569f}, + {0.12549f, 0.12549f, 0.12549f}, + {0.129412f, 0.129412f, 0.129412f}, + {0.133333f, 0.133333f, 0.133333f}, + {0.137255f, 0.137255f, 0.137255f}, + {0.141176f, 0.141176f, 0.141176f}, + {0.145098f, 0.145098f, 0.145098f}, + {0.14902f, 0.14902f, 0.14902f}, + {0.152941f, 0.152941f, 0.152941f}, + {0.156863f,0.156863f,0.156863f}, + {0.160784f,0.160784f,0.160784f}, + {0.164706f,0.164706f,0.164706f}, + {0.168627f,0.168627f,0.168627f}, + {0.172549f, 0.172549f, 0.172549f}, + {0.176471f, 0.176471f, 0.176471f}, + {0.180392f, 0.180392f, 0.180392f}, + {0.184314f, 0.184314f, 0.184314f}, + {0.188235f, 0.188235f, 0.188235f}, + {0.192157f, 0.192157f, 0.192157f}, + {0.196078f, 0.196078f, 0.196078f}, + {0.2f, 0.2f, 0.2f}, + {0.203922f, 0.203922f, 0.203922f}, + {0.207843f, 0.207843f, 0.207843f}, + {0.211765f, 0.211765f, 0.211765f}, + {0.215686f, 0.215686f, 0.215686f}, + {0.219608f, 0.219608f, 0.219608f}, + {0.223529f, 0.223529f, 0.223529f}, + {0.227451f, 0.227451f, 0.227451f}, + {0.231373f, 0.231373f, 0.231373f}, + {0.235294f, 0.235294f, 0.235294f}, + {0.239216f, 0.239216f, 0.239216f}, + {0.243137f, 0.243137f, 0.243137f}, + {0.247059f, 0.247059f, 0.247059f}, + {0.25098f, 0.25098f, 0.25098f}, + {0.254902f, 0.254902f, 0.254902f}, + {0.258824f, 0.258824f, 0.258824f}, + {0.262745f, 0.262745f, 0.262745f}, + {0.266667f, 0.266667f, 0.266667f}, + {0.270588f, 0.270588f, 0.270588f}, + {0.27451f, 0.27451f, 0.27451f}, + {0.278431f, 0.278431f, 0.278431f}, + {0.282353f, 0.282353f, 0.282353f}, + {0.286275f, 0.286275f, 0.286275f}, + {0.290196f, 0.290196f, 0.290196f}, + {0.294118f, 0.294118f, 0.294118f}, + {0.298039f, 0.298039f, 0.298039f}, + {0.301961f, 0.301961f, 0.301961f}, + {0.305882f, 0.305882f, 0.305882f}, + {0.309804f, 0.309804f, 0.309804f}, + {0.313726f, 0.313726f, 0.313726f}, + {0.317647f, 0.317647f, 0.317647f}, + {0.321569f, 0.321569f, 0.321569f}, + {0.32549f, 0.32549f, 0.32549f}, + {0.329412f, 0.329412f, 0.329412f}, + {0.333333f, 0.333333f, 0.333333f}, + {0.337255f, 0.337255f, 0.337255f}, + {0.341176f, 0.341176f, 0.341176f}, + {0.345098f, 0.345098f, 0.345098f}, + {0.34902f, 0.34902f, 0.34902f}, + {0.352941f, 0.352941f, 0.352941f}, + {0.356863f, 0.356863f, 0.356863f}, + {0.360784f, 0.360784f, 0.360784f}, + {0.364706f, 0.364706f, 0.364706f}, + {0.368627f, 0.368627f, 0.368627f}, + {0.372549f, 0.372549f, 0.372549f}, + {0.376471f, 0.376471f, 0.376471f}, + {0.380392f, 0.380392f, 0.380392f}, + {0.384314f, 0.384314f, 0.384314f}, + {0.388235f, 0.388235f, 0.388235f}, + {0.392157f, 0.392157f, 0.392157f}, + {0.396078f, 0.396078f, 0.396078f}, + {0.4f, 0.4f, 0.4f}, + {0.403922f, 0.403922f, 0.403922f}, + {0.407843f, 0.407843f, 0.407843f}, + {0.411765f, 0.411765f, 0.411765f}, + {0.415686f, 0.415686f, 0.415686f}, + {0.419608f, 0.419608f, 0.419608f}, + {0.423529f, 0.423529f, 0.423529f}, + {0.427451f, 0.427451f, 0.427451f}, + {0.431373f, 0.431373f, 0.431373f}, + {0.435294f, 0.435294f, 0.435294f}, + {0.439216f, 0.439216f, 0.439216f}, + {0.443137f, 0.443137f, 0.443137f}, + {0.447059f, 0.447059f, 0.447059f}, + {0.45098f, 0.45098f, 0.45098f}, + {0.454902f, 0.454902f, 0.454902f}, + {0.458824f, 0.458824f, 0.458824f}, + {0.462745f, 0.462745f, 0.462745f}, + {0.466667f, 0.466667f, 0.466667f}, + {0.470588f, 0.470588f, 0.470588f}, + {0.47451f, 0.47451f, 0.47451f}, + {0.478431f, 0.478431f, 0.478431f}, + {0.482353f, 0.482353f, 0.482353f}, + {0.486275f, 0.486275f, 0.486275f}, + {0.490196f, 0.490196f, 0.490196f}, + {0.494118f, 0.494118f, 0.494118f}, + {0.498039f, 0.498039f, 0.498039f}, + {0.501961f, 0.501961f, 0.501961f}, + {0.505882f, 0.505882f, 0.505882f}, + {0.509804f, 0.509804f, 0.509804f}, + {0.513726f, 0.513726f, 0.513726f}, + {0.517647f, 0.517647f, 0.517647f}, + {0.521569f, 0.521569f, 0.521569f}, + {0.52549f, 0.52549f, 0.52549f}, + {0.529412f, 0.529412f, 0.529412f}, + {0.533333f, 0.533333f, 0.533333f}, + {0.537255f, 0.537255f, 0.537255f}, + {0.541176f, 0.541176f, 0.541176f}, + {0.545098f, 0.545098f, 0.545098f}, + {0.54902f, 0.54902f, 0.54902f}, + {0.552941f, 0.552941f, 0.552941f}, + {0.556863f, 0.556863f, 0.556863f}, + {0.560784f, 0.560784f, 0.560784f}, + {0.564706f, 0.564706f, 0.564706f}, + {0.568627f, 0.568627f, 0.568627f}, + {0.572549f, 0.572549f, 0.572549f}, + {0.576471f, 0.576471f, 0.576471f}, + {0.580392f, 0.580392f, 0.580392f}, + {0.584314f, 0.584314f, 0.584314f}, + {0.588235f, 0.588235f, 0.588235f}, + {0.592157f, 0.592157f, 0.592157f}, + {0.596078f, 0.596078f, 0.596078f}, + {0.6f, 0.6f, 0.6f}, + {0.603922f, 0.603922f, 0.603922f}, + {0.607843f, 0.607843f, 0.607843f}, + {0.611765f, 0.611765f, 0.611765f}, + {0.615686f, 0.615686f, 0.615686f}, + {0.619608f, 0.619608f, 0.619608f}, + {0.623529f, 0.623529f, 0.623529f}, + {0.627451f, 0.627451f, 0.627451f}, + {0.631373f, 0.631373f, 0.631373f}, + {0.635294f, 0.635294f, 0.635294f}, + {0.639216f, 0.639216f, 0.639216f}, + {0.643137f, 0.643137f, 0.643137f}, + {0.647059f, 0.647059f, 0.647059f}, + {0.65098f, 0.65098f, 0.65098f}, + {0.654902f, 0.654902f, 0.654902f}, + {0.658824f, 0.658824f, 0.658824f}, + {0.662745f, 0.662745f, 0.662745f}, + {0.666667f, 0.666667f, 0.666667f}, + {0.670588f, 0.670588f, 0.670588f}, + {0.67451f, 0.67451f, 0.67451f}, + {0.678431f, 0.678431f, 0.678431f}, + {0.682353f, 0.682353f, 0.682353f}, + {0.686275f, 0.686275f, 0.686275f}, + {0.690196f, 0.690196f, 0.690196f}, + {0.694118f, 0.694118f, 0.694118f}, + {0.698039f, 0.698039f, 0.698039f}, + {0.701961f, 0.701961f, 0.701961f}, + {0.705882f, 0.705882f, 0.705882f}, + {0.709804f, 0.709804f, 0.709804f}, + {0.713726f, 0.713726f, 0.713726f}, + {0.717647f, 0.717647f, 0.717647f}, + {0.721569f, 0.721569f, 0.721569f}, + {0.72549f, 0.72549f, 0.72549f}, + {0.729412f, 0.729412f, 0.729412f}, + {0.733333f, 0.733333f, 0.733333f}, + {0.737255f, 0.737255f, 0.737255f}, + {0.741176f, 0.741176f, 0.741176f}, + {0.745098f, 0.745098f, 0.745098f}, + {0.74902f, 0.74902f, 0.74902f}, + {0.752941f, 0.752941f, 0.752941f}, + {0.756863f, 0.756863f, 0.756863f}, + {0.760784f, 0.760784f, 0.760784f}, + {0.764706f, 0.764706f, 0.764706f}, + {0.768627f, 0.768627f, 0.768627f}, + {0.772549f, 0.772549f, 0.772549f}, + {0.776471f, 0.776471f, 0.776471f}, + {0.780392f, 0.780392f, 0.780392f}, + {0.784314f, 0.784314f, 0.784314f}, + {0.788235f, 0.788235f, 0.788235f}, + {0.792157f, 0.792157f, 0.792157f}, + {0.796078f, 0.796078f, 0.796078f}, + {0.8f, 0.8f, 0.8f}, + {0.803922f, 0.803922f, 0.803922f}, + {0.807843f, 0.807843f, 0.807843f}, + {0.811765f, 0.811765f, 0.811765f}, + {0.815686f, 0.815686f, 0.815686f}, + {0.819608f, 0.819608f, 0.819608f}, + {0.823529f, 0.823529f, 0.823529f}, + {0.827451f, 0.827451f, 0.827451f}, + {0.831373f, 0.831373f, 0.831373f}, + {0.835294f, 0.835294f, 0.835294f}, + {0.839216f, 0.839216f, 0.839216f}, + {0.843137f, 0.843137f, 0.843137f}, + {0.847059f, 0.847059f, 0.847059f}, + {0.85098f, 0.85098f, 0.85098f}, + {0.854902f, 0.854902f, 0.854902f}, + {0.858824f, 0.858824f, 0.858824f}, + {0.862745f, 0.862745f, 0.862745f}, + {0.866667f, 0.866667f, 0.866667f}, + {0.870588f, 0.870588f, 0.870588f}, + {0.87451f, 0.87451f, 0.87451f}, + {0.878431f, 0.878431f, 0.878431f}, + {0.882353f, 0.882353f, 0.882353f}, + {0.886275f, 0.886275f, 0.886275f}, + {0.890196f, 0.890196f, 0.890196f}, + {0.894118f, 0.894118f, 0.894118f}, + {0.898039f, 0.898039f, 0.898039f}, + {0.901961f, 0.901961f, 0.901961f}, + {0.905882f, 0.905882f, 0.905882f}, + {0.909804f, 0.909804f, 0.909804f}, + {0.913725f, 0.913725f, 0.913725f}, + {0.917647f, 0.917647f, 0.917647f}, + {0.921569f, 0.921569f, 0.921569f}, + {0.92549f, 0.92549f, 0.92549f}, + {0.929412f, 0.929412f, 0.929412f}, + {0.933333f, 0.933333f, 0.933333f}, + {0.937255f, 0.937255f, 0.937255f}, + {0.941176f, 0.941176f, 0.941176f}, + {0.945098f, 0.945098f, 0.945098f}, + {0.94902f, 0.94902f, 0.94902f}, + {0.952941f, 0.952941f, 0.952941f}, + {0.956863f, 0.956863f, 0.956863f}, + {0.960784f, 0.960784f, 0.960784f}, + {0.964706f, 0.964706f, 0.964706f}, + {0.968627f, 0.968627f, 0.968627f}, + {0.972549f, 0.972549f, 0.972549f}, + {0.976471f, 0.976471f, 0.976471f}, + {0.980392f, 0.980392f, 0.980392f}, + {0.984314f, 0.984314f, 0.984314f}, + {0.988235f, 0.988235f, 0.988235f}, + {0.992157f, 0.992157f, 0.992157f}, + {0.996078f, 0.996078f, 0.996078f}, + {1.0f, 1.0f, 1.0f}}; + +// original colormaps licensed CC0f, public domain, no attribution needed: +// https://github.com/BIDS/colormap/blob/master/LICENSE.txt +const size_t viridis_n = 256; +const float viridis_palette[viridis_n][3] = { + {0.26700401f, 0.00487433f, 0.32941519f}, + {0.26851048f, 0.00960483f, 0.33542652f}, + {0.26994384f, 0.01462494f, 0.34137895f}, + {0.27130489f, 0.01994186f, 0.34726862f}, + {0.27259384f, 0.02556309f, 0.35309303f}, + {0.27380934f, 0.03149748f, 0.35885256f}, + {0.27495242f, 0.03775181f, 0.36454323f}, + {0.27602238f, 0.04416723f, 0.37016418f}, + {0.2770184f, 0.05034437f, 0.37571452f}, + {0.27794143f, 0.05632444f, 0.38119074f}, + {0.27879067f, 0.06214536f, 0.38659204f}, + {0.2795655f, 0.06783587f, 0.39191723f}, + {0.28026658f, 0.07341724f, 0.39716349f}, + {0.28089358f, 0.07890703f, 0.40232944f}, + {0.28144581f, 0.0843197f, 0.40741404f}, + {0.28192358f, 0.08966622f, 0.41241521f}, + {0.28232739f, 0.09495545f, 0.41733086f}, + {0.28265633f, 0.10019576f, 0.42216032f}, + {0.28291049f, 0.10539345f, 0.42690202f}, + {0.28309095f, 0.11055307f, 0.43155375f}, + {0.28319704f, 0.11567966f, 0.43611482f}, + {0.28322882f, 0.12077701f, 0.44058404f}, + {0.28318684f, 0.12584799f, 0.44496f}, + {0.283072f, 0.13089477f, 0.44924127f}, + {0.28288389f, 0.13592005f, 0.45342734f}, + {0.28262297f, 0.14092556f, 0.45751726f}, + {0.28229037f, 0.14591233f, 0.46150995f}, + {0.28188676f, 0.15088147f, 0.46540474f}, + {0.28141228f, 0.15583425f, 0.46920128f}, + {0.28086773f, 0.16077132f, 0.47289909f}, + {0.28025468f, 0.16569272f, 0.47649762f}, + {0.27957399f, 0.17059884f, 0.47999675f}, + {0.27882618f, 0.1754902f, 0.48339654f}, + {0.27801236f, 0.18036684f, 0.48669702f}, + {0.27713437f, 0.18522836f, 0.48989831f}, + {0.27619376f, 0.19007447f, 0.49300074f}, + {0.27519116f, 0.1949054f, 0.49600488f}, + {0.27412802f, 0.19972086f, 0.49891131f}, + {0.27300596f, 0.20452049f, 0.50172076f}, + {0.27182812f, 0.20930306f, 0.50443413f}, + {0.27059473f, 0.21406899f, 0.50705243f}, + {0.26930756f, 0.21881782f, 0.50957678f}, + {0.26796846f, 0.22354911f, 0.5120084f}, + {0.26657984f, 0.2282621f, 0.5143487f}, + {0.2651445f, 0.23295593f, 0.5165993f}, + {0.2636632f, 0.23763078f, 0.51876163f}, + {0.26213801f, 0.24228619f, 0.52083736f}, + {0.26057103f, 0.2469217f, 0.52282822f}, + {0.25896451f, 0.25153685f, 0.52473609f}, + {0.25732244f, 0.2561304f, 0.52656332f}, + {0.25564519f, 0.26070284f, 0.52831152f}, + {0.25393498f, 0.26525384f, 0.52998273f}, + {0.25219404f, 0.26978306f, 0.53157905f}, + {0.25042462f, 0.27429024f, 0.53310261f}, + {0.24862899f, 0.27877509f, 0.53455561f}, + {0.2468114f, 0.28323662f, 0.53594093f}, + {0.24497208f, 0.28767547f, 0.53726018f}, + {0.24311324f, 0.29209154f, 0.53851561f}, + {0.24123708f, 0.29648471f, 0.53970946f}, + {0.23934575f, 0.30085494f, 0.54084398f}, + {0.23744138f, 0.30520222f, 0.5419214f}, + {0.23552606f, 0.30952657f, 0.54294396f}, + {0.23360277f, 0.31382773f, 0.54391424f}, + {0.2316735f, 0.3181058f, 0.54483444f}, + {0.22973926f, 0.32236127f, 0.54570633f}, + {0.22780192f, 0.32659432f, 0.546532f}, + {0.2258633f, 0.33080515f, 0.54731353f}, + {0.22392515f, 0.334994f, 0.54805291f}, + {0.22198915f, 0.33916114f, 0.54875211f}, + {0.22005691f, 0.34330688f, 0.54941304f}, + {0.21812995f, 0.34743154f, 0.55003755f}, + {0.21620971f, 0.35153548f, 0.55062743f}, + {0.21429757f, 0.35561907f, 0.5511844f}, + {0.21239477f, 0.35968273f, 0.55171011f}, + {0.2105031f, 0.36372671f, 0.55220646f}, + {0.20862342f, 0.36775151f, 0.55267486f}, + {0.20675628f, 0.37175775f, 0.55311653f}, + {0.20490257f, 0.37574589f, 0.55353282f}, + {0.20306309f, 0.37971644f, 0.55392505f}, + {0.20123854f, 0.38366989f, 0.55429441f}, + {0.1994295f, 0.38760678f, 0.55464205f}, + {0.1976365f, 0.39152762f, 0.55496905f}, + {0.19585993f, 0.39543297f, 0.55527637f}, + {0.19410009f, 0.39932336f, 0.55556494f}, + {0.19235719f, 0.40319934f, 0.55583559f}, + {0.19063135f, 0.40706148f, 0.55608907f}, + {0.18892259f, 0.41091033f, 0.55632606f}, + {0.18723083f, 0.41474645f, 0.55654717f}, + {0.18555593f, 0.4185704f, 0.55675292f}, + {0.18389763f, 0.42238275f, 0.55694377f}, + {0.18225561f, 0.42618405f, 0.5571201f}, + {0.18062949f, 0.42997486f, 0.55728221f}, + {0.17901879f, 0.43375572f, 0.55743035f}, + {0.17742298f, 0.4375272f, 0.55756466f}, + {0.17584148f, 0.44128981f, 0.55768526f}, + {0.17427363f, 0.4450441f, 0.55779216f}, + {0.17271876f, 0.4487906f, 0.55788532f}, + {0.17117615f, 0.4525298f, 0.55796464f}, + {0.16964573f, 0.45626209f, 0.55803034f}, + {0.16812641f, 0.45998802f, 0.55808199f}, + {0.1666171f, 0.46370813f, 0.55811913f}, + {0.16511703f, 0.4674229f, 0.55814141f}, + {0.16362543f, 0.47113278f, 0.55814842f}, + {0.16214155f, 0.47483821f, 0.55813967f}, + {0.16066467f, 0.47853961f, 0.55811466f}, + {0.15919413f, 0.4822374f, 0.5580728f}, + {0.15772933f, 0.48593197f, 0.55801347f}, + {0.15626973f, 0.4896237f, 0.557936f}, + {0.15481488f, 0.49331293f, 0.55783967f}, + {0.15336445f, 0.49700003f, 0.55772371f}, + {0.1519182f, 0.50068529f, 0.55758733f}, + {0.15047605f, 0.50436904f, 0.55742968f}, + {0.14903918f, 0.50805136f, 0.5572505f}, + {0.14760731f, 0.51173263f, 0.55704861f}, + {0.14618026f, 0.51541316f, 0.55682271f}, + {0.14475863f, 0.51909319f, 0.55657181f}, + {0.14334327f, 0.52277292f, 0.55629491f}, + {0.14193527f, 0.52645254f, 0.55599097f}, + {0.14053599f, 0.53013219f, 0.55565893f}, + {0.13914708f, 0.53381201f, 0.55529773f}, + {0.13777048f, 0.53749213f, 0.55490625f}, + {0.1364085f, 0.54117264f, 0.55448339f}, + {0.13506561f, 0.54485335f, 0.55402906f}, + {0.13374299f, 0.54853458f, 0.55354108f}, + {0.13244401f, 0.55221637f, 0.55301828f}, + {0.13117249f, 0.55589872f, 0.55245948f}, + {0.1299327f, 0.55958162f, 0.55186354f}, + {0.12872938f, 0.56326503f, 0.55122927f}, + {0.12756771f, 0.56694891f, 0.55055551f}, + {0.12645338f, 0.57063316f, 0.5498411f}, + {0.12539383f, 0.57431754f, 0.54908564f}, + {0.12439474f, 0.57800205f, 0.5482874f}, + {0.12346281f, 0.58168661f, 0.54744498f}, + {0.12260562f, 0.58537105f, 0.54655722f}, + {0.12183122f, 0.58905521f, 0.54562298f}, + {0.12114807f, 0.59273889f, 0.54464114f}, + {0.12056501f, 0.59642187f, 0.54361058f}, + {0.12009154f, 0.60010387f, 0.54253043f}, + {0.11973756f, 0.60378459f, 0.54139999f}, + {0.11951163f, 0.60746388f, 0.54021751f}, + {0.11942341f, 0.61114146f, 0.53898192f}, + {0.11948255f, 0.61481702f, 0.53769219f}, + {0.11969858f, 0.61849025f, 0.53634733f}, + {0.12008079f, 0.62216081f, 0.53494633f}, + {0.12063824f, 0.62582833f, 0.53348834f}, + {0.12137972f, 0.62949242f, 0.53197275f}, + {0.12231244f, 0.63315277f, 0.53039808f}, + {0.12344358f, 0.63680899f, 0.52876343f}, + {0.12477953f, 0.64046069f, 0.52706792f}, + {0.12632581f, 0.64410744f, 0.52531069f}, + {0.12808703f, 0.64774881f, 0.52349092f}, + {0.13006688f, 0.65138436f, 0.52160791f}, + {0.13226797f, 0.65501363f, 0.51966086f}, + {0.13469183f, 0.65863619f, 0.5176488f}, + {0.13733921f, 0.66225157f, 0.51557101f}, + {0.14020991f, 0.66585927f, 0.5134268f}, + {0.14330291f, 0.66945881f, 0.51121549f}, + {0.1466164f, 0.67304968f, 0.50893644f}, + {0.15014782f, 0.67663139f, 0.5065889f}, + {0.15389405f, 0.68020343f, 0.50417217f}, + {0.15785146f, 0.68376525f, 0.50168574f}, + {0.16201598f, 0.68731632f, 0.49912906f}, + {0.1663832f, 0.69085611f, 0.49650163f}, + {0.1709484f, 0.69438405f, 0.49380294f}, + {0.17570671f, 0.6978996f, 0.49103252f}, + {0.18065314f, 0.70140222f, 0.48818938f}, + {0.18578266f, 0.70489133f, 0.48527326f}, + {0.19109018f, 0.70836635f, 0.48228395f}, + {0.19657063f, 0.71182668f, 0.47922108f}, + {0.20221902f, 0.71527175f, 0.47608431f}, + {0.20803045f, 0.71870095f, 0.4728733f}, + {0.21400015f, 0.72211371f, 0.46958774f}, + {0.22012381f, 0.72550945f, 0.46622638f}, + {0.2263969f, 0.72888753f, 0.46278934f}, + {0.23281498f, 0.73224735f, 0.45927675f}, + {0.2393739f, 0.73558828f, 0.45568838f}, + {0.24606968f, 0.73890972f, 0.45202405f}, + {0.25289851f, 0.74221104f, 0.44828355f}, + {0.25985676f, 0.74549162f, 0.44446673f}, + {0.26694127f, 0.74875084f, 0.44057284f}, + {0.27414922f, 0.75198807f, 0.4366009f}, + {0.28147681f, 0.75520266f, 0.43255207f}, + {0.28892102f, 0.75839399f, 0.42842626f}, + {0.29647899f, 0.76156142f, 0.42422341f}, + {0.30414796f, 0.76470433f, 0.41994346f}, + {0.31192534f, 0.76782207f, 0.41558638f}, + {0.3198086f, 0.77091403f, 0.41115215f}, + {0.3277958f, 0.77397953f, 0.40664011f}, + {0.33588539f, 0.7770179f, 0.40204917f}, + {0.34407411f, 0.78002855f, 0.39738103f}, + {0.35235985f, 0.78301086f, 0.39263579f}, + {0.36074053f, 0.78596419f, 0.38781353f}, + {0.3692142f, 0.78888793f, 0.38291438f}, + {0.37777892f, 0.79178146f, 0.3779385f}, + {0.38643282f, 0.79464415f, 0.37288606f}, + {0.39517408f, 0.79747541f, 0.36775726f}, + {0.40400101f, 0.80027461f, 0.36255223f}, + {0.4129135f, 0.80304099f, 0.35726893f}, + {0.42190813f, 0.80577412f, 0.35191009f}, + {0.43098317f, 0.80847343f, 0.34647607f}, + {0.44013691f, 0.81113836f, 0.3409673f}, + {0.44936763f, 0.81376835f, 0.33538426f}, + {0.45867362f, 0.81636288f, 0.32972749f}, + {0.46805314f, 0.81892143f, 0.32399761f}, + {0.47750446f, 0.82144351f, 0.31819529f}, + {0.4870258f, 0.82392862f, 0.31232133f}, + {0.49661536f, 0.82637633f, 0.30637661f}, + {0.5062713f, 0.82878621f, 0.30036211f}, + {0.51599182f, 0.83115784f, 0.29427888f}, + {0.52577622f, 0.83349064f, 0.2881265f}, + {0.5356211f, 0.83578452f, 0.28190832f}, + {0.5455244f, 0.83803918f, 0.27562602f}, + {0.55548397f, 0.84025437f, 0.26928147f}, + {0.5654976f, 0.8424299f, 0.26287683f}, + {0.57556297f, 0.84456561f, 0.25641457f}, + {0.58567772f, 0.84666139f, 0.24989748f}, + {0.59583934f, 0.84871722f, 0.24332878f}, + {0.60604528f, 0.8507331f, 0.23671214f}, + {0.61629283f, 0.85270912f, 0.23005179f}, + {0.62657923f, 0.85464543f, 0.22335258f}, + {0.63690157f, 0.85654226f, 0.21662012f}, + {0.64725685f, 0.85839991f, 0.20986086f}, + {0.65764197f, 0.86021878f, 0.20308229f}, + {0.66805369f, 0.86199932f, 0.19629307f}, + {0.67848868f, 0.86374211f, 0.18950326f}, + {0.68894351f, 0.86544779f, 0.18272455f}, + {0.69941463f, 0.86711711f, 0.17597055f}, + {0.70989842f, 0.86875092f, 0.16925712f}, + {0.72039115f, 0.87035015f, 0.16260273f}, + {0.73088902f, 0.87191584f, 0.15602894f}, + {0.74138803f, 0.87344918f, 0.14956101f}, + {0.75188414f, 0.87495143f, 0.14322828f}, + {0.76237342f, 0.87642392f, 0.13706449f}, + {0.77285183f, 0.87786808f, 0.13110864f}, + {0.78331535f, 0.87928545f, 0.12540538f}, + {0.79375994f, 0.88067763f, 0.12000532f}, + {0.80418159f, 0.88204632f, 0.11496505f}, + {0.81457634f, 0.88339329f, 0.11034678f}, + {0.82494028f, 0.88472036f, 0.10621724f}, + {0.83526959f, 0.88602943f, 0.1026459f}, + {0.84556056f, 0.88732243f, 0.09970219f}, + {0.8558096f, 0.88860134f, 0.09745186f}, + {0.86601325f, 0.88986815f, 0.09595277f}, + {0.87616824f, 0.89112487f, 0.09525046f}, + {0.88627146f, 0.89237353f, 0.09537439f}, + {0.89632002f, 0.89361614f, 0.09633538f}, + {0.90631121f, 0.89485467f, 0.09812496f}, + {0.91624212f, 0.89609127f, 0.1007168f}, + {0.92610579f, 0.89732977f, 0.10407067f}, + {0.93590444f, 0.8985704f, 0.10813094f}, + {0.94563626f, 0.899815f, 0.11283773f}, + {0.95529972f, 0.90106534f, 0.11812832f}, + {0.96489353f, 0.90232311f, 0.12394051f}, + {0.97441665f, 0.90358991f, 0.13021494f}, + {0.98386829f, 0.90486726f, 0.13689671f}, + {0.99324789f, 0.90615657f, 0.1439362f}}; + +const size_t magma_n = 256; +const float magma_palette[magma_n][3] = { + {0.001462f, 0.000466f, 0.013866f}, + {0.002258f, 0.001295f, 0.018331f}, + {0.003279f, 0.002305f, 0.023708f}, + {0.004512f, 0.00349f, 0.029965f}, + {0.00595f, 0.004843f, 0.03713f}, + {0.007588f, 0.006356f, 0.044973f}, + {0.009426f, 0.008022f, 0.052844f}, + {0.011465f, 0.009828f, 0.06075f}, + {0.013708f, 0.011771f, 0.068667f}, + {0.016156f, 0.01384f, 0.076603f}, + {0.018815f, 0.016026f, 0.084584f}, + {0.021692f, 0.01832f, 0.09261f}, + {0.024792f, 0.020715f, 0.100676f}, + {0.028123f, 0.023201f, 0.108787f}, + {0.031696f, 0.025765f, 0.116965f}, + {0.03552f, 0.028397f, 0.125209f}, + {0.039608f, 0.03109f, 0.133515f}, + {0.04383f, 0.03383f, 0.141886f}, + {0.048062f, 0.036607f, 0.150327f}, + {0.05232f, 0.039407f, 0.158841f}, + {0.056615f, 0.04216f, 0.167446f}, + {0.060949f, 0.044794f, 0.176129f}, + {0.06533f, 0.047318f, 0.184892f}, + {0.069764f, 0.049726f, 0.193735f}, + {0.074257f, 0.052017f, 0.20266f}, + {0.078815f, 0.054184f, 0.211667f}, + {0.083446f, 0.056225f, 0.220755f}, + {0.088155f, 0.058133f, 0.229922f}, + {0.092949f, 0.059904f, 0.239164f}, + {0.097833f, 0.061531f, 0.248477f}, + {0.102815f, 0.06301f, 0.257854f}, + {0.107899f, 0.064335f, 0.267289f}, + {0.113094f, 0.065492f, 0.276784f}, + {0.118405f, 0.066479f, 0.286321f}, + {0.123833f, 0.067295f, 0.295879f}, + {0.12938f, 0.067935f, 0.305443f}, + {0.135053f, 0.068391f, 0.315f}, + {0.140858f, 0.068654f, 0.324538f}, + {0.146785f, 0.068738f, 0.334011f}, + {0.152839f, 0.068637f, 0.343404f}, + {0.159018f, 0.068354f, 0.352688f}, + {0.165308f, 0.067911f, 0.361816f}, + {0.171713f, 0.067305f, 0.370771f}, + {0.178212f, 0.066576f, 0.379497f}, + {0.184801f, 0.065732f, 0.387973f}, + {0.19146f, 0.064818f, 0.396152f}, + {0.198177f, 0.063862f, 0.404009f}, + {0.204935f, 0.062907f, 0.411514f}, + {0.211718f, 0.061992f, 0.418647f}, + {0.218512f, 0.061158f, 0.425392f}, + {0.225302f, 0.060445f, 0.431742f}, + {0.232077f, 0.059889f, 0.437695f}, + {0.238826f, 0.059517f, 0.443256f}, + {0.245543f, 0.059352f, 0.448436f}, + {0.25222f, 0.059415f, 0.453248f}, + {0.258857f, 0.059706f, 0.45771f}, + {0.265447f, 0.060237f, 0.46184f}, + {0.271994f, 0.060994f, 0.46566f}, + {0.278493f, 0.061978f, 0.46919f}, + {0.284951f, 0.063168f, 0.472451f}, + {0.291366f, 0.064553f, 0.475462f}, + {0.29774f, 0.066117f, 0.478243f}, + {0.304081f, 0.067835f, 0.480812f}, + {0.310382f, 0.069702f, 0.483186f}, + {0.316654f, 0.07169f, 0.48538f}, + {0.322899f, 0.073782f, 0.487408f}, + {0.329114f, 0.075972f, 0.489287f}, + {0.335308f, 0.078236f, 0.491024f}, + {0.341482f, 0.080564f, 0.492631f}, + {0.347636f, 0.082946f, 0.494121f}, + {0.353773f, 0.085373f, 0.495501f}, + {0.359898f, 0.087831f, 0.496778f}, + {0.366012f, 0.090314f, 0.49796f}, + {0.372116f, 0.092816f, 0.499053f}, + {0.378211f, 0.095332f, 0.500067f}, + {0.384299f, 0.097855f, 0.501002f}, + {0.390384f, 0.100379f, 0.501864f}, + {0.396467f, 0.102902f, 0.502658f}, + {0.402548f, 0.10542f, 0.503386f}, + {0.408629f, 0.10793f, 0.504052f}, + {0.414709f, 0.110431f, 0.504662f}, + {0.420791f, 0.11292f, 0.505215f}, + {0.426877f, 0.115395f, 0.505714f}, + {0.432967f, 0.117855f, 0.50616f}, + {0.439062f, 0.120298f, 0.506555f}, + {0.445163f, 0.122724f, 0.506901f}, + {0.451271f, 0.125132f, 0.507198f}, + {0.457386f, 0.127522f, 0.507448f}, + {0.463508f, 0.129893f, 0.507652f}, + {0.46964f, 0.132245f, 0.507809f}, + {0.47578f, 0.134577f, 0.507921f}, + {0.481929f, 0.136891f, 0.507989f}, + {0.488088f, 0.139186f, 0.508011f}, + {0.494258f, 0.141462f, 0.507988f}, + {0.500438f, 0.143719f, 0.50792f}, + {0.506629f, 0.145958f, 0.507806f}, + {0.512831f, 0.148179f, 0.507648f}, + {0.519045f, 0.150383f, 0.507443f}, + {0.52527f, 0.152569f, 0.507192f}, + {0.531507f, 0.154739f, 0.506895f}, + {0.537755f, 0.156894f, 0.506551f}, + {0.544015f, 0.159033f, 0.506159f}, + {0.550287f, 0.161158f, 0.505719f}, + {0.556571f, 0.163269f, 0.50523f}, + {0.562866f, 0.165368f, 0.504692f}, + {0.569172f, 0.167454f, 0.504105f}, + {0.57549f, 0.16953f, 0.503466f}, + {0.581819f, 0.171596f, 0.502777f}, + {0.588158f, 0.173652f, 0.502035f}, + {0.594508f, 0.175701f, 0.501241f}, + {0.600868f, 0.177743f, 0.500394f}, + {0.607238f, 0.179779f, 0.499492f}, + {0.613617f, 0.181811f, 0.498536f}, + {0.620005f, 0.18384f, 0.497524f}, + {0.626401f, 0.185867f, 0.496456f}, + {0.632805f, 0.187893f, 0.495332f}, + {0.639216f, 0.189921f, 0.49415f}, + {0.645633f, 0.191952f, 0.49291f}, + {0.652056f, 0.193986f, 0.491611f}, + {0.658483f, 0.196027f, 0.490253f}, + {0.664915f, 0.198075f, 0.488836f}, + {0.671349f, 0.200133f, 0.487358f}, + {0.677786f, 0.202203f, 0.485819f}, + {0.684224f, 0.204286f, 0.484219f}, + {0.690661f, 0.206384f, 0.482558f}, + {0.697098f, 0.208501f, 0.480835f}, + {0.703532f, 0.210638f, 0.479049f}, + {0.709962f, 0.212797f, 0.477201f}, + {0.716387f, 0.214982f, 0.47529f}, + {0.722805f, 0.217194f, 0.473316f}, + {0.729216f, 0.219437f, 0.471279f}, + {0.735616f, 0.221713f, 0.46918f}, + {0.742004f, 0.224025f, 0.467018f}, + {0.748378f, 0.226377f, 0.464794f}, + {0.754737f, 0.228772f, 0.462509f}, + {0.761077f, 0.231214f, 0.460162f}, + {0.767398f, 0.233705f, 0.457755f}, + {0.773695f, 0.236249f, 0.455289f}, + {0.779968f, 0.238851f, 0.452765f}, + {0.786212f, 0.241514f, 0.450184f}, + {0.792427f, 0.244242f, 0.447543f}, + {0.798608f, 0.24704f, 0.444848f}, + {0.804752f, 0.249911f, 0.442102f}, + {0.810855f, 0.252861f, 0.439305f}, + {0.816914f, 0.255895f, 0.436461f}, + {0.822926f, 0.259016f, 0.433573f}, + {0.828886f, 0.262229f, 0.430644f}, + {0.834791f, 0.26554f, 0.427671f}, + {0.840636f, 0.268953f, 0.424666f}, + {0.846416f, 0.272473f, 0.421631f}, + {0.852126f, 0.276106f, 0.418573f}, + {0.857763f, 0.279857f, 0.415496f}, + {0.86332f, 0.283729f, 0.412403f}, + {0.868793f, 0.287728f, 0.409303f}, + {0.874176f, 0.291859f, 0.406205f}, + {0.879464f, 0.296125f, 0.403118f}, + {0.884651f, 0.30053f, 0.400047f}, + {0.889731f, 0.305079f, 0.397002f}, + {0.8947f, 0.309773f, 0.393995f}, + {0.899552f, 0.314616f, 0.391037f}, + {0.904281f, 0.31961f, 0.388137f}, + {0.908884f, 0.324755f, 0.385308f}, + {0.913354f, 0.330052f, 0.382563f}, + {0.917689f, 0.3355f, 0.379915f}, + {0.921884f, 0.341098f, 0.377376f}, + {0.925937f, 0.346844f, 0.374959f}, + {0.929845f, 0.352734f, 0.372677f}, + {0.933606f, 0.358764f, 0.370541f}, + {0.937221f, 0.364929f, 0.368567f}, + {0.940687f, 0.371224f, 0.366762f}, + {0.944006f, 0.377643f, 0.365136f}, + {0.94718f, 0.384178f, 0.363701f}, + {0.95021f, 0.39082f, 0.362468f}, + {0.953099f, 0.397563f, 0.361438f}, + {0.955849f, 0.4044f, 0.360619f}, + {0.958464f, 0.411324f, 0.360014f}, + {0.960949f, 0.418323f, 0.35963f}, + {0.96331f, 0.42539f, 0.359469f}, + {0.965549f, 0.432519f, 0.359529f}, + {0.967671f, 0.439703f, 0.35981f}, + {0.96968f, 0.446936f, 0.360311f}, + {0.971582f, 0.45421f, 0.36103f}, + {0.973381f, 0.46152f, 0.361965f}, + {0.975082f, 0.468861f, 0.363111f}, + {0.97669f, 0.476226f, 0.364466f}, + {0.97821f, 0.483612f, 0.366025f}, + {0.979645f, 0.491014f, 0.367783f}, + {0.981f, 0.498428f, 0.369734f}, + {0.982279f, 0.505851f, 0.371874f}, + {0.983485f, 0.51328f, 0.374198f}, + {0.984622f, 0.520713f, 0.376698f}, + {0.985693f, 0.528148f, 0.379371f}, + {0.9867f, 0.535582f, 0.38221f}, + {0.987646f, 0.543015f, 0.38521f}, + {0.988533f, 0.550446f, 0.388365f}, + {0.989363f, 0.557873f, 0.391671f}, + {0.990138f, 0.565296f, 0.395122f}, + {0.990871f, 0.572706f, 0.398714f}, + {0.991558f, 0.580107f, 0.402441f}, + {0.992196f, 0.587502f, 0.406299f}, + {0.992785f, 0.594891f, 0.410283f}, + {0.993326f, 0.602275f, 0.41439f}, + {0.993834f, 0.609644f, 0.418613f}, + {0.994309f, 0.616999f, 0.42295f}, + {0.994738f, 0.62435f, 0.427397f}, + {0.995122f, 0.631696f, 0.431951f}, + {0.99548f, 0.639027f, 0.436607f}, + {0.99581f, 0.646344f, 0.441361f}, + {0.996096f, 0.653659f, 0.446213f}, + {0.996341f, 0.660969f, 0.45116f}, + {0.99658f, 0.668256f, 0.456192f}, + {0.996775f, 0.675541f, 0.461314f}, + {0.996925f, 0.682828f, 0.466526f}, + {0.997077f, 0.690088f, 0.471811f}, + {0.997186f, 0.697349f, 0.477182f}, + {0.997254f, 0.704611f, 0.482635f}, + {0.997325f, 0.711848f, 0.488154f}, + {0.997351f, 0.719089f, 0.493755f}, + {0.997351f, 0.726324f, 0.499428f}, + {0.997341f, 0.733545f, 0.505167f}, + {0.997285f, 0.740772f, 0.510983f}, + {0.997228f, 0.747981f, 0.516859f}, + {0.997138f, 0.75519f, 0.522806f}, + {0.997019f, 0.762398f, 0.528821f}, + {0.996898f, 0.769591f, 0.534892f}, + {0.996727f, 0.776795f, 0.541039f}, + {0.996571f, 0.783977f, 0.547233f}, + {0.996369f, 0.791167f, 0.553499f}, + {0.996162f, 0.798348f, 0.55982f}, + {0.995932f, 0.805527f, 0.566202f}, + {0.99568f, 0.812706f, 0.572645f}, + {0.995424f, 0.819875f, 0.57914f}, + {0.995131f, 0.827052f, 0.585701f}, + {0.994851f, 0.834213f, 0.592307f}, + {0.994524f, 0.841387f, 0.598983f}, + {0.994222f, 0.84854f, 0.605696f}, + {0.993866f, 0.855711f, 0.612482f}, + {0.993545f, 0.862859f, 0.619299f}, + {0.99317f, 0.870024f, 0.626189f}, + {0.992831f, 0.877168f, 0.633109f}, + {0.99244f, 0.88433f, 0.640099f}, + {0.992089f, 0.89147f, 0.647116f}, + {0.991688f, 0.898627f, 0.654202f}, + {0.991332f, 0.905763f, 0.661309f}, + {0.99093f, 0.912915f, 0.668481f}, + {0.99057f, 0.920049f, 0.675675f}, + {0.990175f, 0.927196f, 0.682926f}, + {0.989815f, 0.934329f, 0.690198f}, + {0.989434f, 0.94147f, 0.697519f}, + {0.989077f, 0.948604f, 0.704863f}, + {0.988717f, 0.955742f, 0.712242f}, + {0.988367f, 0.962878f, 0.719649f}, + {0.988033f, 0.970012f, 0.727077f}, + {0.987691f, 0.977154f, 0.734536f}, + {0.987387f, 0.984288f, 0.742002f}, + {0.987053f, 0.991438f, 0.749504f}}; + } // namespace viz } // namespace ouster diff --git a/ouster_viz/src/common.h b/ouster_viz/src/common.h index a3d95e99..6bf0d7e3 100644 --- a/ouster_viz/src/common.h +++ b/ouster_viz/src/common.h @@ -96,18 +96,19 @@ inline GLuint load_shaders(const std::string& vertex_shader_code, * such as float[n][3] * * @param texture array of at least size width * height * elements_per_texel - * where elements per texel is 3 for GL_RGB and 1 for GL_RED + * where elements per texel is 3 for GL_RGB format and 1 for + * GL_RED format * @param width width of texture in texels * @param height height of texture in texels * @param texture_id handle generated by glGenTextures * @param internal_format internal format, e.g. GL_RGB or GL_RGB32F * @param format format, e.g. GL_RGB or GL_RED + * @param type texture element type */ template -void load_texture(const F& texture, const size_t width, const size_t height, - const GLuint texture_id, - const GLenum internal_format = GL_RGB, - const GLenum format = GL_RGB) { +void load_texture(const F& texture, size_t width, size_t height, + GLuint texture_id, GLenum internal_format = GL_RGB, + GLenum format = GL_RGB, GLenum type = GL_FLOAT) { glBindTexture(GL_TEXTURE_2D, texture_id); // we have only 1 level, so we override base/max levels @@ -122,7 +123,7 @@ void load_texture(const F& texture, const size_t width, const size_t height, glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); glTexImage2D(GL_TEXTURE_2D, 0, internal_format, width, height, 0, format, - GL_FLOAT, texture); + type, texture); } /** @@ -157,17 +158,18 @@ static const std::string point_vertex_shader_code = in vec3 xyz; in vec3 offset; in float range; - in float key; - in vec4 mask; in float trans_index; uniform sampler2D transformation; uniform mat4 model; uniform mat4 proj_view; - out float vcolor; - out vec4 overlay_rgba; - void main(){ + in vec4 vkey; + in vec4 vmask; + + out vec4 key; + out vec4 mask; + void main() { vec4 local_point = range > 0 ? model * vec4(xyz * range + offset, 1.0) : vec4(0, 0, 0, 1.0); @@ -189,19 +191,26 @@ static const std::string point_vertex_shader_code = ); gl_Position = proj_view * car_pose * local_point; - vcolor = sqrt(key); - overlay_rgba = mask; + key = vkey; + mask = vmask; })SHADER"; static const std::string point_fragment_shader_code = R"SHADER( #version 330 core - in float vcolor; - in vec4 overlay_rgba; + in vec4 key; + in vec4 mask; + uniform bool mono; uniform sampler2D palette; out vec4 color; void main() { - color = vec4(texture(palette, vec2(vcolor, 1)).xyz * (1.0 - overlay_rgba.w) - + overlay_rgba.xyz * overlay_rgba.w, 1); + // getting color from palette or as it set in the rgb data + // the full resolved color will be in vec4(c, key.a) + vec3 c = mono ? texture(palette, vec2(key.r, 1)).rgb : key.rgb; + // compositing the mask RGBA value on top of the resolved point color c + // using "over" operator https://en.wikipedia.org/wiki/Alpha_compositing + float color_a = mask.a + key.a * (1 - mask.a); + vec3 color_rgb = mask.rgb * mask.a + c * key.a * (1 - mask.a); + color = vec4(color_rgb / color_a, color_a); })SHADER"; static const std::string ring_vertex_shader_code = R"SHADER( @@ -253,14 +262,19 @@ static const std::string image_fragment_shader_code = R"SHADER( #version 330 core in vec2 uv; + uniform bool mono; + uniform bool use_palette; uniform sampler2D image; uniform sampler2D mask; + uniform sampler2D palette; out vec4 color; void main() { vec4 m = texture(mask, uv); - float a = m.a; - float r = sqrt(texture(image, uv).r) * (1.0 - a); - color = vec4(vec3(r, r, r) + m.rgb * a, 1.0); + vec4 itex = texture(image, uv); + vec3 key_color = use_palette ? texture(palette, vec2(itex.r, 1)).rgb : vec3(itex.r); + vec3 img_color = mono ? key_color : itex.rgb; + float color_a = m.a + itex.a * (1 - m.a); + color = vec4((m.rgb * m.a + img_color * (1.0 - m.a)) / color_a, color_a); })SHADER"; } // namespace impl diff --git a/ouster_viz/src/glfw.cpp b/ouster_viz/src/glfw.cpp index e78652c0..346ba288 100644 --- a/ouster_viz/src/glfw.cpp +++ b/ouster_viz/src/glfw.cpp @@ -37,9 +37,9 @@ void handle_key_press(GLFWwindow* window, int key, int /*scancode*/, int action, } /* - * Callback for resizing the window + * Callback for resizing viewport (i.e. framebuffer) */ -void handle_window_resize(GLFWwindow* window, int fb_width, int fb_height) { +void handle_framebuffer_resize(GLFWwindow* window, int fb_width, int fb_height) { auto ctx = static_cast(glfwGetWindowUserPointer(window)); ctx->window_context.viewport_width = fb_width; ctx->window_context.viewport_height = fb_height; @@ -48,6 +48,16 @@ void handle_window_resize(GLFWwindow* window, int fb_width, int fb_height) { if (ctx->resize_handler) ctx->resize_handler(); } +/* + * Callback for resizing the window + */ +void handle_window_resize(GLFWwindow* window, int window_width, + int window_height) { + auto ctx = static_cast(glfwGetWindowUserPointer(window)); + ctx->window_context.window_width = window_width; + ctx->window_context.window_height = window_height; +} + /* * Callback for mouse press. Polled after each drawing. * @@ -181,7 +191,8 @@ GLFWContext::GLFWContext(const std::string& name, bool fix_aspect, } // set up callbacks (run by glfwPollEvents) - glfwSetFramebufferSizeCallback(window, handle_window_resize); + glfwSetFramebufferSizeCallback(window, handle_framebuffer_resize); + glfwSetWindowSizeCallback(window, handle_window_resize); glfwSetKeyCallback(window, handle_key_press); glfwSetMouseButtonCallback(window, handle_mouse_button); glfwSetCursorPosCallback(window, handle_cursor_pos); @@ -204,9 +215,17 @@ GLFWContext::GLFWContext(const std::string& name, bool fix_aspect, int viewport_width, viewport_height; glfwGetFramebufferSize(window, &viewport_width, &viewport_height); + // set viewport for glText (in pixels) gltViewport(viewport_width, viewport_height); + + int win_width, win_height; + glfwGetWindowSize(window, &win_width, &win_height); + + // store window and viewport (i.e. framebuffer) sizes to the context window_context.viewport_width = viewport_width; window_context.viewport_height = viewport_height; + window_context.window_width = win_width; + window_context.window_height = win_height; // release context in case subsequent calls are done from another thread glfwMakeContextCurrent(nullptr); diff --git a/ouster_viz/src/image.cpp b/ouster_viz/src/image.cpp index d6f566cc..49518319 100644 --- a/ouster_viz/src/image.cpp +++ b/ouster_viz/src/image.cpp @@ -21,8 +21,11 @@ bool GLImage::initialized = false; GLuint GLImage::program_id; GLuint GLImage::vertex_id; GLuint GLImage::uv_id; +GLuint GLImage::mono_id; GLuint GLImage::image_id; GLuint GLImage::mask_id; +GLuint GLImage::palette_id; +GLuint GLImage::use_palette_id; GLImage::GLImage() { if (!GLImage::initialized) @@ -36,15 +39,17 @@ GLImage::GLImage() { glBufferData(GL_ELEMENT_ARRAY_BUFFER, 6 * sizeof(GLubyte), indices, GL_STATIC_DRAW); - GLuint textures[2]; - glGenTextures(2, textures); + GLuint textures[3]; + glGenTextures(3, textures); image_texture_id = textures[0]; mask_texture_id = textures[1]; + palette_texture_id = textures[2]; // initialize textures GLfloat init[4] = {0, 0, 0, 0}; load_texture(init, 1, 1, image_texture_id, GL_RED, GL_RED); load_texture(init, 1, 1, mask_texture_id, GL_RGBA, GL_RGBA); + load_texture(init, 1, 1, palette_texture_id, GL_RGBA, GL_RGBA); } GLImage::GLImage(const Image& /*image*/) : GLImage{} {} @@ -53,6 +58,7 @@ GLImage::~GLImage() { glDeleteBuffers(2, vertexbuffers.data()); glDeleteTextures(1, &image_texture_id); glDeleteTextures(1, &mask_texture_id); + glDeleteTextures(1, &palette_texture_id); } void GLImage::draw(const WindowCtx& ctx, const CameraData&, Image& image) { @@ -68,15 +74,21 @@ void GLImage::draw(const WindowCtx& ctx, const CameraData&, Image& image) { glUniform1i(image_id, 0); glUniform1i(mask_id, 1); + glUniform1i(palette_id, 2); glActiveTexture(GL_TEXTURE0); if (image.image_changed_) { load_texture(image.image_data_.data(), image.image_width_, - image.image_height_, image_texture_id, GL_RED, GL_RED); + image.image_height_, image_texture_id, GL_RGBA, GL_RGBA, + GL_FLOAT); image.image_changed_ = false; } glBindTexture(GL_TEXTURE_2D, image_texture_id); + // put the shader into mono or rgb mode + glUniform1i(mono_id, image.mono_ ? 1 : 0); + glUniform1i(use_palette_id, image.use_palette_ ? 1 : 0); + glActiveTexture(GL_TEXTURE1); if (image.mask_changed_) { load_texture(image.mask_data_.data(), image.mask_width_, @@ -85,6 +97,16 @@ void GLImage::draw(const WindowCtx& ctx, const CameraData&, Image& image) { } glBindTexture(GL_TEXTURE_2D, mask_texture_id); + glActiveTexture(GL_TEXTURE2); + if (image.palette_changed_) { + if (!image.palette_data_.empty()) { + load_texture(image.palette_data_.data(), + image.palette_data_.size() / 3, 1, palette_texture_id); + } + image.palette_changed_ = false; + } + glBindTexture(GL_TEXTURE_2D, palette_texture_id); + // draw double aspect = impl::window_aspect(ctx); GLfloat x0_scaled = x0 / aspect + hshift; @@ -130,14 +152,21 @@ void GLImage::initialize() { // TODO: handled differently than cloud ids... GLImage::vertex_id = glGetAttribLocation(GLImage::program_id, "vertex"); GLImage::uv_id = glGetAttribLocation(GLImage::program_id, "vertex_uv"); + GLImage::mono_id = glGetUniformLocation(GLImage::program_id, "mono"); GLImage::image_id = glGetUniformLocation(GLImage::program_id, "image"); GLImage::mask_id = glGetUniformLocation(GLImage::program_id, "mask"); + GLImage::palette_id = glGetUniformLocation(GLImage::program_id, "palette"); + GLImage::use_palette_id = glGetUniformLocation(GLImage::program_id, "use_palette"); GLImage::initialized = true; } void GLImage::uninitialize() { glDeleteProgram(GLImage::program_id); } -void GLImage::beginDraw() { glUseProgram(GLImage::program_id); } +void GLImage::beginDraw() { + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ZERO); + glUseProgram(GLImage::program_id); +} void GLImage::endDraw() {} diff --git a/ouster_viz/src/image.h b/ouster_viz/src/image.h index da47950f..677aed90 100644 --- a/ouster_viz/src/image.h +++ b/ouster_viz/src/image.h @@ -29,13 +29,17 @@ class GLImage { static GLuint program_id; static GLuint vertex_id; static GLuint uv_id; + static GLuint mono_id; static GLuint image_id; static GLuint mask_id; + static GLuint palette_id; + static GLuint use_palette_id; // per-image gl state std::array vertexbuffers; GLuint image_texture_id{0}; GLuint mask_texture_id{0}; + GLuint palette_texture_id{0}; GLuint image_index_id{0}; float x0{-1}, x1{0}, y0{0}, y1{-1}, hshift{0}; diff --git a/ouster_viz/src/misc.cpp b/ouster_viz/src/misc.cpp index 42d5ac81..9641b77d 100644 --- a/ouster_viz/src/misc.cpp +++ b/ouster_viz/src/misc.cpp @@ -298,6 +298,7 @@ void GLLabel::draw(const WindowCtx& ctx, const CameraData& camera, float scale2d = scale; #ifdef __APPLE__ // TODO: maybe try turning GLFW_COCOA_RETINA_FRAMEBUFFER off + // TODO[pb]: Also we can start using GLFW window_content_scale for this scale2d *= 2.0; #endif gltDrawText2DAligned(gltext, x, y, scale2d, halign, valign); diff --git a/ouster_viz/src/point_viz.cpp b/ouster_viz/src/point_viz.cpp index a5f8c3b6..7487a96e 100644 --- a/ouster_viz/src/point_viz.cpp +++ b/ouster_viz/src/point_viz.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "camera.h" #include "cloud.h" @@ -227,12 +228,12 @@ void PointViz::visible(bool state) { pimpl->glfw->visible(state); } bool PointViz::update() { std::lock_guard guard{pimpl->update_mx}; - // propagate camera changes - pimpl->camera_front = pimpl->camera_back; - // last frame hasn't been drawn yet if (pimpl->front_changed) return false; + // propagate camera changes + pimpl->camera_front = pimpl->camera_back; + pimpl->clouds.swap(); pimpl->cuboids.swap(); pimpl->labels.swap(); @@ -244,14 +245,22 @@ bool PointViz::update() { return true; } -int PointViz::viewport_width() { +int PointViz::viewport_width() const { return pimpl->glfw->window_context.viewport_width; } -int PointViz::viewport_height() { +int PointViz::viewport_height() const { return pimpl->glfw->window_context.viewport_height; } +int PointViz::window_width() const { + return pimpl->glfw->window_context.window_width; +} + +int PointViz::window_height() const { + return pimpl->glfw->window_context.window_height; +} + void PointViz::draw() { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glBindVertexArray(pimpl->vao); @@ -402,12 +411,13 @@ Cloud::Cloud(size_t w, size_t h, const mat4d& extrinsic) w_{w}, extrinsic_{extrinsic}, range_data_(n_, 0), - key_data_(n_, 0), + key_data_(4 * n_, 0), mask_data_(4 * n_, 0), xyz_data_(3 * n_, 0), off_data_(3 * n_, 0), transform_data_(12 * w, 0), - palette_data_(3 * n_, 0) { + palette_data_(&spezia_palette[0][0], + &spezia_palette[0][0] + spezia_n * 3) { // set everything to changed so on GLCloud object reuse we properly draw // everything first time range_changed_ = true; @@ -416,6 +426,7 @@ Cloud::Cloud(size_t w, size_t h, const mat4d& extrinsic) xyz_changed_ = true; offset_changed_ = true; point_size_changed_ = true; + palette_changed_ = true; // initialize per-column poses to identity for (size_t v = 0; v < w; v++) { @@ -425,10 +436,13 @@ Cloud::Cloud(size_t w, size_t h, const mat4d& extrinsic) } transform_changed_ = true; + // set initial rgba alpha to 1.0 + for (size_t i = 3; i < 4 * n_; i += 4) { + key_data_[i] = 1.0; + } + Eigen::Map{pose_.data()}.setIdentity(); pose_changed_ = true; - - set_palette(&spezia_palette[0][0], spezia_n); } /* @@ -480,8 +494,53 @@ void Cloud::set_range(const uint32_t* x) { } void Cloud::set_key(const float* key_data) { - std::copy(key_data, key_data + n_, key_data_.begin()); + const float* end = key_data + n_; + float* dst = key_data_.data(); + while (key_data != end) { + *dst = *(key_data++); + dst += 4; + } + key_changed_ = true; + mono_ = true; +} + +void Cloud::set_key_alpha(const float* key_alpha_data) { + const float* end = key_alpha_data + n_; + float* dst = key_data_.data(); + while (key_alpha_data != end) { + *(dst + 3) = *(key_alpha_data++); // 4th component is alpha + dst += 4; + } + key_changed_ = true; + mono_ = true; +} + +void Cloud::set_key_rgb(const float* key_rgb_data) { + // 3 color channels per point (RGB) + const float* end = key_rgb_data + 3 * n_; + float* dst = key_data_.data(); + while (key_rgb_data != end) { + *(dst++) = *(key_rgb_data++); + *(dst++) = *(key_rgb_data++); + *(dst++) = *(key_rgb_data++); + dst++; // alpha left untouched + } key_changed_ = true; + mono_ = false; +} + +void Cloud::set_key_rgba(const float* key_rgba_data) { + // 4 color channels per point (RGBA) + const float* end = key_rgba_data + 4 * n_; + float* dst = key_data_.data(); + while (key_rgba_data != end) { + *(dst++) = *(key_rgba_data++); + *(dst++) = *(key_rgba_data++); + *(dst++) = *(key_rgba_data++); + *(dst++) = *(key_rgba_data++); + } + key_changed_ = true; + mono_ = false; } void Cloud::set_mask(const float* mask_data) { @@ -544,15 +603,57 @@ void Image::clear() { position_changed_ = false; image_changed_ = false; mask_changed_ = false; + palette_changed_ = false; } void Image::set_image(size_t width, size_t height, const float* image_data) { const size_t n = width * height; - image_data_.resize(n); + image_data_.resize(4 * n); image_width_ = width; image_height_ = height; - std::copy(image_data, image_data + n, image_data_.begin()); + const float* end = image_data + n; + float* dst = image_data_.data(); + while (image_data != end) { + *(dst + 0) = *(image_data++); + *(dst + 3) = 1.0; // alpha + dst += 4; + } + image_changed_ = true; + mono_ = true; +} + +void Image::set_image_rgb(size_t width, size_t height, const float* image_data_rgb) { + const size_t n = width * height; + image_data_.resize(4 * n); + image_width_ = width; + image_height_ = height; + const float* end = image_data_rgb + 3 * n; + float* dst = image_data_.data(); + while (image_data_rgb != end) { + *(dst++) = *(image_data_rgb++); + *(dst++) = *(image_data_rgb++); + *(dst++) = *(image_data_rgb++); + *(dst++) = 1.0; // alpha + } image_changed_ = true; + mono_ = false; +} + +void Image::set_image_rgba(size_t width, size_t height, const float* image_data_rgba) { + const size_t n = width * height; + image_data_.resize(4 * n); + image_width_ = width; + image_height_ = height; + const float* end = image_data_rgba + 4 * n; + float* dst = image_data_.data(); + while (image_data_rgba != end) { + *(dst++) = *(image_data_rgba++); + *(dst++) = *(image_data_rgba++); + *(dst++) = *(image_data_rgba++); + *(dst++) = *(image_data_rgba++); + } + image_changed_ = true; + mono_ = false; } void Image::set_mask(size_t width, size_t height, const float* mask_data) { @@ -574,6 +675,19 @@ void Image::set_hshift(float hshift) { position_changed_ = true; } +void Image::set_palette(const float* palette, size_t palette_size) { + palette_data_.resize(palette_size * 3); + std::copy(palette, palette + (palette_size * 3), palette_data_.begin()); + palette_changed_ = true; + use_palette_ = true; +} + +void Image::clear_palette() { + palette_data_.clear(); + palette_changed_ = true; + use_palette_ = false; +} + Cuboid::Cuboid(const mat4d& pose, const std::array& rgba) { set_transform(pose); set_rgba(rgba); @@ -732,11 +846,11 @@ void add_default_controls(viz::PointViz& viz, std::mutex* mx) { viz.camera().yaw(sensitivity * dx); viz.camera().pitch(sensitivity * dy); } else if (wc.mbutton_down) { - // convert from pixels to fractions of window size + // convert from screen coordinated to fractions of window size // TODO: factor out conversion? const double window_diagonal = - std::sqrt(wc.viewport_width * wc.viewport_width + - wc.viewport_height * wc.viewport_height); + std::sqrt(wc.window_width * wc.window_width + + wc.window_height * wc.window_height); dx *= 2.0 / window_diagonal; dy *= 2.0 / window_diagonal; viz.camera().dolly_xy(dx, dy); diff --git a/python/Dockerfile b/python/Dockerfile index 85bab069..243ae8e9 100644 --- a/python/Dockerfile +++ b/python/Dockerfile @@ -4,6 +4,10 @@ FROM ${BASE} ENV DEBIAN_FRONTEND=noninteractive \ BUILD_HOME=/var/lib/build +# Set up non-root build user and environment +ARG BUILD_UID=1000 +ARG BUILD_GID=${BUILD_UID} + # Install build dependencies RUN set -xe \ && apt-get update \ @@ -27,6 +31,7 @@ RUN set -xe \ pybind11-dev \ # Install any additional available cpython versions for testing 'python3.(7|8|9|10)-dev' \ + wget \ && rm -rf /var/lib/apt/lists # Set up non-root build user and environment diff --git a/python/setup.py b/python/setup.py index c5b70144..2bffcdae 100644 --- a/python/setup.py +++ b/python/setup.py @@ -26,7 +26,7 @@ def parse_version(): with open(os.path.join(OUSTER_SDK_PATH, 'CMakeLists.txt')) as listfile: content = listfile.read() - groups = re.search("set\(OusterSDK_VERSION_STRING ([^-\)]+)(.(.*))?\)", content) + groups = re.search(r"set\(OusterSDK_VERSION_STRING ([^-\)]+)(.(.*))?\)", content) return groups.group(1) + (groups.group(3) or "") @@ -147,7 +147,7 @@ def run(self): install_requires=[ 'more-itertools >=8.6', 'numpy >=1.19, <2, !=1.19.4', - 'typing-extensions >=3.7', + 'typing-extensions >=3.7.4.3', 'Pillow >=9.2' ], extras_require={ @@ -169,4 +169,5 @@ def run(self): 'PyQt5; platform_system=="Windows"', ], }, - entry_points={'console_scripts': ['simple-viz=ouster.sdk.simple_viz:main']}) + entry_points={'console_scripts': ['simple-viz=ouster.sdk.simple_viz:main', + 'convert-meta-to-legacy=ouster.sdk.convert_to_legacy:main']}) diff --git a/python/src/cpp/_client.cpp b/python/src/cpp/_client.cpp index e4bbad8a..70dc4e9e 100644 --- a/python/src/cpp/_client.cpp +++ b/python/src/cpp/_client.cpp @@ -83,8 +83,8 @@ extern const Table multipurpose_io_mode_strings; extern const Table polarity_strings; extern const Table nmea_baud_rate_strings; -extern Table chanfield_strings; -extern Table udp_profile_lidar_strings; +extern Table chanfield_strings; +extern Table udp_profile_lidar_strings; extern Table udp_profile_imu_strings; extern Table shot_limiting_status_strings; extern Table @@ -427,7 +427,8 @@ PYBIND11_PLUGIN(_client) { .def_readwrite("pixel_shift_by_row", &data_format::pixel_shift_by_row) .def_readwrite("column_window", &data_format::column_window) .def_readwrite("udp_profile_lidar", &data_format::udp_profile_lidar) - .def_readwrite("udp_profile_imu", &data_format::udp_profile_imu); + .def_readwrite("udp_profile_imu", &data_format::udp_profile_imu) + .def_readwrite("fps", &data_format::fps); // Sensor Info py::class_(m, "SensorInfo", R"( @@ -464,8 +465,9 @@ PYBIND11_PLUGIN(_client) { )") .def("__eq__", [](const sensor_info& i, const sensor_info& j) { return i == j; }) .def("__repr__", [](const sensor_info& self) { + const auto mode = self.mode ? to_string(self.mode) : std::to_string(self.format.fps) + "fps"; return ""; + self.sn + " " + self.fw_rev + " " + mode + ">"; }) .def("__copy__", [](const sensor_info& self) { return sensor_info{self}; }) .def("__deepcopy__", [](const sensor_info& self, py::dict) { return sensor_info{self}; }); @@ -634,6 +636,16 @@ PYBIND11_PLUGIN(_client) { )", py::arg("hostname"), py::arg("config"), py::arg("persist") = false, py::arg("udp_dest_auto") = false, py::arg("force_reinit") = false); + m.def("convert_to_legacy", &ouster::sensor::convert_to_legacy , R"( + Convert a non-legacy metadata in string format to legacy + + Args: + metadata: non-legacy metadata string + + Returns: + returns legacy string representation of metadata. + )", py::arg("metadata")); + m.def("get_config", [](const std::string& hostname, bool active) { sensor::sensor_config config; if (!sensor::get_config(hostname, config, active)) { @@ -742,7 +754,6 @@ PYBIND11_PLUGIN(_client) { fields are staggered, so the ith column header value corresponds to the ith column of data in each field. )") - .def_readonly_static("N_FIELDS", &LidarScan::N_FIELDS, "Deprecated.") // TODO: Python and C++ API differ in h/w order for some reason .def( "__init__", @@ -853,53 +864,6 @@ PYBIND11_PLUGIN(_client) { "The frame shot limiting status.") .def("thermal_shutdown", &LidarScan::thermal_shutdown, "The frame thermal shutdown status.") - .def( - "header", - [](LidarScan& self, py::object& o) { - // the argument should be a ColHeader enum defined in - // data.py - auto ind = py::int_(o).cast(); - switch (ind) { - case 0: - return py::array(py::dtype::of(), - static_cast(self.w), - self.timestamp().data(), - py::cast(self)); - case 1: - // encoder values are deprecated and not included in - // the updated C++ LidarScan API. Access old values - // instead - return py::array(py::dtype::of(), - {static_cast(self.w)}, - {sizeof(LidarScan::BlockHeader)}, - &self.headers.at(0).encoder, - py::cast(self)); - case 2: - return py::array(py::dtype::of(), - static_cast(self.w), - self.measurement_id().data(), - py::cast(self)); - case 3: - return py::array(py::dtype::of(), - static_cast(self.w), - self.status().data(), py::cast(self)); - default: - throw std::invalid_argument( - "Unexpected index for LidarScan.header()"); - } - }, - R"( - Return the specified column header as a numpy array. - - This function is deprecated. Use the ``measurment_id``, ``timestamp`` or - ``status`` properties instead. - - Args: - header: The column header to return - - Returns: - The specified column header as a numpy array - )") .def_property_readonly( "timestamp", [](LidarScan& self) { @@ -1021,6 +985,78 @@ PYBIND11_PLUGIN(_client) { .def("__call__", &image_proc_call, py::arg("image"), py::arg("update_state") = true); + // Imu + py::class_(m, "Imu") + .def( + "__init__", + [](ouster::Imu& self, py::buffer& buf, + const sensor::packet_format& pf) { + new (&self) ouster::Imu{}; + ouster::packet_to_imu(getptr(pf.imu_packet_size, buf), pf, + self); + }, + py::arg("buf"), py::arg("pf"), + "Creates imu object from ``buf`` and ``pf``") + .def( + "__init__", + [](ouster::Imu& self, py::array_t& accel, + py::array_t& angular_vel, uint64_t sys_ts, + uint64_t accel_ts, uint64_t gyro_ts) { + py::buffer_info accel_buf = accel.request(); + py::buffer_info angular_buf = angular_vel.request(); + + if (accel_buf.ndim != 1 || angular_buf.ndim != 1) { + throw std::invalid_argument( + "Expect number of dimensions 1"); + } + new (&self) ouster::Imu{}; + if (accel_buf.size == 3) { + double* ptr = static_cast(accel_buf.ptr); + self.linear_accel[0] = *ptr; + self.linear_accel[1] = *(ptr + 1); + self.linear_accel[2] = *(ptr + 2); + } + if (angular_buf.size == 3) { + double* ptr = static_cast(angular_buf.ptr); + self.angular_vel[0] = *ptr; + self.angular_vel[1] = *(ptr + 1); + self.angular_vel[2] = *(ptr + 2); + } + self.sys_ts = sys_ts; + self.accel_ts = accel_ts; + self.gyro_ts = gyro_ts; + }, + py::arg("accel"), py::arg("angular_vel"), py::arg("sys_ts") = 0, + py::arg("accel_ts") = 0, py::arg("gyro_ts") = 0, + "Creates ``client.Imu`` object from imu data.") + .def_property_readonly( + "sys_ts", [](const ouster::Imu& imu) { return imu.sys_ts; }, + "System timestamp (ns)") + .def_property_readonly( + "accel_ts", [](const ouster::Imu& imu) { return imu.accel_ts; }, + "Accelerometer timestamp (ns)") + .def_property_readonly( + "gyro_ts", [](const ouster::Imu& imu) { return imu.gyro_ts; }, + "Gyroscope timestamp (ns)") + .def_property_readonly( + "accel", + [](const ouster::Imu& imu) { + return py::array(py::dtype::of(), + imu.linear_accel.size(), + imu.linear_accel.data()); + }, + "Accelerometer data") + .def_property_readonly( + "angular_vel", + [](const ouster::Imu& imu) { + return py::array(py::dtype::of(), + imu.angular_vel.size(), + imu.angular_vel.data()); + }, + "Angular velocity data") + .def("__repr__", [](ouster::Imu& p) { return ouster::to_string(p); }) + .def("__str__", [](ouster::Imu& p) { return ouster::to_string(p); }); + m.attr("__version__") = ouster::SDK_VERSION; return m.ptr(); diff --git a/python/src/cpp/_pcap.cpp b/python/src/cpp/_pcap.cpp index 74454e4c..dd2f5e61 100644 --- a/python/src/cpp/_pcap.cpp +++ b/python/src/cpp/_pcap.cpp @@ -6,20 +6,35 @@ * @brief ouster_pyclient_pcap python module */ #include +#include +#include +#include +#include +#include #include #include #include #include "ouster/impl/build.h" +#include "ouster/pcap.h" #include "ouster/os_pcap.h" +#include "ouster/indexed_pcap_reader.h" using namespace ouster::sensor_utils; namespace py = pybind11; PYBIND11_MAKE_OPAQUE(std::shared_ptr); PYBIND11_MAKE_OPAQUE(std::shared_ptr); - +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +using stream_map = std::map; +PYBIND11_MAKE_OPAQUE(stream_map); +using count_map = std::map; +PYBIND11_MAKE_OPAQUE(count_map); PYBIND11_PLUGIN(_pcap) { py::module m("_pcap", R"(Pcap bindings generated by pybind11. @@ -29,7 +44,13 @@ This module is generated from the C++ code and not meant to be used directly. // turn off signatures in docstrings: mypy stubs provide better types py::options options; options.disable_function_signatures(); - + py::bind_vector>(m, "VectorStreamKey"); + py::bind_vector>(m, "VectorInt"); + py::bind_vector>(m, "VectorGuessedPorts"); + py::bind_vector>(m, "VectorUint64"); + py::bind_vector>(m, "VectorUint8"); + py::bind_map>(m, "MapUdpStreams"); + py::bind_map>(m, "CountMap"); py::class_>(m, "packet_info") .def(py::init<>()) .def("__repr__", @@ -38,21 +59,80 @@ This module is generated from the C++ code and not meant to be used directly. result << data; return result.str(); }) - .def_readonly("dst_ip", &packet_info::dst_ip) - .def_readonly("src_ip", &packet_info::src_ip) - .def_readonly("dst_port", &packet_info::dst_port) - .def_readonly("src_port", &packet_info::src_port) + .def_readwrite("dst_ip", &packet_info::dst_ip) + .def_readwrite("src_ip", &packet_info::src_ip) + .def_readwrite("dst_port", &packet_info::dst_port) + .def_readwrite("src_port", &packet_info::src_port) .def_readonly("payload_size", &packet_info::payload_size) - .def_property_readonly("timestamp", - [](packet_info& packet_info) -> double { - return packet_info.timestamp.count() / 1e6; - }) + .def_property( + "timestamp", + [](packet_info& packet_info) -> double { + return packet_info.timestamp.count() / 1e6; + }, + [](packet_info& packet_info, double set) { + std::chrono::microseconds msec{(int)(set * 1e6)}; + packet_info.timestamp = msec; + }) + .def_readonly("file_offset", &packet_info::file_offset) .def_readonly("fragments_in_packet", &packet_info::fragments_in_packet) .def_readonly("ip_version", &packet_info::ip_version) .def_readonly("encapsulation_protocol", &packet_info::encapsulation_protocol) .def_readonly("network_protocol", &packet_info::network_protocol); + py::class_>(m, "guessed_ports") + .def(py::init<>()) + .def_readonly("lidar", &guessed_ports::lidar) + .def_readonly("imu", &guessed_ports::imu); + + py::class_>(m, "stream_key") + .def(py::init<>()) + .def("__repr__", + [](const stream_key& data) { + std::stringstream result; + result << data; + return result.str(); + }) + .def_readonly("dst_ip", &stream_key::dst_ip) + .def_readonly("src_ip", &stream_key::src_ip) + .def_readonly("dst_port", &stream_key::dst_port) + .def_readonly("src_port", &stream_key::src_port); + + py::class_>(m, "stream_data") + .def(py::init<>()) + .def("__repr__", + [](const stream_data& data) { + std::stringstream result; + result << data; + return result.str(); + }) + .def_readonly("count", &stream_data::count) + .def_readonly("payload_size_counts", &stream_data::payload_size_counts) + .def_readonly("fragment_counts", &stream_data::fragment_counts) + .def_readonly("ip_version_counts", &stream_data::ip_version_counts); + + py::class_>(m, "stream_info") + .def(py::init<>()) + .def("__repr__", + [](const stream_info& data) { + std::stringstream result; + result << data; + return result.str(); + }) + .def_readonly("total_packets", &stream_info::total_packets) + .def_readonly("encapsulation_protocol", + &stream_info::encapsulation_protocol) + .def_readonly("timestamp_max", &stream_info::timestamp_max) + .def_readonly("timestamp_min", &stream_info::timestamp_min) + .def_readonly("udp_streams", &stream_info::udp_streams) + .def_property_readonly("timestamp_max", + [](stream_info& info) -> double { + return info.timestamp_max.count() / 1e6; + }) + .def_property_readonly("timestamp_min", + [](stream_info& info) -> double { + return info.timestamp_min.count() / 1e6; + }); // pcap reading py::class_>(m, "playback_handle"); @@ -66,6 +146,20 @@ This module is generated from the C++ code and not meant to be used directly. [](std::shared_ptr& handle, packet_info& packet_info) -> bool { return next_packet_info(*handle, packet_info); }); + m.def("get_stream_info", [] (const std::string& file, + int packets_to_process=-1) { + return get_stream_info(file, packets_to_process); + }); + + m.def("get_stream_info", [] (const std::string& file, + std::function progress_callback, + int packets_per_callback, + int packets_to_process=-1) + { + return get_stream_info(file, progress_callback, + packets_per_callback, packets_to_process); + }); + m.def("guess_ports", &guess_ports); m.def( "read_packet", [](std::shared_ptr& handle, py::buffer buf) -> size_t { @@ -77,7 +171,7 @@ This module is generated from the C++ code and not meant to be used directly. return read_packet(*handle, static_cast(info.ptr), info.size); }); - + m.def("replay_reset", [](std::shared_ptr& handle) { replay_reset(*handle); }); @@ -86,12 +180,11 @@ This module is generated from the C++ code and not meant to be used directly. py::class_>(m, "record_handle"); m.def("record_initialize", - py::overload_cast(&record_initialize), - py::arg("file_name"), py::arg("src_ip"), py::arg("dst_ip"), - py::arg("frag_size"), py::arg("use_sll_encapsulation") = false, + py::overload_cast(&record_initialize), + py::arg("file_name"), py::arg("frag_size"), + py::arg("use_sll_encapsulation") = false, R"( - ``def record_initialize(file_name: str, src_ip: str, dst_ip: str, frag_size: int, + ``def record_initialize(file_name: str, frag_size: int, use_sll_encapsulation: bool = ...) -> record_handle:`` Initialize record handle for single sensor pcap files @@ -103,19 +196,53 @@ This module is generated from the C++ code and not meant to be used directly. }); m.def("record_packet", - [](std::shared_ptr& handle, int src_port, int dst_port, + [](std::shared_ptr& handle, const std::string& src_ip, + const std::string& dst_ip, int src_port, int dst_port, py::buffer buf, double timestamp) { auto info = buf.request(); if (info.format != py::format_descriptor::format()) { throw std::invalid_argument( "Incompatible argument: expected a bytearray"); } - record_packet(*handle, src_port, dst_port, + record_packet(*handle, src_ip, dst_ip, src_port, dst_port, static_cast(info.ptr), info.size, llround(timestamp * 1e6)); }); m.attr("__version__") = ouster::SDK_VERSION; + m.def("record_packet", [](std::shared_ptr& handle, + const packet_info& info, py::buffer buf) { + auto buf_info = buf.request(); + if (buf_info.format != py::format_descriptor::format()) { + throw std::invalid_argument( + "Incompatible argument: expected a bytearray"); + } + record_packet(*handle, info, static_cast(buf_info.ptr), + buf_info.size); + }); + + py::class_(m, "PcapReader"); // TODO add more complete bindings + + py::class_(m, "IndexedPcapReader") + .def(py::init&, std::function>()) + .def("frame_count", &IndexedPcapReader::frame_count) + .def("seek_to_frame", &IndexedPcapReader::seek_to_frame) + .def("get_stream_info", &IndexedPcapReader::get_stream_info) + .def("current_info", &IndexedPcapReader::current_info) + .def("next_packet", &IndexedPcapReader::next_packet) + .def("current_frame_id",[](IndexedPcapReader& reader) -> py::object { + if(auto frame_id = reader.current_frame_id()) { + return py::int_(*frame_id); + } + return py::none(); + }) + .def("current_data", [](IndexedPcapReader& reader) -> py::array { + uint8_t* data = const_cast(reader.current_data()); + size_t data_size = reader.current_length(); + return py::array(py::dtype::of(), data_size, data, py::cast(reader)); + }) + .def_readonly("frame_indices", &IndexedPcapReader::frame_indices_); + return m.ptr(); } diff --git a/python/src/cpp/_viz.cpp b/python/src/cpp/_viz.cpp index 7a6877f2..574c4f68 100644 --- a/python/src/cpp/_viz.cpp +++ b/python/src/cpp/_viz.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "ouster/impl/build.h" #include "ouster/lidar_scan.h" @@ -33,12 +34,14 @@ template static void check_array(const py::array_t& array, size_t size = 0, size_t dims = 0, char storage = 'X') { if (size && static_cast(array.size()) != size) - throw std::invalid_argument("Expected array of size: " + - std::to_string(size)); + throw std::invalid_argument( + "Expected array of size: " + std::to_string(size) + + ", but got: " + std::to_string(array.size())); if (dims && static_cast(array.ndim()) != dims) - throw std::invalid_argument("Expected an array of dimension: " + - std::to_string(dims)); + throw std::invalid_argument( + "Expected an array of dimension: " + std::to_string(dims) + + ", but got: " + std::to_string(array.ndim())); if (storage == 'F' && !(array.flags() & py::array::f_style)) throw std::invalid_argument("Expected a F_CONTIGUOUS array"); @@ -72,7 +75,7 @@ using pymatrixd = PYBIND11_PLUGIN(_viz) { py::module m("_viz", R"( PointViz bindings generated by pybind11. - + This module is generated from the C++ code and not meant to be used directly. )"); @@ -120,6 +123,9 @@ PYBIND11_PLUGIN(_viz) { .def("update", &viz::PointViz::update, "Show updated data in the next rendered frame.") + .def("visible", &viz::PointViz::visible, + "Toggle if the PointViz window is visible") + // misc .def( "push_key_handler", @@ -163,6 +169,13 @@ PYBIND11_PLUGIN(_viz) { &viz::PointViz::viewport_height, "Current viewport height in pixels") + .def_property_readonly("window_width", &viz::PointViz::window_width, + "Current window width in screen coordinates") + + .def_property_readonly("window_height", + &viz::PointViz::window_height, + "Current window height in screen coordinates") + .def("add", py::overload_cast&>( &viz::PointViz::add), @@ -213,7 +226,11 @@ PYBIND11_PLUGIN(_viz) { .def_readonly("viewport_width", &viz::WindowCtx::viewport_width, "Current viewport width in pixels") .def_readonly("viewport_height", &viz::WindowCtx::viewport_height, - "Current viewport height in pixels"); + "Current viewport height in pixels") + .def_readonly("window_width", &viz::WindowCtx::window_width, + "Current window width in screen coordinates") + .def_readonly("window_height", &viz::WindowCtx::window_height, + "Current window height in screen coordinates"); py::class_(m, "Camera", "Controls the camera view and projection.") @@ -312,10 +329,17 @@ PYBIND11_PLUGIN(_viz) { )") .def( "__init__", - [](viz::Cloud& self, size_t n) { new (&self) viz::Cloud{n}; }, - py::arg("n"), + [](viz::Cloud& self, size_t n, pymatrixd extrinsics) { + // expect homogeneous 4x4 pose matrix in 2d or 1d shape + check_array(extrinsics, 16, 0, 'F'); + viz::mat4d extrinsica; + std::copy(extrinsics.data(), extrinsics.data() + 16, + extrinsica.data()); + new (&self) viz::Cloud(n, extrinsica); + }, + py::arg("n"), py::arg("extrinsics") = viz::identity4d, R"( - ``def __init__(self, n_points: int) -> None:`` + ``def __init__(self, n_points: int, extrinsics: np.ndarray) -> None:`` Unstructured point cloud for visualization. @@ -323,8 +347,8 @@ PYBIND11_PLUGIN(_viz) { Args: n: number of points - extrinsic: sensor extrinsic calibration. 4x4 column-major - homogeneous transformation matrix + extrinsics: sensor extrinsic calibration. 4x4 column-major + homogeneous transformation matrix )") .def( "__init__", @@ -374,17 +398,71 @@ PYBIND11_PLUGIN(_viz) { .def( "set_key", [](viz::Cloud& self, py::array_t key) { - check_array(key, self.get_size(), 0, 'C'); - self.set_key(key.data()); + // TODO[pb]: Error reporting and error messages needs + // improvements (across all _viz module in general) + check_array(key, 0, 0, 'C'); + if (static_cast(key.size()) == self.get_size()) { + if (key.ndim() == 1 || key.ndim() == 2 || key.ndim() == 3) { + self.set_key(key.data()); + return; + } + } + + check_array(key, 0, 3, 'C'); + + if (key.shape(2) == 3) { + check_array(key, self.get_size() * 3); + self.set_key_rgb(key.data()); + return; + } + + if (key.shape(2) == 4) { + check_array(key, self.get_size() * 4); + self.set_key_rgba(key.data()); + return; + } + + throw std::invalid_argument( + "Expected array with size of 3rd dimension: 1,3 or " + "4, but got: " + + std::to_string(key.shape(2))); }, py::arg("key"), R"( Set the key values, used for colouring. + Number of elements defines the type of Cloud coloration: + - num elements == cloud.get_size(): MONO with palette + - 3 dimensions with the last dimesion: 3 - RGB, 4 - RGBA, + no palette used + Args: key: array of at least as many elements as there are points, preferably normalized between 0 and 1 )") + .def( + "set_key_alpha", + [](viz::Cloud& self, py::array_t key_alpha) { + check_array(key_alpha, self.get_size(), 0, 'C'); + self.set_key_alpha(key_alpha.data()); + }, + py::arg("key_alpha"), + R"( + Set the key alpha values, color is not changed. + + The set alpha will be applied for these forms of + CLoud.set_key() calls: + + - MONO (palette used to pick a color) + - RGB + + However when set_key() is used with RGBA the previously set alpha + with set_key_alpha() will be overwritten with the new values. + + Args: + key_alpha: array of at least as many elements as there are + points, normalized between 0 and 1 + )") .def( "set_mask", [](viz::Cloud& self, py::array_t mask) { @@ -487,17 +565,13 @@ PYBIND11_PLUGIN(_viz) { .def("__copy__", [](const viz::Cloud& self) { return viz::Cloud{self}; }) .def("__deepcopy__", - [](viz::Cloud& self, py::dict) { - return viz::Cloud{self}; - }) - .def("__repr__", - [](const viz::Cloud& self) { - std::stringstream ss; - ss << ""; - return ss.str(); - }); + [](viz::Cloud& self, py::dict) { return viz::Cloud{self}; }) + .def("__repr__", [](const viz::Cloud& self) { + std::stringstream ss; + ss << ""; + return ss.str(); + }); py::class_>( m, "Image", "Manages the state of an image.") @@ -505,14 +579,46 @@ PYBIND11_PLUGIN(_viz) { .def( "set_image", [](viz::Image& self, py::array_t image) { - check_array(image, 0, 2, 'C'); - self.set_image(image.shape(1), image.shape(0), image.data()); + check_array(image, 0, 0, 'C'); // check for C-CONTIGUOUS + if (image.ndim() == 2 || + (image.ndim() == 3 && image.shape(2) == 1)) { + // MONO + self.set_image(image.shape(1), image.shape(0), + image.data()); + return; + } + + check_array(image, 0, 3); + + if (image.shape(2) == 3) { + // RGB + self.set_image_rgb(image.shape(1), image.shape(0), + image.data()); + return; + } + + if (image.shape(2) == 4) { + // RGBA + self.set_image_rgba(image.shape(1), image.shape(0), + image.data()); + return; + } + + throw std::invalid_argument( + "Expected array with size of 3rd dimension: 1,3 or " + "4, but got: " + + std::to_string(image.shape(2))); }, py::arg("image"), R"( - Set the image data. + Set the image data, MONO or RGB/RGBA depending on dimensions. + + Color palette is applied for MONO mode if set_palette() was + used to set the palette, otherwise MONO mode makes the + monochrome image. Args: - image: 2D array with image data + image: 2D array of floats for a monochrome image or 3D array + with RGB or RGBA components for color image. )") .def( "set_mask", @@ -559,7 +665,24 @@ PYBIND11_PLUGIN(_viz) { ``-1`` - image moved to the left for the 1/2 of a horizontal viewport ``+1`` - image moved to the right for the 1/2 of a horizontal viewport ``+0.5`` - image moved to the right for the 1/4 of a horizontal viewport - )"); + )") + .def( + "set_palette", + [](viz::Image& self, py::array_t buf) { + check_array(buf, 0, 2, 'C'); + if (buf.shape(1) != 3) + throw std::invalid_argument("Expected a N x 3 array"); + self.set_palette(buf.data(), buf.shape(0)); + }, + py::arg("palette"), + R"( + Set the image color palette. + + Args: + palette: the new palette to use, must have size 3*palette_size + )") + .def("clear_palette", &viz::Image::clear_palette, + "Removes the image palette and use keys as grey color in MONO"); py::class_>( m, "Cuboid", "Manages the state of a single cuboid.") @@ -711,6 +834,15 @@ PYBIND11_PLUGIN(_viz) { m.attr("calref_palette") = py::array_t{ {static_cast(viz::calref_n), static_cast(3)}, &viz::calref_palette[0][0]}; + m.attr("grey_palette") = py::array_t{ + {static_cast(viz::grey_n), static_cast(3)}, + &viz::grey_palette[0][0]}; + m.attr("viridis_palette") = py::array_t{ + {static_cast(viz::viridis_n), static_cast(3)}, + &viz::viridis_palette[0][0]}; + m.attr("magma_palette") = py::array_t{ + {static_cast(viz::magma_n), static_cast(3)}, + &viz::magma_palette[0][0]}; m.attr("__version__") = ouster::SDK_VERSION; diff --git a/python/src/ouster/client/__init__.py b/python/src/ouster/client/__init__.py index 97ef8d39..4fe2f580 100644 --- a/python/src/ouster/client/__init__.py +++ b/python/src/ouster/client/__init__.py @@ -19,9 +19,11 @@ from ._client import UDPProfileIMU from ._client import SensorConfig from ._client import init_logger +from ._client import convert_to_legacy from ._client import get_config from ._client import set_config from ._client import LidarScan +from ._client import Imu from .data import BufferT from .data import FieldDType @@ -31,6 +33,8 @@ from .data import ColHeader from .data import XYZLut from .data import destagger +from .data import PacketIdError +from .data import imu_from_packet from .core import ClientError from .core import ClientTimeout diff --git a/python/src/ouster/client/_client.pyi b/python/src/ouster/client/_client.pyi index 2604c420..b36ec461 100644 --- a/python/src/ouster/client/_client.pyi +++ b/python/src/ouster/client/_client.pyi @@ -142,6 +142,7 @@ class DataFormat: column_window: Tuple[int, int] udp_profile_lidar: UDPProfileLidar udp_profile_imu: UDPProfileIMU + fps: int class PacketFormat: @@ -412,6 +413,7 @@ class ChanField: FLAGS: ClassVar[ChanField] FLAGS2: ClassVar[ChanField] NEAR_IR: ClassVar[ChanField] + RAW_HEADERS: ClassVar[ChanField] CUSTOM0: ClassVar[ChanField] CUSTOM1: ClassVar[ChanField] CUSTOM2: ClassVar[ChanField] @@ -426,6 +428,11 @@ class ChanField: RAW32_WORD2: ClassVar[ChanField] RAW32_WORD3: ClassVar[ChanField] RAW32_WORD4: ClassVar[ChanField] + RAW32_WORD5: ClassVar[ChanField] + RAW32_WORD6: ClassVar[ChanField] + RAW32_WORD7: ClassVar[ChanField] + RAW32_WORD8: ClassVar[ChanField] + RAW32_WORD9: ClassVar[ChanField] __members__: ClassVar[Dict[str, ChanField]] values: ClassVar[Iterator[ChanField]] @@ -454,6 +461,7 @@ class UDPProfileLidar: PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL: ClassVar[UDPProfileLidar] PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16: ClassVar[UDPProfileLidar] PROFILE_LIDAR_RNG15_RFL8_NIR8: ClassVar[UDPProfileLidar] + PROFILE_LIDAR_FIVE_WORD_PIXEL: ClassVar[UDPProfileLidar] __members__: ClassVar[Dict[str, UDPProfileLidar]] values: ClassVar[Iterator[UDPProfileLidar]] @@ -535,6 +543,8 @@ class SensorConfig: def __init__(self, config_string: str) -> None: ... +def convert_to_legacy(metadata: str) -> str: + ... def init_logger(log_level: str, log_file_path: str = ..., @@ -575,7 +585,6 @@ class Version: class LidarScan: - N_FIELDS: ClassVar[int] frame_id: int frame_status: int @@ -608,9 +617,6 @@ class LidarScan: def shot_limiting(self) -> int: ... - def header(self, header: ColHeader) -> ndarray: - ... - def field(self, field: ChanField) -> ndarray: ... @@ -727,7 +733,7 @@ class AutoExposure: update_every: int) -> None: ... - def __call__(self, image: ndarray) -> None: + def __call__(self, image: ndarray, update_state: Optional[bool] = True) -> None: ... @@ -737,3 +743,22 @@ class BeamUniformityCorrector: def __call__(self, image: ndarray) -> None: ... + +class Imu: + @overload + def __init__(self, buf: BufferT, pf) -> None: ... + + @overload + def __init__(self, accel: ndarray, angular_vel: ndarray, + sys_ts: int = ..., accel_ts: int = ..., gyro_ts: int = ...) -> None: ... + + @property + def accel(self) -> ndarray: ... + @property + def accel_ts(self) -> int: ... + @property + def angular_vel(self) -> ndarray: ... + @property + def gyro_ts(self) -> int: ... + @property + def sys_ts(self) -> int: ... diff --git a/python/src/ouster/client/_digest.py b/python/src/ouster/client/_digest.py index 803e5eff..464a8962 100644 --- a/python/src/ouster/client/_digest.py +++ b/python/src/ouster/client/_digest.py @@ -42,7 +42,10 @@ def __eq__(self, other: object): def check(self, other: 'FieldDigest'): for k, v in sorted(self.hashes.items()): - assert other.hashes.get(k) == v, f"Match failure key: {k}" + # TODO - remove when remove deprecated 'ENCODER_COUNT' + # TODO (cnt) from packet parsing and from digest test data + if k != 'ENCODER_COUNT': + assert other.hashes.get(k) == v, f"Match failure key: {k}" @classmethod def from_packet(cls, packet: LidarPacket) -> 'FieldDigest': @@ -73,10 +76,6 @@ def from_scan(cls, ls: LidarScan) -> 'FieldDigest': hashes['STATUS'] = _md5(ls.status.astype(np.uint64)) hashes['MEASUREMENT_ID'] = _md5(ls.measurement_id.astype(np.uint16)) - # deprecated, zeroed out for non-legacy UDP profiles - hashes['ENCODER_COUNT'] = _md5( - ls.header(ColHeader.ENCODER_COUNT).astype(np.uint64)) - hashes.update({c.name: _md5(ls.field(c)) for c in ls.fields}) return cls(**hashes) diff --git a/python/src/ouster/client/core.py b/python/src/ouster/client/core.py index 1566152d..a8c6645a 100644 --- a/python/src/ouster/client/core.py +++ b/python/src/ouster/client/core.py @@ -16,7 +16,8 @@ from . import _client from ._client import (SensorInfo, LidarScan, UDPProfileLidar) -from .data import (ChanField, FieldDType, ImuPacket, LidarPacket, Packet) +from .data import (ChanField, FieldDType, ImuPacket, LidarPacket, Packet, + PacketIdError) class ClientError(Exception): @@ -132,7 +133,8 @@ def __init__(self, _overflow_err: bool = False, _flush_before_read: bool = True, _flush_frames: int = 5, - _legacy_format: bool = True) -> None: + _legacy_format: bool = False, + _soft_id_check: bool = False) -> None: """ Neither the ports nor udp destination configuration on the sensor will be updated. The metadata will be fetched over the network from the @@ -148,6 +150,8 @@ def __init__(self, _overflow_err: if True, raise ClientOverflow _flush_before_read: if True, try to clear buffers before reading _legacy_format: if True, use legacy metadata format + _soft_id_check: if True, don't skip lidar packets buffers on + id mismatch (init_id/sn pair) Raises: ClientError: If initializing the client fails. @@ -161,6 +165,9 @@ def __init__(self, self._flush_frames = _flush_frames self._legacy_format = _legacy_format + self._soft_id_check = _soft_id_check + self._id_error_count = 0 + # Fetch from sensor if not explicitly provided if metadata: self._metadata = metadata @@ -202,7 +209,12 @@ def _next_packet(self) -> Optional[Packet]: if self._overflow_err and st & _client.ClientState.OVERFLOW: raise ClientOverflow() if st & _client.ClientState.LIDAR_DATA: - return LidarPacket(buf, self._metadata) + lp = LidarPacket(buf, + self._metadata, + _raise_on_id_check=not self._soft_id_check) + if lp.id_error: + self._id_error_count += 1 + return lp elif st & _client.ClientState.IMU_DATA: return ImuPacket(buf, self._metadata) elif st == _client.ClientState.TIMEOUT: @@ -242,7 +254,7 @@ def __iter__(self) -> Iterator[Packet]: # Attempt to flush any old data before producing packets if self._flush_before_read: - self.flush(n_frames = self._flush_frames, full=True) + self.flush(n_frames=self._flush_frames, full=True) while True: try: @@ -251,8 +263,10 @@ def __iter__(self) -> Iterator[Packet]: yield p else: break + except PacketIdError: + self._id_error_count += 1 except ValueError: - # bad packet size or init_id here: this can happen when + # bad packet size here: this can happen when # packets are buffered by the OS, not necessarily an error # same pass as in data.py # TODO: introduce status for PacketSource to indicate frequency @@ -312,6 +326,10 @@ def flush(self, n_frames: int = 3, *, full=False) -> int: def buf_use(self) -> int: return self._cli.size + @property + def id_error_count(self) -> int: + return self._id_error_count + def close(self) -> None: """Shut down producer thread and close network connection. @@ -449,9 +467,7 @@ def sample( A tuple of metadata queried from the sensor and an iterator that samples n consecutive scans """ - with closing(Sensor(hostname, - lidar_port, - 7503, + with closing(Sensor(hostname, lidar_port, 7503, metadata=metadata)) as sensor: metadata = sensor.metadata diff --git a/python/src/ouster/client/data.py b/python/src/ouster/client/data.py index 25c1775e..56139213 100644 --- a/python/src/ouster/client/data.py +++ b/python/src/ouster/client/data.py @@ -6,12 +6,13 @@ from copy import deepcopy from enum import Enum from typing import Callable, Iterator, Type, List, Optional, Union +import logging import warnings import numpy as np from . import _client -from ._client import (ChanField, LidarScan, SensorInfo) +from ._client import (ChanField, LidarScan, SensorInfo, Imu) BufferT = Union[bytes, bytearray, memoryview, np.ndarray] """Types that support the buffer protocol.""" @@ -22,6 +23,8 @@ Packet = Union['ImuPacket', 'LidarPacket'] """Packets emitted by a sensor.""" +logger = logging.getLogger("ouster.client.data") + class ImuPacket: """Read IMU Packet data from a bufer.""" @@ -112,6 +115,11 @@ def __int__(self) -> int: return self.value +class PacketIdError(Exception): + """Exception raised when init_id/sn from metadata and packet doesn't match.""" + pass + + class LidarPacket: """Read lidar packet data as numpy arrays. @@ -122,12 +130,16 @@ class LidarPacket: """ _pf: _client.PacketFormat _data: np.ndarray + _metadata_init_id: int + _metadata_sn: int capture_timestamp: Optional[float] def __init__(self, data: BufferT, info: SensorInfo, - timestamp: Optional[float] = None) -> None: + timestamp: Optional[float] = None, + *, + _raise_on_id_check: bool = True) -> None: """ This will always alias the supplied buffer-like object. Pass in a copy to avoid unintentional aliasing. @@ -136,6 +148,8 @@ def __init__(self, data: Buffer containing the packet payload info: Metadata associated with the sensor packet stream timestamp: A capture timestamp, in seconds + _raise_on_id_check: raise PacketIdError if matadata + init_id/sn doesn't match packet init_id/sn. Raises: ValueError: If the buffer is smaller than the size specified by the @@ -146,10 +160,21 @@ def __init__(self, dtype=np.uint8, count=self._pf.lidar_packet_size) self.capture_timestamp = timestamp + self._metadata_init_id = info.init_id + self._metadata_sn = int(info.sn) # check that metadata came from the same sensor initialization as data - if self.init_id and self.init_id != info.init_id: - raise ValueError("Metadata init id does not match") + if self.id_error: + error_msg = f"Metadata init_id/sn does not match: " \ + f"expected by metadata - {info.init_id}/{info.sn}, " \ + f"but got from packet buffer - {self.init_id}/{self.prod_sn}" + if _raise_on_id_check: + raise PacketIdError(error_msg) + else: + # Continue with warning. When init_ids/sn doesn't match + # the resulting LidarPacket has high chances to be + # incompatible with data format set in metadata json file + logger.warn(f"LidarPacket validation: {error_msg}") def __deepcopy__(self, memo) -> 'LidarPacket': cls = type(self) @@ -160,6 +185,12 @@ def __deepcopy__(self, memo) -> 'LidarPacket': cpy.capture_timestamp = self.capture_timestamp return cpy + @property + def id_error(self) -> bool: + """Check the metadata init_id/sn and packet init_id/sn mismatch.""" + return bool(self.init_id and (self.init_id != self._metadata_init_id or + self.prod_sn != self._metadata_sn)) + @property def packet_type(self) -> int: """Get the type header of the packet.""" @@ -366,3 +397,9 @@ def res(ls: Union[LidarScan, np.ndarray]) -> np.ndarray: info.format.columns_per_frame, 3) return res + + +def imu_from_packet(packet: ImuPacket) -> Imu: + """Transform ImuPacket to client.Imu data type""" + return Imu(packet.accel, packet.angular_vel, packet.sys_ts, packet.accel_ts, + packet.gyro_ts) diff --git a/python/src/ouster/pcap/__init__.py b/python/src/ouster/pcap/__init__.py index 69964491..e7511254 100644 --- a/python/src/ouster/pcap/__init__.py +++ b/python/src/ouster/pcap/__init__.py @@ -10,4 +10,3 @@ from .pcap import _guess_ports from .pcap import _packet_info_stream from .pcap import _replay -from .pcap import _stream_info diff --git a/python/src/ouster/pcap/_pcap.pyi b/python/src/ouster/pcap/_pcap.pyi index dcc601d7..9ee0602e 100644 --- a/python/src/ouster/pcap/_pcap.pyi +++ b/python/src/ouster/pcap/_pcap.pyi @@ -5,6 +5,8 @@ All rights reserved. Type annotations for pcap python bindings. """ +from typing import (overload, List, Callable) + from ..client.data import BufferT @@ -16,26 +18,50 @@ class record_handle: pass -class packet_info: +class guessed_ports: def __init__(self) -> None: ... - @property - def dst_ip(self) -> str: - ... + lidar: int + imu: int - @property - def src_ip(self) -> str: - ... - @property - def dst_port(self) -> int: +class stream_info: + def __init__(self) -> None: ... - @property - def src_port(self) -> int: + total_packets: int + encapsulation_protocol: int + + +def guess_ports(file: str, + meta_data_file: str, + packets_to_process: int) -> List[guessed_ports]: + ... + + +@overload +def get_stream_info(file: str, packets_to_process: int) -> stream_info: + pass + + +@overload +def get_stream_info(file: str, progress_callback: Callable[[int, int], int], + callback_frequency: int, + packets_to_process: int) -> stream_info: + pass + + +class packet_info: + + def __init__(self) -> None: ... + dst_ip: str + src_ip: str + dst_port: int + src_port: int + @property def payload_size(self) -> int: ... @@ -44,6 +70,10 @@ class packet_info: def timestamp(self) -> float: ... + @timestamp.setter + def timestamp(self, seconds: float) -> None: + ... + @property def fragments_in_packet(self) -> int: ... @@ -82,8 +112,6 @@ def replay_reset(handle: playback_handle) -> None: def record_initialize(file_name: str, - src_ip: str, - dst_ip: str, frag_size: int, use_sll_encapsulation: bool = ...) -> record_handle: ... @@ -93,6 +121,14 @@ def record_uninitialize(handle: record_handle) -> None: ... -def record_packet(handle: record_handle, src_port: int, dst_port: int, - buf: BufferT, timestamp: float) -> None: +@overload +def record_packet(handle: record_handle, src_ip: str, dst_ip: str, + src_port: int, dst_port: int, buf: BufferT, + timestamp: float) -> None: + ... + + +@overload +def record_packet(handle: record_handle, info: packet_info, + buf: BufferT) -> None: ... diff --git a/python/src/ouster/pcap/pcap.py b/python/src/ouster/pcap/pcap.py index 314a3646..9cac472e 100644 --- a/python/src/ouster/pcap/pcap.py +++ b/python/src/ouster/pcap/pcap.py @@ -2,130 +2,36 @@ Copyright (c) 2021, Ouster, Inc. All rights reserved. """ -from collections import defaultdict from copy import copy -from dataclasses import dataclass, field -from itertools import islice import os import socket import time from threading import Lock -from typing import (Dict, Iterable, Iterator, List, Optional, Set, Tuple) +from typing import (Iterable, Iterator, Optional, Tuple) from ouster.client import (LidarPacket, ImuPacket, Packet, PacketSource, - SensorInfo, _client) + SensorInfo, _client, PacketIdError) from . import _pcap +MTU_SIZE = 1500 -@dataclass(frozen=True, order=True) -class _UDPStreamKey: - """Identifies a single logical 'stream' of UDP packets.""" - src_ip: str - dst_ip: str - src_port: int - dst_port: int - - -@dataclass -class _UDPStreamInfo: - """Info about packets in a stream.""" - count: int = 0 - payload_size: Set[int] = field(default_factory=set) - fragments_in_packet: Set[int] = field(default_factory=set) - ip_version: Set[int] = field(default_factory=set) - - -class _stream_info: - """Gather some useful info about UDP data in a pcap.""" - - def __init__(self, infos: Iterable[_pcap.packet_info]) -> None: - self.total_packets = 0 - self.encapsulation_protocol = set() - self.timestamp_min = float('inf') - self.timestamp_max = float('-inf') - self.udp_streams: Dict[_UDPStreamKey, - _UDPStreamInfo] = defaultdict(_UDPStreamInfo) - - for i in infos: - self.total_packets += 1 - self.encapsulation_protocol.add(i.encapsulation_protocol) - self.timestamp_min = min(self.timestamp_min, i.timestamp) - self.timestamp_max = max(self.timestamp_max, i.timestamp) - - val = self.udp_streams[_UDPStreamKey(i.src_ip, i.dst_ip, - i.src_port, i.dst_port)] - val.count += 1 - val.payload_size.add(i.payload_size) - val.fragments_in_packet.add(i.fragments_in_packet) - val.ip_version.add(i.ip_version) - - -def _guess_ports(udp_streams: Dict[_UDPStreamKey, _UDPStreamInfo], - info: SensorInfo) -> List[Tuple[int, int]]: - """Find possible UDP sources matching the metadata. - - The current approach is roughly: 1) treat each unique source / destination - port and IP as a single logical 'stream' of data, 2) filter out streams that - don't match the expected packet sizes specified by the metadata, 3) pair up - any potential lidar/imu streams that appear to be coming from the same - sensor (have matching source IPs) 4) and finally, filter out the pairs that - contradict any ports specified in the metadata. - Returns: - List of dst port pairs that probably contain lidar/imu data. Duplicate - entries are possible and indicate packets from distinct sources. - """ - - # allow lone streams when there's no matching data from the same ip - lidar_keys: Set[Optional[_UDPStreamKey]] = {None} - imu_keys: Set[Optional[_UDPStreamKey]] = {None} - - # find all lidar and imu 'streams' that match expected packet sizes - pf = _client.PacketFormat.from_info(info) - ss = udp_streams.items() - lidar_keys |= {k for k, v in ss if pf.lidar_packet_size in v.payload_size} - imu_keys |= {k for k, v in ss if pf.imu_packet_size in v.payload_size} - - # find all src ips for candidate streams - lidar_src_ips = {k.src_ip for k in lidar_keys if k} - imu_src_ips = {k.src_ip for k in imu_keys if k} - - # yapf: disable - # "full outer join" on src_ip to produce lidar/imu streams from one source - keys = [(klidar, kimu) for klidar in lidar_keys for kimu in imu_keys - if (klidar and kimu and klidar.src_ip == kimu.src_ip) - or (not klidar and kimu and kimu.src_ip not in lidar_src_ips) - or (klidar and not kimu and klidar.src_ip not in imu_src_ips)] - - # map down to just dst port pairs, with 0 meaning none found - ports = [(klidar.dst_port if klidar else 0, kimu.dst_port if kimu else 0) - for (klidar, kimu) in keys] - - # filter out candidates that don't match specified ports - lidar_spec, imu_spec = info.udp_port_lidar, info.udp_port_imu - guesses = [(plidar, pimu) for plidar, pimu in ports - if (plidar == lidar_spec or lidar_spec == 0 or plidar == 0) - and (pimu == imu_spec or imu_spec == 0 or pimu == 0)] - # yapf: enable - - # sort sensor ports to prefer both found > just lidar > just imu +def _guess_ports(stream_info, sensor_info): + pf = _client.PacketFormat.from_info(sensor_info) + lidar_spec, imu_spec = sensor_info.udp_port_lidar, sensor_info.udp_port_imu + guesses = [(i.lidar, i.imu) for i in _pcap.guess_ports( + stream_info, pf.lidar_packet_size, pf.imu_packet_size, + lidar_spec, imu_spec)] guesses.sort(reverse=True, key=lambda p: (p[0] != 0, p[1] != 0, p)) - return guesses -def _packet_info_stream(path: str) -> Iterator[_pcap.packet_info]: - """Read just packet headers without payloads.""" - handle = _pcap.replay_initialize(path) - - try: - while True: - pi = _pcap.packet_info() - if not _pcap.next_packet_info(handle, pi): - break - yield pi - finally: - _pcap.replay_uninitialize(handle) +def _packet_info_stream(path: str, n_packets, progress_callback=None, callback_frequency=1): + if progress_callback is not None: + result = _pcap.get_stream_info(path, progress_callback, callback_frequency, n_packets) + else: + result = _pcap.get_stream_info(path, n_packets) + return result class Pcap(PacketSource): @@ -142,7 +48,8 @@ def __init__(self, *, rate: float = 0.0, lidar_port: Optional[int] = None, - imu_port: Optional[int] = None): + imu_port: Optional[int] = None, + _soft_id_check: bool = False): """Read a single sensor data stream from a packet capture. Packet captures can contain arbitrary network traffic or even multiple @@ -168,6 +75,8 @@ def __init__(self, rate: Output packets in real time, if non-zero lidar_port: Specify the destination port of lidar packets imu_port: Specify the destination port of imu packets + _soft_id_check: if True, don't skip lidar packets buffers on + init_id/sn mismatch """ # prefer explicitly specified ports (can probably remove the args?) @@ -179,10 +88,13 @@ def __init__(self, self._metadata.udp_port_lidar = lidar_port self._metadata.udp_port_imu = imu_port + self._soft_id_check = _soft_id_check + self._id_error_count = 0 + # sample pcap and attempt to find UDP ports consistent with metadata n_packets = 1000 - stats = _stream_info(islice(_packet_info_stream(pcap_path), n_packets)) - self._guesses = _guess_ports(stats.udp_streams, self._metadata) + stats = _packet_info_stream(pcap_path, n_packets) + self._guesses = _guess_ports(stats, self._metadata) # fill in unspecified (0) ports with inferred values if len(self._guesses) > 0: @@ -225,15 +137,24 @@ def __iter__(self) -> Iterator[Packet]: try: if (packet_info.dst_port == self._metadata.udp_port_lidar): - yield LidarPacket(buf[0:n], self._metadata, timestamp) + lp = LidarPacket( + buf[0:n], + self._metadata, + timestamp, + _raise_on_id_check=not self._soft_id_check) + if lp.id_error: + self._id_error_count += 1 + yield lp elif (packet_info.dst_port == self._metadata.udp_port_imu): yield ImuPacket(buf[0:n], self._metadata, timestamp) + except PacketIdError: + self._id_error_count += 1 except ValueError: - # bad packet size or init_id here: this can happen when + # bad packet size: this can happen when # packets are buffered by the OS, not necessarily an error # same pass as in core.py # TODO: introduce status for PacketSource to indicate frequency - # of bad packet size or init_id errors + # of bad packet size or init_id/sn errors pass @property @@ -260,6 +181,10 @@ def closed(self) -> bool: with self._lock: return self._handle is None + @property + def id_error_count(self) -> int: + return self._id_error_count + def close(self) -> None: """Release resources. Thread-safe.""" with self._lock: @@ -330,9 +255,8 @@ def record(packets: Iterable[Packet], """ has_timestamp = None error = False - buf_size = 2**16 n = 0 - handle = _pcap.record_initialize(pcap_path, src_ip, dst_ip, buf_size, + handle = _pcap.record_initialize(pcap_path, MTU_SIZE, use_sll_encapsulation) try: for packet in packets: @@ -351,7 +275,7 @@ def record(packets: Iterable[Packet], raise ValueError("Mixing timestamped/untimestamped packets") ts = packet.capture_timestamp or time.time() - _pcap.record_packet(handle, src_port, dst_port, packet._data, ts) + _pcap.record_packet(handle, src_ip, dst_ip, src_port, dst_port, packet._data, ts) n += 1 except Exception: error = True diff --git a/python/src/ouster/sdk/_viz.pyi b/python/src/ouster/sdk/_viz.pyi index 20bd786d..08e0a3c7 100644 --- a/python/src/ouster/sdk/_viz.pyi +++ b/python/src/ouster/sdk/_viz.pyi @@ -5,7 +5,7 @@ All rights reserved. Type annotations for viz python bindings. """ -from typing import Callable, overload, Tuple, Callable, List +from typing import Callable, overload, Tuple, List import numpy as np @@ -13,7 +13,9 @@ from ..client import SensorInfo calref_palette: np.ndarray spezia_palette: np.ndarray - +grey_palette: np.ndarray +viridis_palette: np.ndarray +magma_palette: np.ndarray class WindowCtx: @@ -41,6 +43,14 @@ class WindowCtx: def viewport_height(self) -> int: ... + @property + def window_width(self) -> int: + ... + + @property + def window_height(self) -> int: + ... + class Camera: @@ -136,6 +146,9 @@ class Cloud: def set_key(self, key: np.ndarray) -> None: ... + def set_key_alpha(self, rgb: np.ndarray) -> None: + ... + def set_mask(self, mask: np.ndarray) -> None: ... @@ -180,6 +193,12 @@ class Image: def set_hshift(self, hshift: float) -> None: ... + def set_palette(self, palette: np.ndarray) -> None: + ... + + def clear_palette(self) -> None: + ... + class Cuboid: @@ -255,6 +274,9 @@ class PointViz: def update(self) -> bool: ... + def visible(self, state: bool) -> bool: + ... + def push_key_handler(self, f: Callable[[WindowCtx, int, int], bool]) -> None: ... diff --git a/python/src/ouster/sdk/convert_to_legacy.py b/python/src/ouster/sdk/convert_to_legacy.py new file mode 100644 index 00000000..e4e58bee --- /dev/null +++ b/python/src/ouster/sdk/convert_to_legacy.py @@ -0,0 +1,29 @@ +""" +Copyright (c) 2023, Ouster, Inc. +All rights reserved. +""" + +import argparse + +from ouster import client + + +def main() -> None: + descr = """Convert non-legacy metadata to legacy format""" + + parser = argparse.ArgumentParser(description=descr) + parser.add_argument('metadata_path') + parser.add_argument('-o', '--output-path') + + args = parser.parse_args() + + with open(args.metadata_path) as json: + print(f"Reading metadata from: {args.metadata_path}") + legacy = client.convert_to_legacy(json.read()) + + if args.output_path: + with open(args.output_path, "w") as output: + print(f"Writing converted legacy metadata to: {args.output_path}") + output.write(legacy) + else: + print(legacy) diff --git a/python/src/ouster/sdk/simple_viz.py b/python/src/ouster/sdk/simple_viz.py index 28ac5a3b..030d37a8 100644 --- a/python/src/ouster/sdk/simple_viz.py +++ b/python/src/ouster/sdk/simple_viz.py @@ -4,11 +4,11 @@ """ import argparse -import os import numpy as np from ouster import client from .viz import SimpleViz +from .util import resolve_metadata def main() -> None: @@ -34,7 +34,8 @@ def main() -> None: type=float, required=False, nargs=16, - help='lidar sensor extrinsics to use in viz') + help='lidar sensor extrinsics in homogenous matrix given' + 'in row-major order to use in 3D viz') args = parser.parse_args() @@ -59,14 +60,13 @@ def main() -> None: elif args.pcap: import ouster.pcap as pcap - if args.meta: - metadata_path = args.meta - else: - print("Deducing metadata based on pcap name. " - "To provide a different metadata path, use --meta") - metadata_path = os.path.splitext(args.pcap)[0] + ".json" - + metadata_path = resolve_metadata(args.pcap, args.meta) + if not metadata_path: + print("Metadata file not found, please specify " + "a valid metadata file with `--meta`") + return with open(metadata_path) as json: + print(f"Reading metadata from: {metadata_path}") info = client.SensorInfo(json.read()) scans = client.Scans(pcap.Pcap(args.pcap, info)) rate = 1.0 diff --git a/python/src/ouster/sdk/util.py b/python/src/ouster/sdk/util.py new file mode 100644 index 00000000..74bdc409 --- /dev/null +++ b/python/src/ouster/sdk/util.py @@ -0,0 +1,80 @@ +"""Miscellaneous utilites.""" + +import os +from typing import Optional, List + + +def resolve_metadata_multi_with_prefix_guess(data_path: str) -> List[str]: + """Look for best-matching metadata files from all json files in the same dir + + Args: + data_path: filename location with the data, usually .pcap or .bag + + Returns: + list of metadata json paths guessed with the most common prefix match + """ + dirname, pcap_ = os.path.split(data_path) + if not dirname: + dirname = os.getcwd() + # find all .json in same dir + options = list(filter(lambda f: f.endswith(".json"), os.listdir(dirname))) + # for each json name, find how many characters are in common + option_scores = map(lambda f: len(os.path.commonprefix([f, pcap_])), + options) + if not options: + return [] + # select all jsons with the longest common prefix of equal size + sorted_options = sorted(zip(options, option_scores), + key=lambda i: i[1], + reverse=True) + best_score = sorted_options[0][1] + if not best_score: + # return a single json if there is no files with commonprefix + # because it's probably not a multi-sensor recording + return [os.path.join(dirname, sorted_options[0][0])] + else: + return [ + os.path.join(dirname, b_path) for b_path, _ in filter( + lambda i: i[1] == best_score, sorted_options) + ] + + +def resolve_metadata(data_path: str, + meta_path: Optional[str] = None) -> Optional[str]: + """Look for a metadata file based on the data path if needed. + + Convenient to use in CLI tools when --meta param can be ommited + in lots of trivial cases when pcap filename has the same prefix + as the metadata json filename. + + Args: + data_path: filename location with the data, usually .pcap or .bag + that is used to search metadata with the most common + prefix file + meta_path: the pass through metadata path, if set guessing and + search for other metadata jsons is skipped + + Returns: + metadata json paths guessed with the most common prefix match or passed + through from `meta_path` parameter + """ + if meta_path is None: + meta_paths = resolve_metadata_multi_with_prefix_guess(data_path) + meta_path = meta_paths[0] if meta_paths else "" + if os.path.exists(meta_path): + return meta_path + elif os.path.exists(meta_path): + return meta_path + return None + + +def resolve_metadata_multi(data_path: str) -> List[str]: + """Look for a metadata files based on the pcap path with multi sensors. + + Args: + data_path: filename location with the data, usually .pcap or .bag + + Returns: + list of metadata json paths guessed with the most common prefix match + """ + return resolve_metadata_multi_with_prefix_guess(data_path) diff --git a/python/src/ouster/sdk/viz.py b/python/src/ouster/sdk/viz.py index a98235ab..079afc09 100644 --- a/python/src/ouster/sdk/viz.py +++ b/python/src/ouster/sdk/viz.py @@ -7,14 +7,16 @@ Visualize lidar data using OpenGL. """ -from collections import (defaultdict, deque) +from collections import (deque) +from dataclasses import dataclass from functools import partial import os import threading import time from datetime import datetime from typing import (Callable, ClassVar, Deque, Dict, Generic, Iterable, List, - Optional, Tuple, TypeVar, Union) + Optional, Tuple, TypeVar, Union, Any) +from typing_extensions import Protocol, runtime_checkable import weakref import logging @@ -22,10 +24,11 @@ from PIL import Image as PILImage from ouster import client -from ouster.client import _utils +from ouster.client import _utils, ChanField +from ..client._client import Version from ._viz import (PointViz, Cloud, Image, Cuboid, Label, WindowCtx, Camera, TargetDisplay, add_default_controls, calref_palette, - spezia_palette) + spezia_palette, grey_palette, viridis_palette, magma_palette) logger = logging.getLogger("viz-logger") @@ -86,54 +89,237 @@ def handle_fb_data(fb_data: List, fb_width: int, fb_height: int) -> bool: viz.push_frame_buffer_handler(handle_fb_data) -class LidarScanViz: - """Visualize LidarScan data. +@runtime_checkable +class FieldViewMode(Protocol): + """LidarScan field processor - Uses the supplied PointViz instance to easily display the contents of a - LidarScan. Sets up key bindings to toggle which channel fields and returns - are displayed, and change 2D image and point size. + View modes define the process of getting the key data for + the scan and return number as well as checks the possibility + of showing data in that mode, see `enabled()`. """ - @staticmethod - def _reflectivity_pp( - info: client.SensorInfo) -> Callable[[np.ndarray], None]: + @property + def name(self) -> str: + """Name of the view mode""" + ... + + @property + def names(self) -> List[str]: + """Name of the view mode per return number""" + ... + + def _prepare_data(self, + ls: client.LidarScan, + return_num: int = 0) -> Optional[np.ndarray]: + """Prepares data for visualization given the scan and return number""" + ... + + def enabled(self, ls: client.LidarScan, return_num: int = 0) -> bool: + """Checks the view mode availability for a scan and return number""" + ... + + +@runtime_checkable +class ImageMode(FieldViewMode, Protocol): + """Applies the view mode key to the viz.Image""" + def set_image(self, + img: Image, + ls: client.LidarScan, + return_num: int = 0) -> None: + """Prepares the key data and sets the image key to it.""" + ... + + +@runtime_checkable +class CloudMode(FieldViewMode, Protocol): + """Applies the view mode key to the viz.Cloud""" + def set_cloud_color(self, + cloud: Cloud, + ls: client.LidarScan, + return_num: int = 0) -> None: + """Prepares the key data and sets the cloud key to it.""" + ... + + +class ImageCloudMode(ImageMode, CloudMode, Protocol): + """Applies the view mode to viz.Cloud and viz.Image""" + pass + + +def _second_chan_field(field: client.ChanField) -> Optional[client.ChanField]: + """Get the second return field name.""" + # yapf: disable + second_fields = dict({ + client.ChanField.RANGE: client.ChanField.RANGE2, + client.ChanField.SIGNAL: client.ChanField.SIGNAL2, + client.ChanField.REFLECTIVITY: client.ChanField.REFLECTIVITY2, + client.ChanField.FLAGS: client.ChanField.FLAGS2 + }) + # yapf: enable + return second_fields.get(field, None) + + +class SimpleMode(ImageCloudMode): + """Basic view mode with AutoExposure and BeamUniformityCorrector + + Handles single and dual returns scans. + + When AutoExposure is enabled its state updates only for return_num=0 but + applies for both returns. + """ + def __init__(self, + info: client.SensorInfo, + field: client.ChanField, + *, + prefix: Optional[str] = "", + suffix: Optional[str] = "", + use_ae: bool = True, + use_buc: bool = False) -> None: + """ + Args: + info: sensor metadata used mainly for destaggering here + field: ChanField to process, second return is handled automatically + prefix: name prefix + suffix: name suffix + use_ae: if True, use AutoExposure for the field + use_buc: if True, use BeamUniformityCorrector for the field + """ + self._info = info + self._fields = [field] + field2 = _second_chan_field(field) + if field2: + self._fields.append(field2) + self._ae = _utils.AutoExposure() if use_ae else None + self._buc = _utils.BeamUniformityCorrector() if use_buc else None + self._prefix = f"{prefix}: " if prefix else "" + self._suffix = f" ({suffix})" if suffix else "" + self._wrap_name = lambda n: f"{self._prefix}{n}{self._suffix}" + + @property + def name(self) -> str: + return self._wrap_name(str(self._fields[0])) + + @property + def names(self) -> List[str]: + return [self._wrap_name(str(f)) for f in self._fields] - if client._client.Version.from_string(info.fw_rev) >= client._client.Version.from_string("v2.1.0"): + def _prepare_data(self, + ls: client.LidarScan, + return_num: int = 0) -> Optional[np.ndarray]: + if not self.enabled(ls, return_num): + return None - def proc_cal(refl, update_state: bool = True) -> None: - refl /= 255.0 + f = self._fields[return_num] + key_data = ls.field(f).astype(np.float32) - return proc_cal + if self._buc: + self._buc(key_data) + + if self._ae: + self._ae(key_data, update_state=(return_num == 0)) + else: + key_max = np.max(key_data) + if key_max: + key_data = key_data / key_max + + return key_data + + def set_image(self, + img: Image, + ls: client.LidarScan, + return_num: int = 0) -> None: + key_data = self._prepare_data(ls, return_num) + if key_data is not None: + img.set_image(client.destagger(self._info, key_data)) + + def set_cloud_color(self, + cloud: Cloud, + ls: client.LidarScan, + return_num: int = 0) -> None: + key_data = self._prepare_data(ls, return_num) + if key_data is not None: + cloud.set_key(key_data) + + def enabled(self, ls: client.LidarScan, return_num: int = 0): + return (self._fields[return_num] in ls.fields + if return_num < len(self._fields) else False) + + +class ReflMode(SimpleMode, ImageCloudMode): + """Prepares image/cloud data for REFLECTIVITY channel""" + + def __init__(self, info: client.SensorInfo) -> None: + super().__init__(info, client.ChanField.REFLECTIVITY, use_ae=True) + # used only for uncalibrated reflectivity in FW prior v2.1.0 + # TODO: should we check for calibrated reflectivity status from + # metadata too? + self._normalized_refl = (Version.from_string(self._info.fw_rev) >= + Version.from_string("v2.1.0")) + + def _prepare_data(self, + ls: client.LidarScan, + return_num: int = 0) -> Optional[np.ndarray]: + if not self.enabled(ls, return_num): + return None + + f = self._fields[return_num] + refl_data = ls.field(f).astype(np.float32) + if self._normalized_refl: + refl_data /= 255.0 else: - return _utils.AutoExposure() + # mypy doesn't recognize that we always should have _ae here + # so we have explicit check + if self._ae: + self._ae(refl_data, update_state=(return_num == 0)) + return refl_data - @staticmethod - def _near_ir_pp(info: client.SensorInfo) -> Callable[[np.ndarray], None]: - buc = _utils.BeamUniformityCorrector() - ae = _utils.AutoExposure() - def proc(ambient) -> None: - destag = client.destagger(info, ambient) - buc(destag) - ambient[:] = client.destagger(info, destag, inverse=True) - ae(ambient) +def is_norm_reflectivity_mode(mode: FieldViewMode) -> bool: + """Checks whether the image/cloud mode is a normalized REFLECTIVITY mode - return proc + NOTE[pb]: This is highly implementation specific and doesn't look nicely, + i.e. it's more like duck/duct plumbing .... but suits the need. + """ + return (hasattr(mode, "_normalized_refl") and mode._normalized_refl) - _cloud_mode_channels: ClassVar[List[Tuple[client.ChanField, client.ChanField]]] = [ - (client.ChanField.RANGE, client.ChanField.RANGE2), - (client.ChanField.SIGNAL, client.ChanField.SIGNAL2), - (client.ChanField.REFLECTIVITY, client.ChanField.REFLECTIVITY2), - (client.ChanField.NEAR_IR, client.ChanField.NEAR_IR), - ] - _available_fields: List[client.ChanField] - _cloud_palette: Optional[np.ndarray] - _field_pp: Dict[client.ChanField, Callable[[np.ndarray], None]] +@dataclass +class ImgModeItem: + """Image mode for specific return with explicit name.""" + mode: ImageMode + name: str + return_num: int = 0 - def __init__(self, - meta: client.SensorInfo, - viz: Optional[PointViz] = None) -> None: + +@dataclass +class CloudPaletteItem: + """Palette with a name""" + name: str + palette: np.ndarray + + +LidarScanVizMode = Union[ImageCloudMode, ImageMode, CloudMode] +"""Field view mode types""" + + +class LidarScanViz: + """Visualize LidarScan data. + + Uses the supplied PointViz instance to easily display the contents of a + LidarScan. Sets up key bindings to toggle which channel fields and returns + are displayed, and change 2D image and point size. + """ + + _cloud_palette: Optional[CloudPaletteItem] + + def __init__( + self, + meta: client.SensorInfo, + viz: Optional[PointViz] = None, + *, + _img_aspect_ratio: float = 0, + _ext_modes: Optional[List[LidarScanVizMode]] = None, + _ext_palettes: Optional[List[CloudPaletteItem]] = None) -> None: """ Args: meta: sensor metadata used to interpret scans @@ -147,35 +333,54 @@ def __init__(self, self._cloud_mode_ind = 0 # index into _cloud_mode_channels self._cloud_enabled = [True, True] self._cloud_pt_size = 2.0 - self._cloud_palette = spezia_palette + + self._cloud_refl_mode = False + + self._cloud_palettes: List[CloudPaletteItem] + self._cloud_palettes = [ + CloudPaletteItem("Ouster Colors", spezia_palette), + CloudPaletteItem("Greyscale", grey_palette), + CloudPaletteItem("Viridis", viridis_palette), + CloudPaletteItem("Magma", magma_palette), + ] + self._cloud_palettes.extend(_ext_palettes or []) + + self._cloud_palette_ind = 0 + self._cloud_palette = self._cloud_palettes[self._cloud_palette_ind] + self._cloud_palette_name = self._cloud_palette.name # image display state self._img_ind = [0, 1] # index of field to display + self._img_refl_mode = [False, False] self._img_size_fraction = 6 - self._img_aspect = (meta.beam_altitude_angles[0] - - meta.beam_altitude_angles[-1]) / 360.0 + self._img_aspect = _img_aspect_ratio or ( + max(meta.beam_altitude_angles) - + min(meta.beam_altitude_angles)) / 360.0 # misc display state - self._available_fields = [] self._ring_size = 1 self._ring_line_width = 1 self._osd_enabled = True - # set up post-processing for each channel field - range_pp = _utils.AutoExposure() - signal_pp = _utils.AutoExposure() - refl_pp = LidarScanViz._reflectivity_pp(meta) - nearir_pp = LidarScanViz._near_ir_pp(meta) - - self._field_pp = { - client.ChanField.RANGE: range_pp, - client.ChanField.RANGE2: partial(range_pp, update_state=False), - client.ChanField.SIGNAL: signal_pp, - client.ChanField.SIGNAL2: partial(signal_pp, update_state=False), - client.ChanField.REFLECTIVITY: refl_pp, - client.ChanField.REFLECTIVITY2: partial(refl_pp, update_state=False), - client.ChanField.NEAR_IR: nearir_pp, - } + self._modes: List[LidarScanVizMode] + self._modes = [ + SimpleMode(meta, ChanField.NEAR_IR, use_ae=True, use_buc=True), + ReflMode(meta), + SimpleMode(meta, ChanField.SIGNAL), + SimpleMode(meta, ChanField.RANGE), + ] + + self._modes.extend(_ext_modes or []) + + self._image_modes: List[ImgModeItem] + self._image_modes = [ + ImgModeItem(mode, name, num) for mode in self._modes + if isinstance(mode, ImageMode) + for num, name in enumerate(mode.names) + ] + + self._cloud_modes: List[CloudMode] + self._cloud_modes = [m for m in self._modes if isinstance(m, CloudMode)] self._viz = viz or PointViz("Ouster Viz") @@ -209,6 +414,7 @@ def __init__(self, (ord('B'), 0): partial(LidarScanViz.cycle_img_mode, i=0), (ord('N'), 0): partial(LidarScanViz.cycle_img_mode, i=1), (ord('M'), 0): LidarScanViz.cycle_cloud_mode, + (ord('F'), 0): LidarScanViz.cycle_cloud_palette, (ord("'"), 0): partial(LidarScanViz.update_ring_size, amount=1), (ord("'"), 1): partial(LidarScanViz.update_ring_size, amount=-1), (ord("'"), 2): LidarScanViz.cicle_ring_line_width, @@ -228,19 +434,19 @@ def handle_keys(self: LidarScanViz, ctx: WindowCtx, key: int, def cycle_img_mode(self, i: int) -> None: """Change the displayed field of the i'th image.""" with self._lock: - nfields = len(self._available_fields) - if nfields > 0: - self._img_ind[i] = (self._img_ind[i] + 1) % nfields + self._img_ind[i] += 1 def cycle_cloud_mode(self) -> None: - """Change the channel field used to color the 3D point cloud.""" + """Change the coloring mode of the 3D point cloud.""" with self._lock: - nfields = len(LidarScanViz._cloud_mode_channels) - self._cloud_mode_ind = (self._cloud_mode_ind + 1) % nfields - new_fields = LidarScanViz._cloud_mode_channels[ - self._cloud_mode_ind] - self._cloud_palette = (calref_palette if client.ChanField.REFLECTIVITY - in new_fields else spezia_palette) + self._cloud_mode_ind = (self._cloud_mode_ind + 1) + + def cycle_cloud_palette(self) -> None: + """Change the color palette of the 3D point cloud.""" + with self._lock: + self._cloud_palette_ind = (self._cloud_palette_ind + 1) % len( + self._cloud_palettes) + self._cloud_palette = self._cloud_palettes[self._cloud_palette_ind] def toggle_cloud(self, i: int) -> None: """Toggle whether the i'th return is displayed.""" @@ -328,60 +534,85 @@ def _draw(self) -> None: # figure out what to draw based on current viz state scan = self._scan - self._available_fields = list(scan.fields) - image_fields = tuple(self._available_fields[i] for i in self._img_ind) - cloud_fields = LidarScanViz._cloud_mode_channels[self._cloud_mode_ind] - - # extract field data and apply post-processing - field_data: Dict[client.ChanField, np.ndarray] - field_data = defaultdict(lambda: np.zeros( - (scan.h, scan.w), dtype=np.float32)) - - for field in {*image_fields, *cloud_fields}: - if field in scan.fields: - field_data[field] = scan.field(field).astype(np.float32) - for field, data in field_data.items(): - if field in self._field_pp: - self._field_pp[field](data) + # available display modes + img_modes = list( + filter(lambda m: m.mode.enabled(scan, m.return_num), + self._image_modes)) + cloud_modes = list(filter(lambda m: m.enabled(scan), + self._cloud_modes)) # update 3d display - palette = self._cloud_palette - self._cloud_palette = None + self._cloud_mode_ind %= len(cloud_modes) + cloud_mode = cloud_modes[self._cloud_mode_ind] + + refl_mode = is_norm_reflectivity_mode(cloud_mode) + if refl_mode: + self._cloud_palette = (CloudPaletteItem("Cal. Ref", calref_palette) + if not self._cloud_refl_mode else None) + else: + if self._cloud_refl_mode: + self._cloud_palette = self._cloud_palettes[ + self._cloud_palette_ind] + self._cloud_refl_mode = refl_mode - for i, range_field in ((0, client.ChanField.RANGE), (1, client.ChanField.RANGE2)): + for i, range_field in ((0, ChanField.RANGE), (1, ChanField.RANGE2)): if range_field in scan.fields: range_data = scan.field(range_field) else: range_data = np.zeros((scan.h, scan.w), dtype=np.uint32) self._clouds[i].set_range(range_data) - self._clouds[i].set_key(field_data[cloud_fields[i]].astype( - np.float32)) - if palette is not None: - self._clouds[i].set_palette(palette) + + if self._cloud_palette is not None: + self._clouds[i].set_palette(self._cloud_palette.palette) + + if cloud_mode.enabled(scan, i): + cloud_modes[self._cloud_mode_ind].set_cloud_color( + self._clouds[i], scan, return_num=i) + else: + cloud_modes[self._cloud_mode_ind].set_cloud_color( + self._clouds[i], scan, return_num=0) + + if self._cloud_palette is not None: + self._cloud_palette_name = self._cloud_palette.name + + # palette is set only on the first _draw when it's changed + self._cloud_palette = None # update 2d images for i in (0, 1): - image_data = client.destagger( - self._metadata, field_data[image_fields[i]].astype(np.float32)) - self._images[i].set_image(image_data) + self._img_ind[i] %= len(img_modes) + img_mode_item = img_modes[self._img_ind[i]] + img_mode = img_mode_item.mode + + refl_mode = is_norm_reflectivity_mode(img_mode) + if refl_mode and not self._img_refl_mode[i]: + self._images[i].set_palette(calref_palette) + if not refl_mode and self._img_refl_mode[i]: + self._images[i].clear_palette() + self._img_refl_mode[i] = refl_mode + + img_mode.set_image(self._images[i], scan, img_mode_item.return_num) # update osd meta = self._metadata enable_ind = [i + 1 for i, b in enumerate(self._cloud_enabled) if b] - first_ts = scan.timestamp[np.nonzero(scan.timestamp)][0] + + nonzeros = np.flatnonzero(scan.timestamp) + first_ts = scan.timestamp[nonzeros[0]] if len(nonzeros) > 0 else 0 + if self._osd_enabled: self._osd.set_text( - f"image: {image_fields[0]}/{image_fields[1]}\n" - f"cloud{enable_ind}: {cloud_fields[0]}\n" + f"image: {img_modes[self._img_ind[0]].name}/{img_modes[self._img_ind[1]].name}\n" + f"cloud{enable_ind}: {cloud_modes[self._cloud_mode_ind].name}\n" + f"palette: {self._cloud_palette_name}\n" f"frame: {scan.frame_id}\n" f"sensor ts: {first_ts / 1e9:.3f}s\n" f"profile: {str(meta.format.udp_profile_lidar)}\n" f"{meta.prod_line} {meta.fw_rev} {meta.mode}\n" f"shot limiting status: {str(scan.shot_limiting())}\n" - f"thermal shutdown status: {str(scan.thermal_shutdown())}" - ) + f"thermal shutdown status: {str(scan.thermal_shutdown())}") else: self._osd.set_text("") @@ -478,6 +709,13 @@ def _save_fb_to_png(fb_data: List, return img_fname +# TODO: Make/Define a better ScanViz interface +# not a best way to describe interface, yeah duck typing danger, etc ... +# but ScanViz object shoud have a write property 'scan' and underlying +# Point viz member at '_viz' +AnyScanViz = Union[LidarScanViz, Any] + + class SimpleViz: """Visualize a stream of LidarScans. @@ -487,8 +725,9 @@ class SimpleViz: _playback_rates = (0.25, 0.5, 0.75, 1.0, 1.5, 2.0, 3.0, 0.0) def __init__(self, - arg: Union[client.SensorInfo, LidarScanViz], + arg: Union[client.SensorInfo, AnyScanViz], rate: Optional[float] = None, + pause_at: int = -1, _buflen: int = 50) -> None: """ Args: @@ -496,6 +735,8 @@ def __init__(self, LidarScanViz instance to use. rate: Playback rate. One of 0.25, 0.5, 0.75, 1.0, 1.5, 2.0, 3.0 or None for "live" playback (the default). + pause_at: scan number to pause at, dafault (-1) - no auto pause, to + stop after the very first scan use 0 Raises: ValueError: if the specified rate isn't one of the options @@ -509,12 +750,16 @@ def __init__(self, self._viz = arg._viz self._scan_viz = arg else: - raise TypeError(f"Bad type for 1st constructor arg: {type(arg)}") + # we continue, so custom ScanVizs can be used with the same + # SimpleViz class and basic controls + self._viz = arg._viz + self._scan_viz = arg self._lock = threading.Lock() self._live = (rate is None) self._rate_ind = SimpleViz._playback_rates.index(rate or 0.0) self._buflen = _buflen + self._pause_at = pause_at # pausing and stepping self._cv = threading.Condition() @@ -608,7 +853,7 @@ def toggle_img_recording(self) -> None: if self._viz_img_recording: self._viz_img_recording = False self._viz.pop_frame_buffer_handler() - print("Key SHIFT-X: Img Recording STOPED") + print("Key SHIFT-X: Img Recording STOPPED") else: self._viz_img_recording = True @@ -637,13 +882,19 @@ def handle_fb_once(viz: PointViz, fb_data: List, fb_width: int, def _frame_period(self) -> float: rate = SimpleViz._playback_rates[self._rate_ind] if rate and not self._paused: - return 1.0 / (self._metadata.mode.frequency * rate) + if isinstance(self._scan_viz, LidarScanViz): + return 1.0 / (self._metadata.format.fps * rate) + else: + # if some other scan viz that is not derived from LidarScanViz + # we default to 10 Hz + return 1.0 / (10 * rate) else: return 0.0 def _process(self, seekable: _Seekable[client.LidarScan]) -> None: last_ts = time.monotonic() + scan_idx = -1 try: while True: # wait until unpaused, step, or quit @@ -660,12 +911,19 @@ def _process(self, seekable: _Seekable[client.LidarScan]) -> None: period = self._frame_period() # process new data + scan_idx = seekable.next_ind self._scan_viz.scan = next(seekable) self._scan_viz.draw(update=False) + if self._pause_at == scan_idx: + self._paused = True + self._update_playback_osd() + # sleep for remainder of scan period to_sleep = max(0.0, period - (time.monotonic() - last_ts)) - time.sleep(to_sleep) + if scan_idx > 0: + time.sleep(to_sleep) + last_ts = time.monotonic() # show new data @@ -675,7 +933,11 @@ def _process(self, seekable: _Seekable[client.LidarScan]) -> None: pass finally: - # signal rendering (main) thread to exit + # signal rendering (main) thread to exit, with a delay + # because the viz in main thread may not have been started + # and on Mac it was observed that it fails to set a flag if + # _process fails immediately after start + time.sleep(0.5) self._viz.running(False) def run(self, scans: Iterable[client.LidarScan]) -> None: @@ -706,8 +968,12 @@ def run(self, scans: Iterable[client.LidarScan]) -> None: except KeyboardInterrupt: pass finally: - # some scan sources may be waiting on IO, blocking the processing thread - seekable.close() + try: + # some scan sources may be waiting on IO, blocking the + # processing thread + seekable.close() + except Exception as e: + logger.warn(f"Data source closed with error: '{e}'") # processing thread will still be running if e.g. viz window was closed with self._cv: @@ -720,5 +986,7 @@ def run(self, scans: Iterable[client.LidarScan]) -> None: __all__ = [ 'PointViz', 'Cloud', 'Image', 'Cuboid', 'Label', 'WindowCtx', 'Camera', - 'TargetDisplay', 'add_default_controls', 'calref_palette', 'spezia_palette' + 'TargetDisplay', 'add_default_controls', 'calref_palette', 'spezia_palette', + 'grey_palette', 'viridis_palette', 'magma_palette', 'ImageMode', + 'CloudMode', 'ImageCloudMode', 'CloudPaletteItem' ] diff --git a/python/tests/__init__.py b/python/tests/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/tests/test_core.py b/python/tests/test_core.py index 1df3d263..3a3cba2e 100644 --- a/python/tests/test_core.py +++ b/python/tests/test_core.py @@ -10,7 +10,7 @@ import pytest from ouster import client -from ouster.client import ColHeader, ChanField +from ouster.client import ChanField pytest.register_assert_rewrite('ouster.client._digest') import ouster.client._digest as digest # noqa @@ -120,7 +120,6 @@ def test_scans_meta(packets: client.PacketSource) -> None: assert len(scan.timestamp) == scan.w assert len(scan.measurement_id) == scan.w assert len(scan.status) == scan.w - assert len(scan.header(ColHeader.ENCODER_COUNT)) == scan.w assert scan.complete() @@ -131,14 +130,9 @@ def test_scans_meta(packets: client.PacketSource) -> None: client.UDPProfileLidar.PROFILE_LIDAR_LEGACY): # check that all columns are valid assert (scan.status == 0xffffffff).all() - # only first encoder count is zero - assert np.count_nonzero(scan.header( - ColHeader.ENCODER_COUNT)) == scan.w - 1 else: # only lowest bit indicates valid assert (scan.status & 0x1).all() - # encoder counts zeroed - assert (scan.header(ColHeader.ENCODER_COUNT) == 0).all() def test_scans_first_packet(packet: client.LidarPacket, @@ -174,9 +168,6 @@ def test_scans_first_packet(packet: client.LidarPacket, assert np.array_equal(packet.timestamp, scan.timestamp[:w]) - assert np.array_equal(packet.header(ColHeader.ENCODER_COUNT), - scan.header(ColHeader.ENCODER_COUNT)[:w]) - assert np.array_equal(packet.status, scan.status[:w]) diff --git a/python/tests/test_data.py b/python/tests/test_data.py index 19587965..b6fbba0f 100644 --- a/python/tests/test_data.py +++ b/python/tests/test_data.py @@ -63,7 +63,7 @@ def test_lidar_packet(meta: client.SensorInfo) -> None: client.UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8) assert len( - client.ChanField.__members__) == 24, "Don't forget to update tests!" + client.ChanField.__members__) == 29, "Don't forget to update tests!" assert np.array_equal(p.field(client.ChanField.RANGE), np.zeros((h, w))) assert np.array_equal(p.field(client.ChanField.REFLECTIVITY), np.zeros((h, w))) @@ -170,15 +170,9 @@ def test_scan_writeable() -> None: ls.status[-1] = 0xffff assert ls.status[-1] == 0xffff - assert ls.header(client.ColHeader.STATUS)[-1] == 0xffff - - ls.header(client.ColHeader.STATUS)[-2] = 0xffff - assert ls.status[-2] == 0xffff - assert ls.header(client.ColHeader.STATUS)[-2] == 0xffff ls.status[:] = 0x1 assert np.all(ls.status == 0x1) - assert np.all(ls.header(client.ColHeader.STATUS) == 0x1) def test_scan_from_native() -> None: diff --git a/python/tests/test_metadata.py b/python/tests/test_metadata.py index 09d88e71..3574bb60 100644 --- a/python/tests/test_metadata.py +++ b/python/tests/test_metadata.py @@ -115,6 +115,7 @@ def test_write_info(meta: client.SensorInfo) -> None: meta.format.pixel_shift_by_row = [] meta.format.udp_profile_lidar = client.UDPProfileLidar(0) meta.format.udp_profile_imu = client.UDPProfileIMU(0) + meta.format.fps = 0 meta.beam_azimuth_angles = [] meta.beam_altitude_angles = [] meta.imu_to_sensor_transform = numpy.zeros((4, 4)) @@ -212,4 +213,4 @@ def test_equality_format() -> None: data_format_attributes = inspect.getmembers(client.DataFormat, lambda a: not inspect.isroutine(a)) data_format_properties = [a for a in data_format_attributes if not (a[0].startswith('__') and a[0].endswith('__'))] - assert len(data_format_properties) == 7, "Don't forget to update tests and the data_format == operator!" + assert len(data_format_properties) == 8, "Don't forget to update tests and the data_format == operator!" diff --git a/python/tests/test_pcap.py b/python/tests/test_pcap.py index d31d6871..debd34fc 100644 --- a/python/tests/test_pcap.py +++ b/python/tests/test_pcap.py @@ -7,7 +7,7 @@ from copy import copy from os import path from random import getrandbits, shuffle, random -from typing import (Dict, Iterable, Iterator, List) +from typing import (Dict, Iterable, Iterator, List, Callable) from itertools import chain import pytest @@ -17,6 +17,8 @@ from ouster.pcap import _pcap from ouster import client from ouster.client import _client +from tests.conftest import DATA_DIR, TESTS +from tests.test_batching import _patch_frame_id SLL_PROTO = 113 ETH_PROTO = 1 @@ -46,6 +48,45 @@ def fake_packets(metadata: client.SensorInfo, yield client.ImuPacket(buf, metadata, packet_ts) +def set_init_id(data: bytearray, init_id: int) -> None: + """Rewrite the init id of a non-legacy format lidar packet.""" + data[4:7] = memoryview(init_id.to_bytes(3, byteorder='little')) + + +def set_prod_sn(data: bytearray, prod_sn: int) -> None: + """Rewrite the sn of a non-legacy format lidar packet.""" + data[7:11] = memoryview(prod_sn.to_bytes(5, byteorder='little')) + + +def fake_packet_stream_with_frame_id(metadata: client.SensorInfo, + n_frames: int, + n_lidar_packets_per_frame: int, + n_imu_packets_per_frame: int, + frame_id_fn: Callable[[int], int]) -> Iterator[client.Packet]: + """Generate non-legacy lidar packets with frame_id set. + Include some IMU packets to make sure indices are + computed correctly with IMU data present.""" + pf = _client.PacketFormat.from_info(metadata) + + for frame in range(n_frames): + choices = [True] * n_lidar_packets_per_frame + [False] * n_imu_packets_per_frame + shuffle(choices) + for is_lidar in choices: + packet_ts = None + if is_lidar: + buf = bytearray( + getrandbits(8) for _ in range(pf.lidar_packet_size)) + set_init_id(buf, metadata.init_id) + set_prod_sn(buf, int(metadata.sn)) + packet = client.LidarPacket(buf, metadata, packet_ts) + assert not packet.id_error + _patch_frame_id(packet, frame_id_fn(frame)) + yield packet + else: + buf = bytearray(getrandbits(8) for _ in range(pf.imu_packet_size)) + yield client.ImuPacket(buf, metadata, packet_ts) + + @pytest.fixture def n_packets() -> int: return 10 @@ -161,21 +202,19 @@ def test_pcap_info_10(fake_meta, fake_pcap_path) -> None: """Test reading packet headers with private helper.""" ports: Dict[int, int] = defaultdict(int) sizes: Dict[int, int] = defaultdict(int) - encap: Dict[int, int] = defaultdict(int) - net: Dict[int, int] = defaultdict(int) af: Dict[int, int] = defaultdict(int) + info = pcap._packet_info_stream(fake_pcap_path, 0) - for item in pcap._packet_info_stream(fake_pcap_path): - ports[item.dst_port] += 1 - sizes[item.payload_size] += 1 - encap[item.encapsulation_protocol] += 1 - net[item.network_protocol] += 1 - af[item.ip_version] += 1 + for key in info.udp_streams: + ports[key.dst_port] += 1 + for size in info.udp_streams[key].payload_size_counts: + sizes[size] += info.udp_streams[key].payload_size_counts[size] + for net_ver in info.udp_streams[key].ip_version_counts: + af[net_ver] += info.udp_streams[key].ip_version_counts[net_ver] - assert ports[7502] + ports[7503] == 10 + assert ports[7502] + ports[7503] == 2 assert sizes[6464] + sizes[48] == 10 - assert encap == {ETH_PROTO: 10} - assert net == {UDP_PROTO: 10} + assert info.encapsulation_protocol == ETH_PROTO assert af == {4: 10} @@ -183,13 +222,11 @@ def test_pcap_info_10(fake_meta, fake_pcap_path) -> None: @pytest.mark.parametrize('use_sll', [True, False]) def test_pcap_info_encap_proto(fake_pcap_path, use_sll) -> None: """Test reading/writing pcaps with different encapsulation.""" - encap: Dict[int, int] = defaultdict(int) - for item in pcap._packet_info_stream(fake_pcap_path): - encap[item.encapsulation_protocol] += 1 + info = pcap._packet_info_stream(fake_pcap_path, 0) proto = SLL_PROTO if use_sll else ETH_PROTO - assert encap == {proto: 10} + assert info.encapsulation_protocol == proto def test_pcap_reset(fake_pcap) -> None: @@ -326,11 +363,12 @@ def test_lidar_guess_ambiguous(fake_meta, tmpdir) -> None: file_path = path.join(tmpdir, "pcap_test.pcap") buf_size = 2**16 - handle = _pcap.record_initialize(file_path, "127.0.0.1", "127.0.0.1", - buf_size) + handle = _pcap.record_initialize(file_path, buf_size) try: - _pcap.record_packet(handle, 7502, 7502, (next(packets))._data, 1) - _pcap.record_packet(handle, 7503, 7503, (next(packets))._data, 2) + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7502, 7502, + (next(packets))._data, 1) + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7503, 7503, + (next(packets))._data, 2) finally: _pcap.record_uninitialize(handle) @@ -346,11 +384,12 @@ def test_imu_guess_ambiguous(fake_meta, tmpdir) -> None: file_path = path.join(tmpdir, "pcap_test.pcap") buf_size = 2**16 - handle = _pcap.record_initialize(file_path, "127.0.0.1", "127.0.0.1", - buf_size) + handle = _pcap.record_initialize(file_path, buf_size) try: - _pcap.record_packet(handle, 7502, 7502, (next(packets))._data, 1) - _pcap.record_packet(handle, 7503, 7503, (next(packets))._data, 2) + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7502, 7502, + (next(packets))._data, 1) + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7503, 7503, + (next(packets))._data, 2) finally: _pcap.record_uninitialize(handle) @@ -360,6 +399,32 @@ def test_imu_guess_ambiguous(fake_meta, tmpdir) -> None: assert len(list(source)) == 1 +def test_lidar_imu_guess_ambiguous(fake_meta, tmpdir) -> None: + """Test reading when there's more than one possible lidar port.""" + lidar_packets = fake_packets(fake_meta, n_lidar=2) + imu_packets = fake_packets(fake_meta, n_imu=2) + file_path = path.join(tmpdir, "pcap_test.pcap") + + buf_size = 2**16 + handle = _pcap.record_initialize(file_path, buf_size) + try: + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7502, 7502, + (next(lidar_packets))._data, 1) + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7503, 7503, + (next(lidar_packets))._data, 2) + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7504, 7504, + (next(imu_packets))._data, 3) + _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7505, 7505, + (next(imu_packets))._data, 4) + finally: + _pcap.record_uninitialize(handle) + + source = pcap.Pcap(file_path, fake_meta) + assert len(source._guesses) > 1 + assert source.ports == (7503, 7505) # arbitrary but deterministic + assert len(list(source)) == 2 + + def test_pcap_read_real(real_pcap: pcap.Pcap) -> None: """Sanity check reading pcaps with real data.""" @@ -388,3 +453,162 @@ def test_pcap_guess_real(meta: client.SensorInfo, real_pcap_path: str) -> None: real_pcap = pcap.Pcap(real_pcap_path, meta_no_ports) assert real_pcap.ports[0] == 7502 + + +def test_record_packet_info(fake_meta, tmpdir) -> None: + """Test recording packets using the packet_info interface.""" + packets = fake_packets(fake_meta, 10, 10) + file_path = path.join(tmpdir, "pcap_test.pcap") + + buf_size = 2**16 + record = _pcap.record_initialize(file_path, buf_size) + i = 0 + for next_packet in packets: + info = _pcap.packet_info() + + info.dst_ip = "127.0.0." + str(i) + info.src_ip = "127.0.1." + str(i) + + info.dst_port = 10000 + i + info.src_port = 20000 + i + + info.timestamp = i + + _pcap.record_packet(record, info, next_packet._data) + + i += 1 + + _pcap.record_uninitialize(record) + + playback = _pcap.replay_initialize(file_path) + info = _pcap.packet_info() + i = 0 + while _pcap.next_packet_info(playback, info): + assert (info.dst_ip == "127.0.0." + str(i)) + assert (info.src_ip == "127.0.1." + str(i)) + + assert (info.dst_port == 10000 + i) + assert (info.src_port == 20000 + i) + + assert (info.timestamp == i) + + i += 1 + + _pcap.replay_uninitialize(playback) + + +def test_indexed_pcap_reader(tmpdir): + """It should correctly locate the start of frames in a PCAP file""" + meta_path = path.join(DATA_DIR, f"{TESTS['dual-2.2']}.json") + + def progress_callback(offset, delta, filesize): + pass + sensor_info = client.SensorInfo(open(meta_path).read()) + num_frames = 10 + in_packets = list(fake_packet_stream_with_frame_id(sensor_info, num_frames, 3, 3, lambda frame_num: frame_num)) + assert len(in_packets) > 0 + file_path = path.join(tmpdir, "pcap_index_test.pcap") + pcap.record(in_packets, file_path) + reader = _pcap.IndexedPcapReader(file_path, [meta_path], progress_callback) + + # the index should contain the number of frames from the file + assert reader.frame_count(0) == num_frames + assert len(reader.frame_indices[0]) == num_frames + + # make sure that the file offset for a frame in the index corresponds to the offset of the first packet of the frame + frame_num = 0 + previous_frame_id = None + while reader.next_packet(): + info = reader.current_info() + if info.dst_port == sensor_info.udp_port_lidar: + packet_frame_id = reader.current_frame_id() + if previous_frame_id is None or packet_frame_id > previous_frame_id: + assert reader.frame_indices[0][frame_num] == info.file_offset + previous_frame_id = packet_frame_id + frame_num += 1 + + +def test_indexed_pcap_reader_seek(tmpdir): + """After seeking to the start of a frame, next_packet should return the first packet of that frame""" + meta_path = path.join(DATA_DIR, f"{TESTS['dual-2.2']}.json") + + def progress_callback(offset, delta, filesize): + pass + sensor_info = client.SensorInfo(open(meta_path).read()) + packet_format = _client.PacketFormat.from_info(sensor_info) + num_frames = 10 + packets_per_frame = 3 + in_packets = list(fake_packet_stream_with_frame_id(sensor_info, num_frames, + packets_per_frame, 3, lambda frame_num: frame_num)) + assert len(in_packets) > 0 + file_path = path.join(tmpdir, "pcap_index_test.pcap") + pcap.record(in_packets, file_path) + reader = _pcap.IndexedPcapReader(file_path, [meta_path], progress_callback) + + for frame in range(num_frames): + reader.seek_to_frame(0, frame) + assert packet_format.lidar_packet_size == reader.next_packet() + assert reader.current_info().file_offset == reader.frame_indices[0][frame] + assert reader.current_frame_id() == frame + assert reader.current_frame_id() == packet_format.frame_id(reader.current_data().tobytes()) + + +def test_out_of_order_frames(tmpdir): + """Frames that are out of order are skipped""" + meta_path = path.join(DATA_DIR, f"{TESTS['dual-2.2']}.json") + + def progress_callback(offset, delta, filesize): + pass + sensor_info = client.SensorInfo(open(meta_path).read()) + packet_format = _client.PacketFormat.from_info(sensor_info) + num_frames = 10 + packets_per_frame = 3 + + # odd number frames are out of order in this dataset + # frame ids are 2, 1, 4, 3, 6, ... + in_packets = list(fake_packet_stream_with_frame_id(sensor_info, num_frames, + packets_per_frame, 3, lambda f: f + 1 + (1 - f % 2) - f % 2)) + + assert len(in_packets) > 0 + file_path = path.join(tmpdir, "pcap_index_test.pcap") + pcap.record(in_packets, file_path) + reader = _pcap.IndexedPcapReader(file_path, [meta_path], progress_callback) + + # since odd number frames are out of order, there should be half as many in the index as in the input + assert len(reader.frame_indices[0]) == num_frames // 2 + + for frame in range(len(reader.frame_indices[0])): + reader.seek_to_frame(0, frame) + assert packet_format.lidar_packet_size == reader.next_packet() + assert reader.current_info().file_offset == reader.frame_indices[0][frame] + assert reader.current_frame_id() == (frame + 1) * 2 + assert reader.current_frame_id() == packet_format.frame_id(reader.current_data().tobytes()) + + +def test_current_data(fake_meta, tmpdir): + """It should provide access to current packet data as a memory view""" + meta_path = path.join(DATA_DIR, f"{TESTS['dual-2.2']}.json") + + def progress_callback(offset, delta, filesize): + pass + sensor_info = client.SensorInfo(open(meta_path).read()) + packet_format = _client.PacketFormat.from_info(sensor_info) + num_frames = 10 + packets_per_frame = 1 + + # odd number frames are out of order in this dataset + # frame ids are 2, 1, 4, 3, 6, ... + in_packets = list(fake_packet_stream_with_frame_id(sensor_info, num_frames, + packets_per_frame, 3, lambda f: f + 1 + (1 - f % 2) - f % 2)) + + assert len(in_packets) > 0 + file_path = path.join(tmpdir, "pcap_index_test.pcap") + pcap.record(in_packets, file_path) + reader = _pcap.IndexedPcapReader(file_path, [meta_path], progress_callback) + frame_ids = [] + while reader.next_packet(): + info = reader.current_info() + if info.dst_port == sensor_info.udp_port_lidar: + packet_data = reader.current_data() + frame_ids.append(packet_format.frame_id(packet_data.tobytes())) + assert frame_ids == [2, 1, 4, 3, 6, 5, 8, 7, 10, 9] diff --git a/python/tests/test_viz.py b/python/tests/test_viz.py index eada810f..a2cf2a35 100644 --- a/python/tests/test_viz.py +++ b/python/tests/test_viz.py @@ -4,7 +4,7 @@ """ import weakref -from typing import TYPE_CHECKING, Tuple +from typing import TYPE_CHECKING, Tuple, Optional, Callable import pytest import numpy as np @@ -48,6 +48,7 @@ def handle_keys(ctx, key, mods) -> bool: return True point_viz.push_key_handler(handle_keys) + viz.add_default_controls(point_viz) return point_viz @@ -63,6 +64,128 @@ def test_point_viz_image(point_viz: viz.PointViz) -> None: point_viz.run() +def test_point_viz_rgb_image(point_viz: viz.PointViz) -> None: + """Test displaying a rgb image.""" + img = viz.Image() + img.set_position(-4 / 3, 4 / 3, -1, 1) + point_viz.add(img) + + label = viz.Label("", 0, 0, align_top=True) + point_viz.add(label) + + def show_viz(): + point_viz.update() + point_viz.run() + + label.set_text("Image RGB: set..image(mono), 2dim") + image_data_mono = np.array( + [[0.1, 0.3, 0.7], [0.2, 0.4, 0.8], [0.3, 0.5, 0.9]], dtype=float) + img.set_image(image_data_mono) + show_viz() + + label.set_text("Image RGB: set..image(mono), 3dim") + img.set_image(image_data_mono[:, :, np.newaxis]) + show_viz() + + # 2 elements on 3rd dimension is an error + with pytest.raises(ValueError): + img.set_image(np.dstack((image_data_mono, image_data_mono))) + + label.set_text("Image RGB: set..image(rgb), 3dim") + image_data_rgb = np.array( + [[[0, 0, 0], [1, 0, 0], [0, 1, 0]], [[1, 1, 0], [0, 0, 1], [1, 0, 1]], + [[0, 1, 1], [1, 1, 1], [0, 0, 0]]], + dtype=float) + img.set_image(image_data_rgb) + show_viz() + + label.set_text( + "Image RGB: set..image(rgba), 3dim, two right columns has 0.5 alpha") + image_data_rgba = np.dstack( + (image_data_rgb, np.full((*image_data_rgb.shape[:2], 1), 0.5))) + image_data_rgba[:, 0, 3] = 1.0 + img.set_image(image_data_rgba) + show_viz() + + label.set_text( + "Image RGB: set..mask(rgba), 4 pixels with different alphas, image the same" + ) + mask_data_rgba = np.array([[[0, 0, 1.0, 0.2], [0, 0, 1.0, 0.4]], + [[0, 0, 1.0, 0.6], [0, 0, 1.0, 0.8]]], + dtype=float) + img.set_mask(mask_data_rgba) + show_viz() + + label.set_text( + "Image RGB: set..mask(rgba), +3 columns added to mask, image the same") + mask_data_rgba = np.hstack( + (mask_data_rgba, + np.array([[[1.0, 0, 0, 0.3], [1.0, 0, 0, 0.5], [1.0, 0, 0, 0.7]], + [[1.0, 0, 0, 0.3], [1.0, 0, 0, 0.5], [1.0, 0, 0, 0.7]]], + dtype=float))) + img.set_mask(mask_data_rgba) + show_viz() + + label.set_text( + "Image RGB: set..mask(rgba), last row alpha 0.8, image the same") + mask_data_rgba[-1, :, 3] = 0.8 + img.set_mask(mask_data_rgba) + show_viz() + + label.set_text( + "Image RGB: set..mask(rgba), last row alpha 1.0, image the same") + mask_data_rgba[-1, :, 3] = 1.0 + img.set_mask(mask_data_rgba) + show_viz() + + label.set_text("Image RGB: set..image(mono), mask the same") + img.set_image(image_data_mono) + show_viz() + + +@pytest.mark.parametrize('test_key', ['single-2.3']) +def test_point_viz_image_palette(meta: client.SensorInfo, + scan: client.LidarScan, + point_viz: viz.PointViz) -> None: + """Test displaying a full-window image.""" + + palettes = [ + ('no..palette', lambda i: i.clear_palette()), + ('spezia..palette', lambda i: i.set_palette(viz.spezia_palette)), + ('calref..palette', lambda i: i.set_palette(viz.calref_palette)), + ('viridis..palette', lambda i: i.set_palette(viz.viridis_palette)), + ('magma..palette', lambda i: i.set_palette(viz.magma_palette)), + ('grey..palette', lambda i: i.set_palette(viz.grey_palette)), + ] + + num_palettes = len(palettes) + + images = [] + labels = [] + + key = np.tile(np.linspace(0.0, 1.0, 1024, dtype=np.float32), (128, 1)) + + for idx, (palette_name, palette_enable) in enumerate(palettes): + img = viz.Image() + img_y0 = -1 + idx * 2 / num_palettes + img_y1 = -1 + (idx + 1) * 2 / num_palettes + img.set_position(-4 / 3, 4 / 3, img_y0, img_y1) + palette_enable(img) + img.set_image(key) + point_viz.add(img) + images.append(img) + + label = viz.Label(palette_name, 0.5, (1 - img_y1) / 2, align_top=True) + point_viz.add(label) + labels.append(label) + + def show_viz(): + point_viz.update() + point_viz.run() + + show_viz() + + def test_point_viz_image_with_labels_aligned(point_viz: viz.PointViz) -> None: """Test displaying a set of images aligned to the corners.""" @@ -244,6 +367,113 @@ def animate() -> None: thread.join() +def test_point_viz_rgb_cloud(point_viz: viz.PointViz) -> None: + """Test rgb coloring of clouds.""" + + cloud = viz.Cloud(1024) + point_viz.add(cloud) + + points = np.random.rand(3, 1024).astype(np.float32) * 30 - 15 + + # should be r, g, b from top to bottom + key = np.zeros(1024, dtype=float) + + key[5.0 < points[2, :]] = 0.2 + key[(-5.0 < points[2, :]) & (points[2, :] <= 5.0)] = 0.5 + key[points[2, :] < -5.0] = 0.8 + + cloud.set_xyz(points) + + cloud.set_point_size(10) + point_viz.camera.pitch(-45.0) + + label = viz.Label("", 0, 0, align_top=True) + point_viz.add(label) + + def show_viz(): + point_viz.update() + point_viz.run() + + with pytest.raises(ValueError): + cloud.set_key(key.reshape((1, 1024, 1, 1))) + + with pytest.raises(ValueError): + cloud.set_key(key[:100]) + + with pytest.raises(ValueError): + cloud.set_key(np.dstack((key, key))) + + # 1dim key, mono, (HxW) size =========================== + label.set_text("Cloud RGB: MONO set..key()") + cloud.set_key(key) + show_viz() + + # + set_key_alpha() ==================================== + label.set_text("Cloud RGB: add set..key..alpha() to parts") + key_alpha = np.full(1024, 1.0, dtype=float) + key_alpha[points[1, :] > 0] = 0.3 + cloud.set_key_alpha(key_alpha) + show_viz() + + # + set_mask(rgba) ===================================== + label.set_text("Cloud RGB: add set..mask() 1/3 of cloud " + "in RED with transparency") + ones = np.ones([1024, 1], dtype=float) + mask_rgba = np.hstack((0.1 * ones, 0.1 * ones, 0.1 * ones, 0.5 * ones)) + mask_rgba[points[0, :] > 5.0] = np.array([0.8, 0.1, 0.1, 0.8]) + cloud.set_mask(mask_rgba) + show_viz() + + # remove mask + zeros = np.zeros([1024, 4], dtype=float) + cloud.set_mask(zeros) + + # 2dim key, mono, (HxW) size =========================== + label.set_text("Cloud RGB: MONO, set..key() 2dim, no mask, but key alpha " + "left") + key_2dim = key.reshape((1, 1024)) + cloud.set_key(key_2dim) + show_viz() + + # 3dim key, mono, (HxW) size =========================== + label.set_text("Cloud RGB: MONO, set..key() 3dim, x 0.5, no mask, but key " + "alpha left") + key_3dim = key.reshape((1, 1024, 1)) * 0.5 + cloud.set_key(key_3dim) + show_viz() + + # 3dim key, color 3 channel RGB, (HxWx3) size ========== + label.set_text("Cloud RGB: RGB, set..key() 3dim, no mask, but key alpha " + "left") + key_3dim_rgb = np.dstack( + (key, np.full(1024, 0.2, dtype=float), np.full(1024, 0.2, dtype=float))) + cloud.set_key(key_3dim_rgb) + show_viz() + + # 3dim key, color 4 channel RGBA, (HxWx4) size ========= + label.set_text("Cloud RGB: RGBA, set..key() 3dim, no mask, key alpha " + "should be overwritten by key") + key_3dim_rgba = np.dstack( + (np.full(1024, 0.1, dtype=float), np.full(1024, 1.0, dtype=float), + np.full(1024, 0.1, dtype=float), key)) + cloud.set_key(key_3dim_rgba) + show_viz() + + # millions of points =================================== + label.set_text("Cloud RGB: Add big cloud") + many_points_num = 5 * 10**6 + big_cloud = viz.Cloud(many_points_num) + big_points = np.random.rand(3, many_points_num).astype(np.float32) + + big_key = big_points[0] + big_points = big_points * 300 - 14 + + big_cloud.set_xyz(big_points) + big_cloud.set_key(big_key) + point_viz.add(big_cloud) + show_viz() + + def test_point_viz_destruction() -> None: """Check that PointViz is destroyed deterministically.""" point_viz = viz.PointViz("Test Viz") @@ -253,7 +483,7 @@ def test_point_viz_destruction() -> None: assert ref() is None -@pytest.mark.parametrize('test_key', ['legacy-2.0']) +@pytest.mark.parametrize('test_key', ['single-2.3']) def test_scan_viz_destruction(meta: client.SensorInfo, point_viz: viz.PointViz) -> None: """Check that LidarScan is destroyed deterministically.""" @@ -264,7 +494,7 @@ def test_scan_viz_destruction(meta: client.SensorInfo, assert ref() is None -@pytest.mark.parametrize('test_key', ['legacy-2.0']) +@pytest.mark.parametrize('test_key', ['single-2.3']) def test_viz_multiple_instances(meta: client.SensorInfo, scan: client.LidarScan) -> None: """Check that destructing a visualizer doesn't break other instances.""" @@ -291,7 +521,7 @@ def test_scan_viz_smoke(meta: client.SensorInfo, ls_viz.run() -@pytest.mark.parametrize('test_key', ['legacy-2.0']) +@pytest.mark.parametrize('test_key', ['single-2.3']) def test_scan_viz_extras(meta: client.SensorInfo, scan: client.LidarScan) -> None: """Check rendering of labels, cuboids, clouds and images together.""" @@ -319,3 +549,103 @@ def test_scan_viz_extras(meta: client.SensorInfo, point_viz.camera.dolly(150) ls_viz.draw() point_viz.run() + + +class LidarScanVizWithCallbacks(viz.LidarScanViz): + """Add callbacks for pre-draw and post-draw""" + + def __init__( + self, + meta: client.SensorInfo, + viz: Optional[viz.PointViz] = None, + pre_draw_callback: Optional[Callable[[client.LidarScan], + client.LidarScan]] = None, + post_draw_callback: Optional[Callable[['LidarScanVizWithCallbacks'], + None]] = None + ) -> None: + super().__init__(meta, viz) + + self._pre_draw_callback = pre_draw_callback + self._post_draw_callback = post_draw_callback + + def _draw(self) -> None: + """Overriding the draw and setting pre/post callbacks""" + + # pre draw callbacl takes LidarScan and returns LidarScan + if self._pre_draw_callback: + self._scan = self._pre_draw_callback(self._scan) + + # call original draw + super()._draw() + + # post draw callbacks takes LidarScanViz object so it can get access + # to _clouds and _images within it and set additional masks, colors, etc. + if self._post_draw_callback: + self._post_draw_callback(self) + + +@pytest.mark.parametrize('test_key', ['single-2.3']) +def test_simple_viz_with_callbacks(meta: client.SensorInfo, + scan: client.LidarScan) -> None: + """Call the callback on pre/post draw example.""" + + from itertools import repeat + from copy import deepcopy + + start_range = 1 # in meters + num_steps = 100 + + # this can be any scan source (just a repeater as an example) + scans = repeat(scan, num_steps) + + point_viz = viz.PointViz("Test Viz") + + scan_cnt = 0 + + def pre_draw(ls: client.LidarScan) -> client.LidarScan: + nonlocal scan_cnt + + nls = deepcopy(ls) + ratio = scan_cnt / num_steps + + # modifying range in some way + range = nls.field(client.ChanField.RANGE) + range = start_range * 1000 + (range - start_range * 1000) * ratio + nls.field(client.ChanField.RANGE)[:] = range + + scan_cnt += 1 + # don't forget to return it back + return nls + + def post_draw(scan_viz: LidarScanVizWithCallbacks) -> None: + + # currently discplayed LidarScan + ls = scan_viz._scan + + ratio = scan_cnt / num_steps + img_mask = np.zeros((ls.h, ls.w, 4)) + col_idx = int(ls.w * ratio) + img_mask[:, col_idx - 5:col_idx + 5] = np.array([1.0, 0.3, 0.3, 1.0]) + + # there are 2 images - single return and second + # can safely skip the second, but here we draw on both smth + for img in scan_viz._images: + # set some mask + img.set_mask(img_mask) + + # same for clouds, first and second return + for cloud in scan_viz._clouds: + # set some mask + cloud.set_mask(img_mask) + + # regular LidarScanViz + # ls_viz = viz.LidarScanViz(meta, point_viz) + + ls_viz = LidarScanVizWithCallbacks(meta, + point_viz, + pre_draw_callback=pre_draw, + post_draw_callback=post_draw) + + viz.SimpleViz(ls_viz, 1.0).run(scans) + + print("Done") diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index c781cc03..cd74dc3a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -56,3 +56,15 @@ set_tests_properties( ENVIRONMENT DATA_DIR=${CMAKE_CURRENT_LIST_DIR}/metadata/ ) + +add_executable(pcap_test pcap_test.cpp) + +target_link_libraries(pcap_test OusterSDK::ouster_pcap GTest::gtest GTest::gtest_main) + +add_test(NAME pcap_test COMMAND pcap_test --gtest_output=xml:pcap_test.xml) +set_tests_properties( + pcap_test + PROPERTIES + ENVIRONMENT + DATA_DIR=${CMAKE_CURRENT_LIST_DIR}/pcaps/ +) diff --git a/tests/bcompat_meta_json_test.cpp b/tests/bcompat_meta_json_test.cpp index c0ac0f29..713c07c6 100644 --- a/tests/bcompat_meta_json_test.cpp +++ b/tests/bcompat_meta_json_test.cpp @@ -56,9 +56,13 @@ testing::Values( "2_3_1_os-992146000760-128_legacy", "2_3_1_os-992146000760-128", "2_4_0_os-992146000760-128_legacy", - "2_4_0_os-992146000760-128" - + "2_4_0_os-992146000760-128", + "3_0_1_os-122246000293-128_legacy", + "3_0_1_os-122246000293-128", + "2_5_0_os-992146000760-128_legacy", + "2_5_0_os-992146000760-128" )); + // clang-format on std::string data_dir = "."; @@ -103,6 +107,8 @@ TEST_P(MetaJsonTest, MetadataFromJson) { si_expected.lidar_to_sensor_transform); EXPECT_EQ(si.extrinsic, si_expected.extrinsic); EXPECT_EQ(si.init_id, si_expected.init_id); + EXPECT_EQ(si.udp_port_lidar, si_expected.udp_port_lidar); + EXPECT_EQ(si.udp_port_imu, si_expected.udp_port_imu); } int main(int argc, char** argv) { diff --git a/tests/bcompat_sensor_info_data.h b/tests/bcompat_sensor_info_data.h index 38274460..63bf6bc9 100644 --- a/tests/bcompat_sensor_info_data.h +++ b/tests/bcompat_sensor_info_data.h @@ -33,7 +33,7 @@ static sensor_info si_1_12_os1_991913000010_64 = { "v1.12.0", MODE_1024x10, "OS-1-64", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {3.073, 0.904, -1.261, -3.384, 3.048, 0.9, -1.235, -3.335, 3.029, 0.915, -1.2, -3.301, 3.026, 0.921, -1.179, -3.268, 3.032, 0.933, -1.156, -3.23, 3.028, 0.953, -1.131, -3.209, 3.059, 0.976, -1.112, -3.182, 3.071, 0.99, -1.087, -3.155, 3.097, 1.014, -1.103, -3.133, 3.118, 1.035, -1.039, -3.116, 3.144, 1.061, -1.013, -3.101, 3.178, 1.092, -0.993, -3.089, 3.217, 1.125, -0.972, -3.072, 3.265, 1.159, -0.949, -3.066, 3.321, 1.204, -0.921, -3.055, 3.381, 1.252, -0.898, -3.071}, {16.729, 16.118, 15.543, 14.995, 14.526, 13.937, 13.381, 12.837, 12.378, 11.801, 11.251, 10.704, 10.251, 9.693, 9.138, 8.603, 8.149, 7.594, 7.049, 6.513, 6.056, 5.504, 4.961, 4.419, 3.967, 3.424, 2.881, 2.328, 1.88, 1.337, 0.787, 0.239, -0.205, -0.756, -1.39, -1.854, -2.3, -2.839, -3.386, -3.935, -4.391, -4.929, -5.472, -6.028, -6.471, -7.02, -7.563, -8.116, -8.572, -9.112, -9.66, -10.227, -10.687, -11.226, -11.777, -12.35, -12.807, -13.347, -13.902, -14.49, -14.968, -15.504, -16.078, -16.679}, 15.806, @@ -42,8 +42,8 @@ static sensor_info si_1_12_os1_991913000010_64 = { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + 60823, + 60023, }; static sensor_info si_1_12_os1_991937000062_16A0_legacy = { @@ -52,7 +52,7 @@ static sensor_info si_1_12_os1_991937000062_16A0_legacy = { "v1.12.0", MODE_1024x10, "OS-1-16-A0", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {3.041, 0.895, -1.255, -3.387, 3.024, 0.897, -1.223, -3.326, 3.032, 0.916, -1.189, -3.288, 3.014, 0.926, -1.175, -3.243, 3.024, 0.956, -1.138, -3.206, 3.04, 0.958, -1.13, -3.183, 3.06, 0.991, -1.095, -3.159, 3.079, 1.003, -1.064, -3.129, 3.105, 1.037, -1.09, -3.1, 3.155, 1.06, -1.007, -3.083, 3.166, 1.087, -1.014, -3.067, 3.215, 1.11, -0.96, -3.048, 3.228, 1.145, -0.936, -3.04, 3.29, 1.188, -0.917, -3.037, 3.331, 1.222, -0.895, -3.043, 3.4, 1.265, -0.884, -3.041, }, {16.509, 15.91, 15.345, 14.799, 14.329, 13.747, 13.185, 12.648, 12.192, 11.624, 11.081, 10.537, 10.083, 9.525, 8.97, 8.442, 7.984, 7.429, 6.892, 6.357, 5.899, 5.349, 4.811, 4.262, 3.828, 3.257, 2.726, 2.183, 1.733, 1.186, 0.64, 0.1, -0.349, -0.893, -1.499, -1.991, -2.444, -2.983, -3.532, -4.079, -4.53, -5.065, -5.642, -6.168, -6.616, -7.179, -7.708, -8.275, -8.707, -9.248, -9.8, -10.373, -10.836, -11.368, -11.92, -12.489, -12.966, -13.516, -14.077, -14.658, -15.121, -15.684, -16.238, -16.858, }, 15.806, @@ -71,7 +71,7 @@ static sensor_info si_1_12_os1_991937000062_64_legacy = { "v1.12.0", MODE_1024x10, "OS-1-64", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {3.041, 0.895, -1.255, -3.387, 3.024, 0.897, -1.223, -3.326, 3.032, 0.916, -1.189, -3.288, 3.014, 0.926, -1.175, -3.243, 3.024, 0.956, -1.138, -3.206, 3.04, 0.958, -1.13, -3.183, 3.06, 0.991, -1.095, -3.159, 3.079, 1.003, -1.064, -3.129, 3.105, 1.037, -1.09, -3.1, 3.155, 1.06, -1.007, -3.083, 3.166, 1.087, -1.014, -3.067, 3.215, 1.11, -0.96, -3.048, 3.228, 1.145, -0.936, -3.04, 3.29, 1.188, -0.917, -3.037, 3.331, 1.222, -0.895, -3.043, 3.4, 1.265, -0.884, -3.041, }, {16.509, 15.91, 15.345, 14.799, 14.329, 13.747, 13.185, 12.648, 12.192, 11.624, 11.081, 10.537, 10.083, 9.525, 8.97, 8.442, 7.984, 7.429, 6.892, 6.357, 5.899, 5.349, 4.811, 4.262, 3.828, 3.257, 2.726, 2.183, 1.733, 1.186, 0.64, 0.1, -0.349, -0.893, -1.499, -1.991, -2.444, -2.983, -3.532, -4.079, -4.53, -5.065, -5.642, -6.168, -6.616, -7.179, -7.708, -8.275, -8.707, -9.248, -9.8, -10.373, -10.836, -11.368, -11.92, -12.489, -12.966, -13.516, -14.077, -14.658, -15.121, -15.684, -16.238, -16.858, }, 15.806, @@ -90,7 +90,7 @@ static sensor_info si_1_13_os1_991913000010_64 = { "v1.13.0", MODE_1024x10, "OS-1-64", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {3.073, 0.904, -1.261, -3.384, 3.048, 0.9, -1.235, -3.335, 3.029, 0.915, -1.2, -3.301, 3.026, 0.921, -1.179, -3.268, 3.032, 0.933, -1.156, -3.23, 3.028, 0.953, -1.131, -3.209, 3.059, 0.976, -1.112, -3.182, 3.071, 0.99, -1.087, -3.155, 3.097, 1.014, -1.103, -3.133, 3.118, 1.035, -1.039, -3.116, 3.144, 1.061, -1.013, -3.101, 3.178, 1.092, -0.993, -3.089, 3.217, 1.125, -0.972, -3.072, 3.265, 1.159, -0.949, -3.066, 3.321, 1.204, -0.921, -3.055, 3.381, 1.252, -0.898, -3.071}, {16.729, 16.118, 15.543, 14.995, 14.526, 13.937, 13.381, 12.837, 12.378, 11.801, 11.251, 10.704, 10.251, 9.693, 9.138, 8.603, 8.149, 7.594, 7.049, 6.513, 6.056, 5.504, 4.961, 4.419, 3.967, 3.424, 2.881, 2.328, 1.88, 1.337, 0.787, 0.239, -0.205, -0.756, -1.39, -1.854, -2.3, -2.839, -3.386, -3.935, -4.391, -4.929, -5.472, -6.028, -6.471, -7.02, -7.563, -8.116, -8.572, -9.112, -9.66, -10.227, -10.687, -11.226, -11.777, -12.35, -12.807, -13.347, -13.902, -14.49, -14.968, -15.504, -16.078, -16.679}, 15.806, @@ -99,8 +99,8 @@ static sensor_info si_1_13_os1_991913000010_64 = { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + 7502, + 7503, }; static sensor_info si_1_13_os1_991937000062_16A0_legacy = { @@ -109,7 +109,7 @@ static sensor_info si_1_13_os1_991937000062_16A0_legacy = { "v1.13.0", MODE_1024x10, "OS-1-16-A0", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {0, 0, 0, -3.387, 0, 0, 0, -3.326, 0, 0, 0, -3.288, 0, 0, 0, -3.243, 0, 0, 0, -3.206, 0, 0, 0, -3.183, 0, 0, 0, -3.159, 0, 0, 0, -3.129, 0, 0, 0, -3.1, 0, 0, 0, -3.083, 0, 0, 0, -3.067, 0, 0, 0, -3.048, 0, 0, 0, -3.04, 0, 0, 0, -3.037, 0, 0, 0, -3.043, 0, 0, 0, -3.041, }, {0, 0, 0, 14.799, 0, 0, 0, 12.648, 0, 0, 0, 10.537, 0, 0, 0, 8.442, 0, 0, 0, 6.357, 0, 0, 0, 4.262, 0, 0, 0, 2.183, 0, 0, 0, 0.1, 0, 0, 0, -1.991, 0, 0, 0, -4.079, 0, 0, 0, -6.168, 0, 0, 0, -8.275, 0, 0, 0, -10.373, 0, 0, 0, -12.489, 0, 0, 0, -14.658, 0, 0, 0, -16.858, }, 15.806, @@ -128,7 +128,7 @@ static sensor_info si_1_13_os1_991937000062_32A02_legacy = { "v1.13.0", MODE_1024x10, "OS-1-32-A02", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {0, 0.895, 0, -3.387, 0, 0.897, 0, -3.326, 0, 0.916, 0, -3.288, 0, 0.926, 0, -3.243, 0, 0.956, 0, -3.206, 0, 0.958, 0, -3.183, 0, 0.991, 0, -3.159, 0, 1.003, 0, -3.129, 0, 1.037, 0, -3.1, 0, 1.06, 0, -3.083, 0, 1.087, 0, -3.067, 0, 1.11, 0, -3.048, 0, 1.145, 0, -3.04, 0, 1.188, 0, -3.037, 0, 1.222, 0, -3.043, 0, 1.265, 0, -3.041, }, {0, 15.91, 0, 14.799, 0, 13.747, 0, 12.648, 0, 11.624, 0, 10.537, 0, 9.525, 0, 8.442, 0, 7.429, 0, 6.357, 0, 5.349, 0, 4.262, 0, 3.257, 0, 2.183, 0, 1.186, 0, 0.1, 0, -0.893, 0, -1.991, 0, -2.983, 0, -4.079, 0, -5.065, 0, -6.168, 0, -7.179, 0, -8.275, 0, -9.248, 0, -10.373, 0, -11.368, 0, -12.489, 0, -13.516, 0, -14.658, 0, -15.684, 0, -16.858, }, 15.806, @@ -147,7 +147,7 @@ static sensor_info si_1_13_os1_991937000062_64_legacy = { "v1.13.0", MODE_1024x10, "OS-1-64", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {3.041, 0.895, -1.255, -3.387, 3.024, 0.897, -1.223, -3.326, 3.032, 0.916, -1.189, -3.288, 3.014, 0.926, -1.175, -3.243, 3.024, 0.956, -1.138, -3.206, 3.04, 0.958, -1.13, -3.183, 3.06, 0.991, -1.095, -3.159, 3.079, 1.003, -1.064, -3.129, 3.105, 1.037, -1.09, -3.1, 3.155, 1.06, -1.007, -3.083, 3.166, 1.087, -1.014, -3.067, 3.215, 1.11, -0.96, -3.048, 3.228, 1.145, -0.936, -3.04, 3.29, 1.188, -0.917, -3.037, 3.331, 1.222, -0.895, -3.043, 3.4, 1.265, -0.884, -3.041, }, {16.509, 15.91, 15.345, 14.799, 14.329, 13.747, 13.185, 12.648, 12.192, 11.624, 11.081, 10.537, 10.083, 9.525, 8.97, 8.442, 7.984, 7.429, 6.892, 6.357, 5.899, 5.349, 4.811, 4.262, 3.828, 3.257, 2.726, 2.183, 1.733, 1.186, 0.64, 0.1, -0.349, -0.893, -1.499, -1.991, -2.444, -2.983, -3.532, -4.079, -4.53, -5.065, -5.642, -6.168, -6.616, -7.179, -7.708, -8.275, -8.707, -9.248, -9.8, -10.373, -10.836, -11.368, -11.92, -12.489, -12.966, -13.516, -14.077, -14.658, -15.121, -15.684, -16.238, -16.858, }, 15.806, @@ -166,7 +166,7 @@ static sensor_info si_1_14_6cccd_os_882002000138_128_legacy = { "v1.14.0-beta.1-928-g6cccd78", MODE_1024x10, "OS-0-128", - {128, 16, 1024, {48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, {48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {11.15, 3.677, -3.639, -10.81, 10.7, 3.534, -3.502, -10.41, 10.32, 3.412, -3.384, -10.08, 9.999, 3.307, -3.283, -9.786, 9.72, 3.217, -3.197, -9.537, 9.48, 3.14, -3.122, -9.322, 9.273, 3.074, -3.059, -9.138, 9.096, 3.017, -3.004, -8.981, 8.945, 2.968, -2.957, -8.847, 8.818, 2.927, -2.918, -8.736, 8.711, 2.894, -2.886, -8.643, 8.623, 2.866, -2.86, -8.57, 8.554, 2.844, -2.84, -8.513, 8.501, 2.828, -2.825, -8.472, 8.465, 2.817, -2.815, -8.447, 8.444, 2.811, -2.811, -8.438, 8.438, 2.811, -2.811, -8.444, 8.447, 2.815, -2.817, -8.465, 8.472, 2.825, -2.828, -8.501, 8.513, 2.84, -2.844, -8.554, 8.57, 2.86, -2.866, -8.623, 8.643, 2.886, -2.894, -8.711, 8.736, 2.918, -2.927, -8.818, 8.847, 2.957, -2.968, -8.945, 8.981, 3.004, -3.017, -9.096, 9.138, 3.059, -3.074, -9.273, 9.322, 3.122, -3.14, -9.48, 9.537, 3.197, -3.217, -9.72, 9.786, 3.283, -3.307, -9.999, 10.08, 3.384, -3.412, -10.32, 10.41, 3.502, -3.534, -10.7, 10.81, 3.639, -3.677, -11.15, }, {46.2, 45.12, 44.36, 43.9, 43.14, 42.09, 41.33, 40.87, 40.11, 39.09, 38.34, 37.86, 37.11, 36.11, 35.37, 34.88, 34.14, 33.16, 32.43, 31.92, 31.19, 30.24, 29.51, 28.99, 28.26, 27.33, 26.61, 26.07, 25.34, 24.44, 23.72, 23.17, 22.45, 21.57, 20.86, 20.29, 19.57, 18.72, 18, 17.42, 16.7, 15.87, 15.16, 14.56, 13.84, 13.04, 12.33, 11.71, 10.99, 10.21, 9.503, 8.862, 8.152, 7.388, 6.684, 6.023, 5.314, 4.572, 3.868, 3.188, 2.479, 1.758, 1.055, 0.3541, -0.3541, -1.055, -1.758, -2.479, -3.188, -3.868, -4.572, -5.314, -6.023, -6.684, -7.388, -8.152, -8.862, -9.503, -10.21, -10.99, -11.71, -12.33, -13.04, -13.84, -14.56, -15.16, -15.87, -16.7, -17.42, -18, -18.72, -19.57, -20.29, -20.86, -21.57, -22.45, -23.17, -23.72, -24.44, -25.34, -26.07, -26.61, -27.33, -28.26, -28.99, -29.51, -30.24, -31.19, -31.92, -32.43, -33.16, -34.14, -34.88, -35.37, -36.11, -37.11, -37.86, -38.34, -39.09, -40.11, -40.87, -41.33, -42.09, -43.14, -43.9, -44.36, -45.12, -46.2, }, 27.67, @@ -185,7 +185,7 @@ static sensor_info si_1_14_6cccd_os_882002000138_32U0_legacy = { "v1.14.0-beta.1-928-g6cccd78", MODE_1024x10, "OS-0-32-U0", - {32, 16, 1024, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {32, 16, 1024, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {-10.81, -10.41, -10.08, -9.786, -9.537, -9.322, -9.138, -8.981, -8.847, -8.736, -8.643, -8.57, -8.513, -8.472, -8.447, -8.438, -8.444, -8.465, -8.501, -8.554, -8.623, -8.711, -8.818, -8.945, -9.096, -9.273, -9.48, -9.72, -9.999, -10.32, -10.7, -11.15, }, {43.9, 40.87, 37.86, 34.88, 31.92, 28.99, 26.07, 23.17, 20.29, 17.42, 14.56, 11.71, 8.862, 6.023, 3.188, 0.3541, -2.479, -5.314, -8.152, -10.99, -13.84, -16.7, -19.57, -22.45, -25.34, -28.26, -31.19, -34.14, -37.11, -40.11, -43.14, -46.2, }, 27.67, @@ -204,7 +204,7 @@ static sensor_info si_1_14_6cccd_os_882002000138_64U02_legacy = { "v1.14.0-beta.1-928-g6cccd78", MODE_1024x10, "OS-0-64-U02", - {64, 16, 1024, {32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, 32, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {3.677, -10.81, 3.534, -10.41, 3.412, -10.08, 3.307, -9.786, 3.217, -9.537, 3.14, -9.322, 3.074, -9.138, 3.017, -8.981, 2.968, -8.847, 2.927, -8.736, 2.894, -8.643, 2.866, -8.57, 2.844, -8.513, 2.828, -8.472, 2.817, -8.447, 2.811, -8.438, 2.811, -8.444, 2.815, -8.465, 2.825, -8.501, 2.84, -8.554, 2.86, -8.623, 2.886, -8.711, 2.918, -8.818, 2.957, -8.945, 3.004, -9.096, 3.059, -9.273, 3.122, -9.48, 3.197, -9.72, 3.283, -9.999, 3.384, -10.32, 3.502, -10.7, 3.639, -11.15, }, {45.12, 43.9, 42.09, 40.87, 39.09, 37.86, 36.11, 34.88, 33.16, 31.92, 30.24, 28.99, 27.33, 26.07, 24.44, 23.17, 21.57, 20.29, 18.72, 17.42, 15.87, 14.56, 13.04, 11.71, 10.21, 8.862, 7.388, 6.023, 4.572, 3.188, 1.758, 0.3541, -1.055, -2.479, -3.868, -5.314, -6.684, -8.152, -9.503, -10.99, -12.33, -13.84, -15.16, -16.7, -18, -19.57, -20.86, -22.45, -23.72, -25.34, -26.61, -28.26, -29.51, -31.19, -32.43, -34.14, -35.37, -37.11, -38.34, -40.11, -41.33, -43.14, -44.36, -46.2, }, 27.67, @@ -223,7 +223,7 @@ static sensor_info si_1_14_beta_os1_991937000062_16A0_legacy = { "v1.14.0-beta.1-87-gde6f92c", MODE_1024x10, "OS-1-16-A0", - {16, 16, 1024, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {16, 16, 1024, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {-3.387, -3.326, -3.288, -3.243, -3.206, -3.183, -3.159, -3.129, -3.1, -3.083, -3.067, -3.048, -3.04, -3.037, -3.043, -3.041, }, {14.799, 12.648, 10.537, 8.442, 6.357, 4.262, 2.183, 0.1, -1.991, -4.079, -6.168, -8.275, -10.373, -12.489, -14.658, -16.858, }, 15.806, @@ -242,7 +242,7 @@ static sensor_info si_1_14_beta_os1_991937000062_32A02_legacy = { "v1.14.0-beta.1-87-gde6f92c", MODE_1024x10, "OS-1-32-A02", - {32, 16, 1024, {12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {32, 16, 1024, {12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, 12, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {0.895, -3.387, 0.897, -3.326, 0.916, -3.288, 0.926, -3.243, 0.956, -3.206, 0.958, -3.183, 0.991, -3.159, 1.003, -3.129, 1.037, -3.1, 1.06, -3.083, 1.087, -3.067, 1.11, -3.048, 1.145, -3.04, 1.188, -3.037, 1.222, -3.043, 1.265, -3.041, }, {15.91, 14.799, 13.747, 12.648, 11.624, 10.537, 9.525, 8.442, 7.429, 6.357, 5.349, 4.262, 3.257, 2.183, 1.186, 0.1, -0.893, -1.991, -2.983, -4.079, -5.065, -6.168, -7.179, -8.275, -9.248, -10.373, -11.368, -12.489, -13.516, -14.658, -15.684, -16.858, }, 15.806, @@ -261,7 +261,7 @@ static sensor_info si_1_14_beta_os1_991937000062_64_legacy = { "v1.14.0-beta.1-87-gde6f92c", MODE_1024x10, "OS-1-64", - {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, {18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, 18, 12, 6, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {3.041, 0.895, -1.255, -3.387, 3.024, 0.897, -1.223, -3.326, 3.032, 0.916, -1.189, -3.288, 3.014, 0.926, -1.175, -3.243, 3.024, 0.956, -1.138, -3.206, 3.04, 0.958, -1.13, -3.183, 3.06, 0.991, -1.095, -3.159, 3.079, 1.003, -1.064, -3.129, 3.105, 1.037, -1.09, -3.1, 3.155, 1.06, -1.007, -3.083, 3.166, 1.087, -1.014, -3.067, 3.215, 1.11, -0.96, -3.048, 3.228, 1.145, -0.936, -3.04, 3.29, 1.188, -0.917, -3.037, 3.331, 1.222, -0.895, -3.043, 3.4, 1.265, -0.884, -3.041, }, {16.509, 15.91, 15.345, 14.799, 14.329, 13.747, 13.185, 12.648, 12.192, 11.624, 11.081, 10.537, 10.083, 9.525, 8.97, 8.442, 7.984, 7.429, 6.892, 6.357, 5.899, 5.349, 4.811, 4.262, 3.828, 3.257, 2.726, 2.183, 1.733, 1.186, 0.64, 0.1, -0.349, -0.893, -1.499, -1.991, -2.444, -2.983, -3.532, -4.079, -4.53, -5.065, -5.642, -6.168, -6.616, -7.179, -7.708, -8.275, -8.707, -9.248, -9.8, -10.373, -10.836, -11.368, -11.92, -12.489, -12.966, -13.516, -14.077, -14.658, -15.121, -15.684, -16.238, -16.858, }, 15.806, @@ -280,7 +280,7 @@ static sensor_info si_1_14_beta_os_882004000055_128_legacy = { "v1.14.0-beta.1-87-gde6f92c", MODE_1024x10, "OS-0-128", - {128, 16, 1024, {48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, {48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, }, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {11.148, 3.6772, -3.6392, -10.8077, 10.7031, 3.5339, -3.5016, -10.4132, 10.3239, 3.4117, -3.3841, -10.0757, 9.999, 3.3071, -3.2833, -9.7858, 9.72, 3.2172, -3.1968, -9.5365, 9.4799, 3.1399, -3.1224, -9.3221, 9.2734, 3.0736, -3.0585, -9.1381, 9.0964, 3.0167, -3.0039, -8.9808, 8.9453, 2.9683, -2.9574, -8.8474, 8.8175, 2.9275, -2.9184, -8.7356, 8.7108, 2.8935, -2.886, -8.6434, 8.6233, 2.8659, -2.8599, -8.5695, 8.5538, 2.8441, -2.8396, -8.5127, 8.5011, 2.8279, -2.8247, -8.4722, 8.4645, 2.817, -2.8151, -8.4474, 8.4436, 2.8113, -2.8107, -8.4379, 8.4379, 2.8107, -2.8113, -8.4436, 8.4474, 2.8151, -2.817, -8.4645, 8.4722, 2.8247, -2.8279, -8.5011, 8.5127, 2.8396, -2.8441, -8.5538, 8.5695, 2.8599, -2.8659, -8.6233, 8.6434, 2.886, -2.8935, -8.7108, 8.7356, 2.9184, -2.9275, -8.8175, 8.8474, 2.9574, -2.9683, -8.9453, 8.9808, 3.0039, -3.0167, -9.0964, 9.1381, 3.0585, -3.0736, -9.2734, 9.3221, 3.1224, -3.1399, -9.4799, 9.5365, 3.1968, -3.2172, -9.72, 9.7858, 3.2833, -3.3071, -9.999, 10.0757, 3.3841, -3.4117, -10.3239, 10.4132, 3.5016, -3.5339, -10.7031, 10.8077, 3.6392, -3.6772, -11.148, }, {46.1999, 45.1197, 44.3589, 43.9039, 43.1423, 42.0882, 41.335, 40.8685, 40.1141, 39.0861, 38.34, 37.8611, 37.1134, 36.1115, 35.3719, 34.8799, 34.1384, 33.1623, 32.4287, 31.9227, 31.1868, 30.2361, 29.5079, 28.9872, 28.2564, 27.3307, 26.6073, 26.0712, 25.3449, 24.4438, 23.7246, 23.1723, 22.45, 21.5729, 20.8575, 20.2885, 19.5696, 18.7161, 18.0038, 17.4175, 16.7015, 15.8712, 15.1615, 14.5574, 13.8438, 13.0361, 12.3286, 11.7061, 10.9945, 10.209, 9.5033, 8.8619, 8.1518, 7.3881, 6.6837, 6.023, 5.3139, 4.5717, 3.868, 3.1876, 2.4791, 1.7579, 1.0547, 0.3541, -0.3541, -1.0547, -1.7579, -2.4791, -3.1876, -3.868, -4.5717, -5.3139, -6.023, -6.6837, -7.3881, -8.1518, -8.8619, -9.5033, -10.209, -10.9945, -11.7061, -12.3286, -13.0361, -13.8438, -14.5574, -15.1615, -15.8712, -16.7015, -17.4175, -18.0038, -18.7161, -19.5696, -20.2885, -20.8575, -21.5729, -22.45, -23.1723, -23.7246, -24.4438, -25.3449, -26.0712, -26.6073, -27.3307, -28.2564, -28.9872, -29.5079, -30.2361, -31.1868, -31.9227, -32.4287, -33.1623, -34.1384, -34.8799, -35.3719, -36.1115, -37.1134, -37.8611, -38.34, -39.0861, -40.1141, -40.8685, -41.335, -42.0882, -43.1423, -43.9039, -44.3589, -45.1197, -46.1999, }, 27.67, @@ -299,7 +299,7 @@ static sensor_info si_ouster_studio_reduced_config_v1 = { "", MODE_2048x10, "", - {64, 16, 2048, {36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, }, {0, 2047}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 2048, {36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, 36, 24, 12, 0, }, {0, 2047}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {1.5, -4.16, 1.49, -4.15, 1.48, -4.15, 1.47, -4.15, 1.46, -4.16, 1.47, -4.18, 1.45, -4.16, 1.45, -4.18, 1.45, -4.19, 1.43, -4.18, 1.43, -4.2, 1.42, -4.2, 1.41, -4.21, 1.41, -4.22, 1.39, -4.21, 1.4, -4.22, 1.39, -4.25, 1.39, -4.24, 1.39, -4.25, 1.38, -4.26, 1.38, -4.27, 1.37, -4.25, 1.37, -4.27, 1.37, -4.28, 1.36, -4.27, 1.37, -4.27, 1.35, -4.29, 1.35, -4.28, 1.34, -4.3, 1.33, -4.3, 1.32, -4.31, 1.34, -4.31, }, {21.45, 20.84, 20.21, 19.6, 18.95, 18.34, 17.69, 17.06, 16.38, 15.76, 15.09, 14.43, 13.74, 13.11, 12.41, 11.77, 11.06, 10.4, 9.68, 9.03, 8.31, 7.64, 6.92, 6.26, 5.53, 4.86, 4.14, 3.46, 2.72, 2.06, 1.33, 0.67, -0.08, -0.77, -1.49, -2.16, -2.88, -3.58, -4.29, -4.96, -5.68, -6.36, -7.07, -7.74, -8.45, -9.13, -9.81, -10.5, -11.19, -11.84, -12.53, -13.19, -13.88, -14.53, -15.21, -15.83, -16.52, -17.14, -17.82, -18.43, -19.1, -19.68, -20.33, -20.92, }, 12.163, @@ -318,7 +318,7 @@ static sensor_info si_2_0_rc2_os_992011000121_32U0_legacy = { "v2.0.0-rc.2", MODE_512x10, "OS-1-32-U0", - {32, 16, 512, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, {0, 511}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {32, 16, 512, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }, {0, 511}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {-4.252, -4.2518, -4.2517, -4.2518, -4.2521, -4.2526, -4.2534, -4.2543, -4.2555, -4.2568, -4.2582, -4.2596, -4.2608, -4.2618, -4.2625, -4.2628, -4.2626, -4.262, -4.2611, -4.2599, -4.2586, -4.2572, -4.2558, -4.2546, -4.2536, -4.2528, -4.2522, -4.2518, -4.2517, -4.2517, -4.2519, -4.2521, }, {20.5477, 19.2919, 18.0171, 16.7242, 15.4141, 14.088, 12.7466, 11.3909, 10.022, 8.641, 7.249, 5.8474, 4.4377, 3.0216, 1.601, 0.1779, -1.2454, -2.6668, -4.0842, -5.4956, -6.8994, -8.2939, -9.6778, -11.0499, -12.409, -13.754, -15.0841, -16.3982, -17.6955, -18.975, -20.2356, -21.4764, }, 15.806, @@ -337,7 +337,7 @@ static sensor_info si_2_0_0_os1_992008000494_128_col_win_legacy = { "v2.0.0", MODE_2048x10, "OS-1-128", - {128, 16, 2048, {48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, }, {2014, 2036}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 2048, {48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, 48, 32, 16, 0, }, {2014, 2036}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {4.2521, 1.4197, -1.4196, -4.252, 4.2519, 1.4196, -1.4196, -4.2518, 4.2517, 1.4196, -1.4195, -4.2517, 4.2517, 1.4196, -1.4196, -4.2518, 4.2518, 1.4197, -1.4197, -4.2521, 4.2522, 1.4198, -1.4199, -4.2526, 4.2528, 1.4201, -1.4202, -4.2534, 4.2536, 1.4204, -1.4205, -4.2543, 4.2546, 1.4208, -1.421, -4.2555, 4.2558, 1.4213, -1.4215, -4.2568, 4.2572, 1.4219, -1.422, -4.2582, 4.2586, 1.4224, -1.4225, -4.2596, 4.2599, 1.4229, -1.423, -4.2608, 4.2611, 1.4234, -1.4234, -4.2618, 4.262, 1.4237, -1.4237, -4.2625, 4.2626, 1.4239, -1.4239, -4.2628, 4.2628, 1.4239, -1.4239, -4.2626, 4.2625, 1.4237, -1.4237, -4.262, 4.2618, 1.4234, -1.4234, -4.2611, 4.2608, 1.423, -1.4229, -4.2599, 4.2596, 1.4225, -1.4224, -4.2586, 4.2582, 1.422, -1.4219, -4.2572, 4.2568, 1.4215, -1.4213, -4.2558, 4.2555, 1.421, -1.4208, -4.2546, 4.2543, 1.4205, -1.4204, -4.2536, 4.2534, 1.4202, -1.4201, -4.2528, 4.2526, 1.4199, -1.4198, -4.2522, 4.2521, 1.4197, -1.4197, -4.2518, 4.2518, 1.4196, -1.4196, -4.2517, 4.2517, 1.4195, -1.4196, -4.2517, 4.2518, 1.4196, -1.4196, -4.2519, 4.252, 1.4196, -1.4197, -4.2521, }, {21.4764, 21.1679, 20.8583, 20.5477, 20.2356, 19.9221, 19.6075, 19.2919, 18.975, 18.6567, 18.3375, 18.0171, 17.6955, 17.3729, 17.0492, 16.7242, 16.3982, 16.0716, 15.7437, 15.4141, 15.0841, 14.7537, 14.4218, 14.088, 13.754, 13.4202, 13.0844, 12.7466, 12.409, 12.0718, 11.7326, 11.3909, 11.0499, 10.7097, 10.3671, 10.022, 9.6778, 9.3348, 8.9892, 8.641, 8.2939, 7.9482, 7.5999, 7.249, 6.8994, 6.5512, 6.2005, 5.8474, 5.4956, 5.1452, 4.7925, 4.4377, 4.0842, 3.7318, 3.3775, 3.0216, 2.6668, 2.3127, 1.9573, 1.601, 1.2454, 0.89, 0.5341, 0.1779, -0.1779, -0.5341, -0.89, -1.2454, -1.601, -1.9573, -2.3127, -2.6668, -3.0216, -3.3775, -3.7318, -4.0842, -4.4377, -4.7925, -5.1452, -5.4956, -5.8474, -6.2005, -6.5512, -6.8994, -7.249, -7.5999, -7.9482, -8.2939, -8.641, -8.9892, -9.3348, -9.6778, -10.022, -10.3671, -10.7097, -11.0499, -11.3909, -11.7326, -12.0718, -12.409, -12.7466, -13.0844, -13.4202, -13.754, -14.088, -14.4218, -14.7537, -15.0841, -15.4141, -15.7437, -16.0716, -16.3982, -16.7242, -17.0492, -17.3729, -17.6955, -18.0171, -18.3375, -18.6567, -18.975, -19.2919, -19.6075, -19.9221, -20.2356, -20.5477, -20.8583, -21.1679, -21.4764, }, 15.806, @@ -356,7 +356,7 @@ static sensor_info si_2_0_0_os1_991913000010_64 = { "v2.0.0", MODE_1024x10, "OS-1-64", - {64, 16, 1024, { 19, 13, 6, 0, 19, 13, 6, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 20, 14, 7, 1}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, { 19, 13, 6, 0, 19, 13, 6, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 20, 14, 7, 1}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, { 3.073, 0.904, -1.261, -3.384, 3.048, 0.9, -1.235, -3.335, 3.029, 0.915, -1.2, -3.301, 3.026, 0.921, -1.179, -3.268, 3.032, 0.933, -1.156, -3.23, 3.028, 0.953, -1.131, -3.209, 3.059, 0.976, -1.112, -3.182, 3.071, 0.99, -1.087, -3.155, 3.097, 1.014, -1.103, -3.133, 3.118, 1.035, -1.039, -3.116, 3.144, 1.061, -1.013, -3.101, 3.178, 1.092, -0.993, -3.089, 3.217, 1.125, -0.972, -3.072, 3.265, 1.159, -0.949, -3.066, 3.321, 1.204, -0.921, -3.055, 3.381, 1.252, -0.898, -3.071}, { 16.729, 16.118, 15.543, 14.995, 14.526, 13.937, 13.381, 12.837, 12.378, 11.801, 11.251, 10.704, 10.251, 9.693, 9.138, 8.603, 8.149, 7.594, 7.049, 6.513, 6.056, 5.504, 4.961, 4.419, 3.967, 3.424, 2.881, 2.328, 1.88, 1.337, 0.787, 0.239, -0.205, -0.756, -1.39, -1.854, -2.3, -2.839, -3.386, -3.935, -4.391, -4.929, -5.472, -6.028, -6.471, -7.02, -7.563, -8.116, -8.572, -9.112, -9.66, -10.227, -10.687, -11.226, -11.777, -12.35, -12.807, -13.347, -13.902, -14.49, -14.968, -15.504, -16.078, -16.679}, 12.163, @@ -365,8 +365,8 @@ static sensor_info si_2_0_0_os1_991913000010_64 = { mkmat4d({ -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1,}), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + 55426, + 55824, }; static sensor_info si_2_1_2_os1_991913000010_64_legacy = { @@ -375,7 +375,7 @@ static sensor_info si_2_1_2_os1_991913000010_64_legacy = { "v2.1.2", MODE_1024x10, "OS-1-64", - {64, 16, 1024, { 19, 13, 6, 0, 19, 13, 6, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 20, 14, 7, 1}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, { 19, 13, 6, 0, 19, 13, 6, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 20, 14, 7, 1}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, { 3.073, 0.904, -1.261, -3.384, 3.048, 0.9, -1.235, -3.335, 3.029, 0.915, -1.2, -3.301, 3.026, 0.921, -1.179, -3.268, 3.032, 0.933, -1.156, -3.23, 3.028, 0.953, -1.131, -3.209, 3.059, 0.976, -1.112, -3.182, 3.071, 0.99, -1.087, -3.155, 3.097, 1.014, -1.103, -3.133, 3.118, 1.035, -1.039, -3.116, 3.144, 1.061, -1.013, -3.101, 3.178, 1.092, -0.993, -3.089, 3.217, 1.125, -0.972, -3.072, 3.265, 1.159, -0.949, -3.066, 3.321, 1.204, -0.921, -3.055, 3.381, 1.252, -0.898, -3.071}, { 16.729, 16.118, 15.543, 14.995, 14.526, 13.937, 13.381, 12.837, 12.378, 11.801, 11.251, 10.704, 10.251, 9.693, 9.138, 8.603, 8.149, 7.594, 7.049, 6.513, 6.056, 5.504, 4.961, 4.419, 3.967, 3.424, 2.881, 2.328, 1.88, 1.337, 0.787, 0.239, -0.205, -0.756, -1.39, -1.854, -2.3, -2.839, -3.386, -3.935, -4.391, -4.929, -5.472, -6.028, -6.471, -7.02, -7.563, -8.116, -8.572, -9.112, -9.66, -10.227, -10.687, -11.226, -11.777, -12.35, -12.807, -13.347, -13.902, -14.49, -14.968, -15.504, -16.078, -16.679}, 12.163, @@ -394,7 +394,7 @@ static sensor_info si_2_1_2_os1_991913000010_64 = { "v2.1.2", MODE_1024x10, "OS-1-64", - {64, 16, 1024, { 19, 13, 6, 0, 19, 13, 6, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 20, 14, 7, 1}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {64, 16, 1024, { 19, 13, 6, 0, 19, 13, 6, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 19, 13, 7, 1, 20, 14, 7, 1}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, { 3.073, 0.904, -1.261, -3.384, 3.048, 0.9, -1.235, -3.335, 3.029, 0.915, -1.2, -3.301, 3.026, 0.921, -1.179, -3.268, 3.032, 0.933, -1.156, -3.23, 3.028, 0.953, -1.131, -3.209, 3.059, 0.976, -1.112, -3.182, 3.071, 0.99, -1.087, -3.155, 3.097, 1.014, -1.103, -3.133, 3.118, 1.035, -1.039, -3.116, 3.144, 1.061, -1.013, -3.101, 3.178, 1.092, -0.993, -3.089, 3.217, 1.125, -0.972, -3.072, 3.265, 1.159, -0.949, -3.066, 3.321, 1.204, -0.921, -3.055, 3.381, 1.252, -0.898, -3.071}, { 16.729, 16.118, 15.543, 14.995, 14.526, 13.937, 13.381, 12.837, 12.378, 11.801, 11.251, 10.704, 10.251, 9.693, 9.138, 8.603, 8.149, 7.594, 7.049, 6.513, 6.056, 5.504, 4.961, 4.419, 3.967, 3.424, 2.881, 2.328, 1.88, 1.337, 0.787, 0.239, -0.205, -0.756, -1.39, -1.854, -2.3, -2.839, -3.386, -3.935, -4.391, -4.929, -5.472, -6.028, -6.471, -7.02, -7.563, -8.116, -8.572, -9.112, -9.66, -10.227, -10.687, -11.226, -11.777, -12.35, -12.807, -13.347, -13.902, -14.49, -14.968, -15.504, -16.078, -16.679}, 12.163, @@ -403,8 +403,8 @@ static sensor_info si_2_1_2_os1_991913000010_64 = { mkmat4d({ -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1,}), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + 7502, + 7503, }; static sensor_info si_2_2_os_992119000444_128_legacy = { @@ -413,7 +413,7 @@ static sensor_info si_2_2_os_992119000444_128_legacy = { "v2.2.0-rc.1", MODE_1024x10, "OS-1-128", - {128, 16, 1024, {24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, {24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {4.26, 1.44, -1.39, -4.22, 4.27, 1.45, -1.38, -4.22, 4.26, 1.45, -1.38, -4.2, 4.25, 1.43, -1.4, -4.22, 4.28, 1.44, -1.4, -4.22, 4.27, 1.45, -1.4, -4.22, 4.27, 1.44, -1.39, -4.24, 4.26, 1.43, -1.39, -4.24, 4.26, 1.44, -1.4, -4.24, 4.25, 1.4, -1.4, -4.25, 4.25, 1.44, -1.41, -4.25, 4.28, 1.42, -1.4, -4.24, 4.26, 1.41, -1.43, -4.25, 4.26, 1.41, -1.42, -4.24, 4.24, 1.43, -1.42, -4.26, 4.27, 1.42, -1.42, -4.27, 4.26, 1.41, -1.45, -4.24, 4.24, 1.42, -1.41, -4.24, 4.25, 1.42, -1.43, -4.25, 4.27, 1.39, -1.43, -4.26, 4.23, 1.39, -1.44, -4.27, 4.26, 1.41, -1.43, -4.26, 4.24, 1.4, -1.42, -4.28, 4.24, 1.42, -1.43, -4.26, 4.24, 1.4, -1.43, -4.26, 4.22, 1.4, -1.43, -4.27, 4.24, 1.41, -1.44, -4.28, 4.24, 1.4, -1.45, -4.29, 4.23, 1.39, -1.45, -4.27, 4.22, 1.38, -1.45, -4.27, 4.2, 1.39, -1.45, -4.29, 4.22, 1.39, -1.45, -4.29}, {21.51, 21.18, 20.87, 20.57, 20.26, 19.94, 19.63, 19.32, 19.01, 18.68, 18.35, 18.05, 17.72, 17.38, 17.06, 16.74, 16.42, 16.09, 15.76, 15.44, 15.09, 14.77, 14.43, 14.1, 13.75, 13.42, 13.1, 12.77, 12.41, 12.06, 11.73, 11.4, 11.04, 10.71, 10.36, 10.03, 9.67, 9.32, 9.01, 8.65, 8.28, 7.94, 7.59, 7.26, 6.91, 6.53, 6.21, 5.87, 5.5, 5.15, 4.8, 4.46, 4.11, 3.74, 3.39, 3.06, 2.68, 2.34, 1.99, 1.62, 1.27, 0.92, 0.58, 0.21, -0.14, -0.5, -0.86, -1.18, -1.55, -1.91, -2.25, -2.59, -2.98, -3.33, -3.68, -4.01, -4.38, -4.74, -5.08, -5.44, -5.8, -6.14, -6.48, -6.83, -7.19, -7.54, -7.88, -8.22, -8.59, -8.94, -9.26, -9.63, -9.97, -10.3, -10.65, -10.97, -11.33, -11.68, -12.02, -12.36, -12.72, -13.06, -13.37, -13.7, -14.05, -14.39, -14.71, -15.06, -15.39, -15.72, -16.04, -16.38, -16.7, -17.03, -17.34, -17.67, -18.01, -18.33, -18.64, -18.93, -19.29, -19.59, -19.89, -20.2, -20.51, -20.82, -21.14, -21.44}, 15.806, @@ -432,7 +432,7 @@ static sensor_info si_2_2_os_992119000444_128 = { "v2.2.0-rc.1", MODE_1024x10, "OS-1-128", - {128, 16, 1024, {24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, {24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0}, {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, {4.26, 1.44, -1.39, -4.22, 4.27, 1.45, -1.38, -4.22, 4.26, 1.45, -1.38, -4.2, 4.25, 1.43, -1.4, -4.22, 4.28, 1.44, -1.4, -4.22, 4.27, 1.45, -1.4, -4.22, 4.27, 1.44, -1.39, -4.24, 4.26, 1.43, -1.39, -4.24, 4.26, 1.44, -1.4, -4.24, 4.25, 1.4, -1.4, -4.25, 4.25, 1.44, -1.41, -4.25, 4.28, 1.42, -1.4, -4.24, 4.26, 1.41, -1.43, -4.25, 4.26, 1.41, -1.42, -4.24, 4.24, 1.43, -1.42, -4.26, 4.27, 1.42, -1.42, -4.27, 4.26, 1.41, -1.45, -4.24, 4.24, 1.42, -1.41, -4.24, 4.25, 1.42, -1.43, -4.25, 4.27, 1.39, -1.43, -4.26, 4.23, 1.39, -1.44, -4.27, 4.26, 1.41, -1.43, -4.26, 4.24, 1.4, -1.42, -4.28, 4.24, 1.42, -1.43, -4.26, 4.24, 1.4, -1.43, -4.26, 4.22, 1.4, -1.43, -4.27, 4.24, 1.41, -1.44, -4.28, 4.24, 1.4, -1.45, -4.29, 4.23, 1.39, -1.45, -4.27, 4.22, 1.38, -1.45, -4.27, 4.2, 1.39, -1.45, -4.29, 4.22, 1.39, -1.45, -4.29}, {21.51, 21.18, 20.87, 20.57, 20.26, 19.94, 19.63, 19.32, 19.01, 18.68, 18.35, 18.05, 17.72, 17.38, 17.06, 16.74, 16.42, 16.09, 15.76, 15.44, 15.09, 14.77, 14.43, 14.1, 13.75, 13.42, 13.1, 12.77, 12.41, 12.06, 11.73, 11.4, 11.04, 10.71, 10.36, 10.03, 9.67, 9.32, 9.01, 8.65, 8.28, 7.94, 7.59, 7.26, 6.91, 6.53, 6.21, 5.87, 5.5, 5.15, 4.8, 4.46, 4.11, 3.74, 3.39, 3.06, 2.68, 2.34, 1.99, 1.62, 1.27, 0.92, 0.58, 0.21, -0.14, -0.5, -0.86, -1.18, -1.55, -1.91, -2.25, -2.59, -2.98, -3.33, -3.68, -4.01, -4.38, -4.74, -5.08, -5.44, -5.8, -6.14, -6.48, -6.83, -7.19, -7.54, -7.88, -8.22, -8.59, -8.94, -9.26, -9.63, -9.97, -10.3, -10.65, -10.97, -11.33, -11.68, -12.02, -12.36, -12.72, -13.06, -13.37, -13.7, -14.05, -14.39, -14.71, -15.06, -15.39, -15.72, -16.04, -16.38, -16.7, -17.03, -17.34, -17.67, -18.01, -18.33, -18.64, -18.93, -19.29, -19.59, -19.89, -20.2, -20.51, -20.82, -21.14, -21.44}, 15.806, @@ -451,7 +451,7 @@ static sensor_info si_2_3_os_992146000760_128_legacy = { "v2.3.0-rc.2", MODE_1024x10, "OS-1-128", - {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, 15.806, @@ -470,7 +470,7 @@ static sensor_info si_2_3_os_992146000760_128 = { "v2.3.0-rc.2", MODE_1024x10, "OS-1-128", - {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, 15.806, @@ -489,7 +489,7 @@ static sensor_info si_2_3_1_os_992146000760_128_legacy = { "v2.3.1-rc.1", MODE_1024x10, "OS-1-128", - {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, 15.806, @@ -508,7 +508,7 @@ static sensor_info si_2_3_1_os_992146000760_128 = { "v2.3.1-rc.1", MODE_1024x10, "OS-1-128", - {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY}, + {128, 16, 1024, { 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0, 24, 16, 8, 0} , {0, 1023}, PROFILE_LIDAR_LEGACY, PROFILE_IMU_LEGACY, 10}, { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, 15.806, @@ -527,7 +527,7 @@ static sensor_info si_2_4_0_os_992146000760_128 = { "v2.4.0", MODE_1024x10, "OS-1-128", - {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12} , {0, 1023}, PROFILE_RNG19_RFL8_SIG16_NIR16, PROFILE_IMU_LEGACY}, + {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12} , {0, 1023}, PROFILE_RNG19_RFL8_SIG16_NIR16, PROFILE_IMU_LEGACY, 10}, { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, 15.806, @@ -546,7 +546,7 @@ static sensor_info si_2_4_0_os_992146000760_128_legacy = { "v2.4.0", MODE_1024x10, "OS-1-128", - {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12} , {0, 1023}, PROFILE_RNG19_RFL8_SIG16_NIR16, PROFILE_IMU_LEGACY}, + {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12} , {0, 1023}, PROFILE_RNG19_RFL8_SIG16_NIR16, PROFILE_IMU_LEGACY, 10}, { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, 15.806, @@ -559,6 +559,85 @@ static sensor_info si_2_4_0_os_992146000760_128_legacy = { 7503, }; +static sensor_info si_3_0_1_os_122246000293_128_legacy = { + "", + "122246000293", + "v3.0.1-rc.1", + MODE_1024x10, + "OS-1-128", + {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12}, {0, 1023}, PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, PROFILE_IMU_LEGACY, 10}, + { 4.2, 1.42, -1.37, -4.14, 4.22, 1.44, -1.37, -4.15, 4.24, 1.43, -1.36, -4.15, 4.24, 1.43, -1.36, -4.16, 4.23, 1.42, -1.37, -4.17, 4.25, 1.43, -1.37, -4.16, 4.24, 1.42, -1.38, -4.18, 4.24, 1.42, -1.38, -4.18, 4.24, 1.41, -1.38, -4.18, 4.23, 1.41, -1.39, -4.19, 4.22, 1.4, -1.39, -4.2, 4.21, 1.4, -1.4, -4.2, 4.2, 1.41, -1.4, -4.21, 4.2, 1.4, -1.41, -4.21, 4.2, 1.4, -1.41, -4.21, 4.2, 1.4, -1.42, -4.21, 4.2, 1.4, -1.42, -4.22, 4.2, 1.39, -1.41, -4.22, 4.2, 1.38, -1.41, -4.23, 4.2, 1.38, -1.41, -4.24, 4.2, 1.39, -1.42, -4.23, 4.2, 1.38, -1.43, -4.22, 4.2, 1.38, -1.42, -4.23, 4.19, 1.38, -1.42, -4.23, 4.2, 1.39, -1.43, -4.22, 4.2, 1.38, -1.42, -4.23, 4.19, 1.39, -1.43, -4.24, 4.19, 1.38, -1.43, -4.23, 4.2, 1.38, -1.42, -4.24, 4.2, 1.38, -1.43, -4.24, 4.19, 1.37, -1.43, -4.24, 4.19, 1.37, -1.44, -4.25}, + { 20.47, 20.21, 19.89, 19.54, 19.23, 18.97, 18.66, 18.31, 18, 17.72, 17.4, 17.05, 16.74, 16.45, 16.13, 15.77, 15.45, 15.15, 14.83, 14.47, 14.15, 13.85, 13.52, 13.17, 12.83, 12.52, 12.19, 11.83, 11.5, 11.18, 10.85, 10.5, 10.14, 9.82, 9.49, 9.14, 8.79, 8.45, 8.13, 7.76, 7.42, 7.08, 6.75, 6.39, 6.04, 5.7, 5.35, 5, 4.64, 4.32, 3.98, 3.61, 3.25, 2.92, 2.57, 2.22, 1.85, 1.52, 1.17, 0.82, 0.45, 0.12, -0.24, -0.58, -0.95, -1.28, -1.64, -1.97, -2.35, -2.69, -3.03, -3.37, -3.74, -4.1, -4.43, -4.77, -5.14, -5.49, -5.82, -6.17, -6.53, -6.88, -7.22, -7.55, -7.91, -8.27, -8.6, -8.92, -9.28, -9.64, -9.97, -10.29, -10.65, -11.01, -11.33, -11.64, -12.01, -12.35, -12.69, -12.98, -13.34, -13.69, -14.03, -14.32, -14.68, -15.02, -15.35, -15.63, -15.98, -16.34, -16.65, -16.93, -17.27, -17.63, -17.93, -18.21, -18.55, -18.9, -19.2, -19.48, -19.81, -20.16, -20.46, -20.72, -21.05, -21.39, -21.68, -21.95}, + 16.721, + mkmat4d({ 1, 0, 0, 16.721, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}), + mkmat4d({1, 0, 0, -2.441, 0, 1, 0, -9.725, 0, 0, 1, 7.533, 0, 0, 0, 1}), + mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 38.195, 0, 0, 0, 1}), + mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), + 1629346, + 7502, + 7503, +}; + + +static sensor_info si_3_0_1_os_122246000293_128 = { + "", + "122246000293", + "v3.0.1-rc.1", + MODE_1024x10, + "OS-1-128", + {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12}, {0, 1023}, PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, PROFILE_IMU_LEGACY, 10}, + { 4.2, 1.42, -1.37, -4.14, 4.22, 1.44, -1.37, -4.15, 4.24, 1.43, -1.36, -4.15, 4.24, 1.43, -1.36, -4.16, 4.23, 1.42, -1.37, -4.17, 4.25, 1.43, -1.37, -4.16, 4.24, 1.42, -1.38, -4.18, 4.24, 1.42, -1.38, -4.18, 4.24, 1.41, -1.38, -4.18, 4.23, 1.41, -1.39, -4.19, 4.22, 1.4, -1.39, -4.2, 4.21, 1.4, -1.4, -4.2, 4.2, 1.41, -1.4, -4.21, 4.2, 1.4, -1.41, -4.21, 4.2, 1.4, -1.41, -4.21, 4.2, 1.4, -1.42, -4.21, 4.2, 1.4, -1.42, -4.22, 4.2, 1.39, -1.41, -4.22, 4.2, 1.38, -1.41, -4.23, 4.2, 1.38, -1.41, -4.24, 4.2, 1.39, -1.42, -4.23, 4.2, 1.38, -1.43, -4.22, 4.2, 1.38, -1.42, -4.23, 4.19, 1.38, -1.42, -4.23, 4.2, 1.39, -1.43, -4.22, 4.2, 1.38, -1.42, -4.23, 4.19, 1.39, -1.43, -4.24, 4.19, 1.38, -1.43, -4.23, 4.2, 1.38, -1.42, -4.24, 4.2, 1.38, -1.43, -4.24, 4.19, 1.37, -1.43, -4.24, 4.19, 1.37, -1.44, -4.25}, + { 20.47, 20.21, 19.89, 19.54, 19.23, 18.97, 18.66, 18.31, 18, 17.72, 17.4, 17.05, 16.74, 16.45, 16.13, 15.77, 15.45, 15.15, 14.83, 14.47, 14.15, 13.85, 13.52, 13.17, 12.83, 12.52, 12.19, 11.83, 11.5, 11.18, 10.85, 10.5, 10.14, 9.82, 9.49, 9.14, 8.79, 8.45, 8.13, 7.76, 7.42, 7.08, 6.75, 6.39, 6.04, 5.7, 5.35, 5, 4.64, 4.32, 3.98, 3.61, 3.25, 2.92, 2.57, 2.22, 1.85, 1.52, 1.17, 0.82, 0.45, 0.12, -0.24, -0.58, -0.95, -1.28, -1.64, -1.97, -2.35, -2.69, -3.03, -3.37, -3.74, -4.1, -4.43, -4.77, -5.14, -5.49, -5.82, -6.17, -6.53, -6.88, -7.22, -7.55, -7.91, -8.27, -8.6, -8.92, -9.28, -9.64, -9.97, -10.29, -10.65, -11.01, -11.33, -11.64, -12.01, -12.35, -12.69, -12.98, -13.34, -13.69, -14.03, -14.32, -14.68, -15.02, -15.35, -15.63, -15.98, -16.34, -16.65, -16.93, -17.27, -17.63, -17.93, -18.21, -18.55, -18.9, -19.2, -19.48, -19.81, -20.16, -20.46, -20.72, -21.05, -21.39, -21.68, -21.95}, + 16.721, + mkmat4d({ 1, 0, 0, 16.721, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}), + mkmat4d({1, 0, 0, -2.441, 0, 1, 0, -9.725, 0, 0, 1, 7.533, 0, 0, 0, 1}), + mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 38.195, 0, 0, 0, 1}), + mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), + 1629346, + 7502, + 7503, +}; + + +static sensor_info si_2_5_0_os_992146000760_128 = { + "", + "992146000760", + "v2.5.0-omega.8", + MODE_1024x10, + "OS-1-128", + {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12} , {912, 1022}, PROFILE_RNG19_RFL8_SIG16_NIR16, PROFILE_IMU_LEGACY, 10}, + { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, + { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, + 15.806, + mkmat4d({1, 0, 0, 15.806, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1,}), + mkmat4d({ 1, 0, 0, 6.253, 0, 1, 0, -11.775, 0, 0, 1, 7.645, 0, 0, 0, 1}), + mkmat4d({ -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1}), + mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), + 8247301, + 7502, + 7503, +}; + +static sensor_info si_2_5_0_os_992146000760_128_legacy = { + "", + "992146000760", + "v2.5.0-omega.8", + MODE_1024x10, + "OS-1-128", + {128, 16, 1024, { 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12, 12, 4, -4, -12} , {912, 1022}, PROFILE_RNG19_RFL8_SIG16_NIR16, PROFILE_IMU_LEGACY, 10}, + { 4.27, 1.43, -1.39, -4.23, 4.26, 1.45, -1.38, -4.22, 4.28, 1.43, -1.39, -4.24, 4.27, 1.44, -1.4, -4.22, 4.28, 1.45, -1.39, -4.23, 4.26, 1.44, -1.4, -4.23, 4.27, 1.43, -1.39, -4.23, 4.26, 1.43, -1.4, -4.24, 4.26, 1.44, -1.4, -4.23, 4.28, 1.43, -1.4, -4.24, 4.26, 1.42, -1.39, -4.24, 4.27, 1.43, -1.42, -4.25, 4.26, 1.43, -1.42, -4.25, 4.28, 1.43, -1.42, -4.25, 4.26, 1.43, -1.44, -4.25, 4.27, 1.43, -1.43, -4.26, 4.27, 1.42, -1.43, -4.26, 4.25, 1.42, -1.43, -4.28, 4.25, 1.42, -1.45, -4.28, 4.26, 1.42, -1.44, -4.26, 4.26, 1.4, -1.46, -4.27, 4.24, 1.41, -1.43, -4.28, 4.26, 1.4, -1.44, -4.28, 4.22, 1.4, -1.43, -4.29, 4.25, 1.41, -1.45, -4.29, 4.24, 1.4, -1.46, -4.28, 4.24, 1.4, -1.44, -4.28, 4.22, 1.41, -1.45, -4.28, 4.25, 1.39, -1.45, -4.29, 4.23, 1.4, -1.45, -4.3, 4.22, 1.39, -1.47, -4.29, 4.22, 1.38, -1.47, -4.3 }, + { 21.34, 21.03, 20.72, 20.41, 20.08, 19.78, 19.48, 19.17, 18.83, 18.51, 18.21, 17.88, 17.56, 17.23, 16.92, 16.59, 16.26, 15.94, 15.61, 15.28, 14.93, 14.6, 14.27, 13.95, 13.61, 13.26, 12.94, 12.6, 12.26, 11.91, 11.58, 11.24, 10.89, 10.55, 10.21, 9.88, 9.52, 9.18, 8.83, 8.5, 8.14, 7.78, 7.46, 7.11, 6.75, 6.39, 6.05, 5.7, 5.33, 4.99, 4.64, 4.29, 3.94, 3.59, 3.24, 2.88, 2.52, 2.18, 1.81, 1.48, 1.11, 0.76, 0.4, 0.05, -0.3, -0.67, -1.01, -1.37, -1.75, -2.08, -2.43, -2.79, -3.15, -3.5, -3.86, -4.2, -4.56, -4.9, -5.27, -5.6, -5.96, -6.32, -6.68, -7, -7.38, -7.71, -8.05, -8.41, -8.77, -9.11, -9.45, -9.79, -10.16, -10.49, -10.83, -11.18, -11.54, -11.86, -12.2, -12.55, -12.91, -13.24, -13.58, -13.89, -14.26, -14.58, -14.89, -15.21, -15.6, -15.89, -16.22, -16.54, -16.88, -17.21, -17.52, -17.85, -18.2, -18.5, -18.82, -19.14, -19.47, -19.78, -20.09, -20.38, -20.72, -21.03, -21.33, -21.62 }, + 15.806, + mkmat4d({1, 0, 0, 15.806, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1,}), + mkmat4d({ 1, 0, 0, 6.253, 0, 1, 0, -11.775, 0, 0, 1, 7.645, 0, 0, 0, 1}), + mkmat4d({ -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1}), + mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), + 8247301, + 7502, + 7503, +}; + + // for lookup by name const std::map expected_sensor_infos{ {"1_12_os1-991913000010-64", si_1_12_os1_991913000010_64}, @@ -589,6 +668,10 @@ const std::map expected_sensor_infos{ {"2_3_1_os-992146000760-128", si_2_3_1_os_992146000760_128}, {"2_4_0_os-992146000760-128", si_2_4_0_os_992146000760_128}, {"2_4_0_os-992146000760-128_legacy", si_2_4_0_os_992146000760_128_legacy}, + {"3_0_1_os-122246000293-128", si_3_0_1_os_122246000293_128}, + {"3_0_1_os-122246000293-128_legacy", si_3_0_1_os_122246000293_128_legacy}, + {"2_5_0_os-992146000760-128", si_2_5_0_os_992146000760_128}, + {"2_5_0_os-992146000760-128_legacy", si_2_5_0_os_992146000760_128_legacy}, }; // clang-format on diff --git a/tests/lidar_scan_test.cpp b/tests/lidar_scan_test.cpp index e61ef8bc..f3d68a9e 100644 --- a/tests/lidar_scan_test.cpp +++ b/tests/lidar_scan_test.cpp @@ -84,8 +84,6 @@ TEST(LidarScan, EmptyConstructorInit) { EXPECT_EQ(scan.frame_id, -1); - EXPECT_EQ(scan.headers.size(), 0u); - EXPECT_EQ(scan.end() - scan.begin(), 0); zero_check_fields(scan); diff --git a/tests/metadata/2_5_0_os-992146000760-128.json b/tests/metadata/2_5_0_os-992146000760-128.json new file mode 100644 index 00000000..10878f07 --- /dev/null +++ b/tests/metadata/2_5_0_os-992146000760-128.json @@ -0,0 +1,525 @@ +{ + "beam_intrinsics": + { + "beam_altitude_angles": + [ + 21.34, + 21.03, + 20.72, + 20.41, + 20.08, + 19.78, + 19.48, + 19.17, + 18.83, + 18.51, + 18.21, + 17.88, + 17.56, + 17.23, + 16.92, + 16.59, + 16.26, + 15.94, + 15.61, + 15.28, + 14.93, + 14.6, + 14.27, + 13.95, + 13.61, + 13.26, + 12.94, + 12.6, + 12.26, + 11.91, + 11.58, + 11.24, + 10.89, + 10.55, + 10.21, + 9.88, + 9.52, + 9.18, + 8.83, + 8.5, + 8.14, + 7.78, + 7.46, + 7.11, + 6.75, + 6.39, + 6.05, + 5.7, + 5.33, + 4.99, + 4.64, + 4.29, + 3.94, + 3.59, + 3.24, + 2.88, + 2.52, + 2.18, + 1.81, + 1.48, + 1.11, + 0.76, + 0.4, + 0.05, + -0.3, + -0.67, + -1.01, + -1.37, + -1.75, + -2.08, + -2.43, + -2.79, + -3.15, + -3.5, + -3.86, + -4.2, + -4.56, + -4.9, + -5.27, + -5.6, + -5.96, + -6.32, + -6.68, + -7, + -7.38, + -7.71, + -8.05, + -8.41, + -8.77, + -9.11, + -9.45, + -9.79, + -10.16, + -10.49, + -10.83, + -11.18, + -11.54, + -11.86, + -12.2, + -12.55, + -12.91, + -13.24, + -13.58, + -13.89, + -14.26, + -14.58, + -14.89, + -15.21, + -15.6, + -15.89, + -16.22, + -16.54, + -16.88, + -17.21, + -17.52, + -17.85, + -18.2, + -18.5, + -18.82, + -19.14, + -19.47, + -19.78, + -20.09, + -20.38, + -20.72, + -21.03, + -21.33, + -21.62 + ], + "beam_azimuth_angles": + [ + 4.27, + 1.43, + -1.39, + -4.23, + 4.26, + 1.45, + -1.38, + -4.22, + 4.28, + 1.43, + -1.39, + -4.24, + 4.27, + 1.44, + -1.4, + -4.22, + 4.28, + 1.45, + -1.39, + -4.23, + 4.26, + 1.44, + -1.4, + -4.23, + 4.27, + 1.43, + -1.39, + -4.23, + 4.26, + 1.43, + -1.4, + -4.24, + 4.26, + 1.44, + -1.4, + -4.23, + 4.28, + 1.43, + -1.4, + -4.24, + 4.26, + 1.42, + -1.39, + -4.24, + 4.27, + 1.43, + -1.42, + -4.25, + 4.26, + 1.43, + -1.42, + -4.25, + 4.28, + 1.43, + -1.42, + -4.25, + 4.26, + 1.43, + -1.44, + -4.25, + 4.27, + 1.43, + -1.43, + -4.26, + 4.27, + 1.42, + -1.43, + -4.26, + 4.25, + 1.42, + -1.43, + -4.28, + 4.25, + 1.42, + -1.45, + -4.28, + 4.26, + 1.42, + -1.44, + -4.26, + 4.26, + 1.4, + -1.46, + -4.27, + 4.24, + 1.41, + -1.43, + -4.28, + 4.26, + 1.4, + -1.44, + -4.28, + 4.22, + 1.4, + -1.43, + -4.29, + 4.25, + 1.41, + -1.45, + -4.29, + 4.24, + 1.4, + -1.46, + -4.28, + 4.24, + 1.4, + -1.44, + -4.28, + 4.22, + 1.41, + -1.45, + -4.28, + 4.25, + 1.39, + -1.45, + -4.29, + 4.23, + 1.4, + -1.45, + -4.3, + 4.22, + 1.39, + -1.47, + -4.29, + 4.22, + 1.38, + -1.47, + -4.3 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 15.806, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "lidar_origin_to_beam_origin_mm": 15.806 + }, + "calibration_status": + { + "reflectivity": + { + "timestamp": "2021-11-23T05:37:45", + "valid": true + } + }, + "client_version": "ouster_client 0.8.0dev0", + "config_params": + { + "azimuth_window": + [ + 583, + 39402 + ], + "columns_per_packet": 16, + "lidar_mode": "1024x10", + "multipurpose_io_mode": "OFF", + "nmea_baud_rate": "BAUD_9600", + "nmea_ignore_valid_char": 0, + "nmea_in_polarity": "ACTIVE_HIGH", + "nmea_leap_seconds": 0, + "operating_mode": "NORMAL", + "phase_lock_enable": false, + "phase_lock_offset": 0, + "signal_multiplier": 1, + "sync_pulse_in_polarity": "ACTIVE_HIGH", + "sync_pulse_out_angle": 360, + "sync_pulse_out_frequency": 1, + "sync_pulse_out_polarity": "ACTIVE_HIGH", + "sync_pulse_out_pulse_width": 10, + "timestamp_mode": "TIME_FROM_INTERNAL_OSC", + "udp_dest": "10.0.0.167", + "udp_port_imu": 7503, + "udp_port_lidar": 7502, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16" + }, + "imu_intrinsics": + { + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + 6.253, + 0, + 1, + 0, + -11.775, + 0, + 0, + 1, + 7.645, + 0, + 0, + 0, + 1 + ] + }, + "lidar_data_format": + { + "column_window": + [ + 912, + 1022 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16" + }, + "lidar_intrinsics": + { + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 36.18, + 0, + 0, + 0, + 1 + ] + }, + "sensor_info": + { + "build_date": "2023-03-18T18:28:26Z", + "build_rev": "v2.5.0-omega.8", + "image_rev": "ousteros-image-prod-aries-v2.5.0-omega.8+20230318213158.staging", + "initialization_id": 8247301, + "prod_line": "OS-1-128", + "prod_pn": "840-103575-06", + "prod_sn": "992146000760", + "status": "RUNNING" + } +} diff --git a/tests/metadata/2_5_0_os-992146000760-128_legacy.json b/tests/metadata/2_5_0_os-992146000760-128_legacy.json new file mode 100644 index 00000000..9ddc8f13 --- /dev/null +++ b/tests/metadata/2_5_0_os-992146000760-128_legacy.json @@ -0,0 +1,480 @@ +{ + "beam_altitude_angles": + [ + 21.34, + 21.03, + 20.72, + 20.41, + 20.08, + 19.78, + 19.48, + 19.17, + 18.83, + 18.51, + 18.21, + 17.88, + 17.56, + 17.23, + 16.92, + 16.59, + 16.26, + 15.94, + 15.61, + 15.28, + 14.93, + 14.6, + 14.27, + 13.95, + 13.61, + 13.26, + 12.94, + 12.6, + 12.26, + 11.91, + 11.58, + 11.24, + 10.89, + 10.55, + 10.21, + 9.88, + 9.52, + 9.18, + 8.83, + 8.5, + 8.14, + 7.78, + 7.46, + 7.11, + 6.75, + 6.39, + 6.05, + 5.7, + 5.33, + 4.99, + 4.64, + 4.29, + 3.94, + 3.59, + 3.24, + 2.88, + 2.52, + 2.18, + 1.81, + 1.48, + 1.11, + 0.76, + 0.4, + 0.05, + -0.3, + -0.67, + -1.01, + -1.37, + -1.75, + -2.08, + -2.43, + -2.79, + -3.15, + -3.5, + -3.86, + -4.2, + -4.56, + -4.9, + -5.27, + -5.6, + -5.96, + -6.32, + -6.68, + -7, + -7.38, + -7.71, + -8.05, + -8.41, + -8.77, + -9.11, + -9.45, + -9.79, + -10.16, + -10.49, + -10.83, + -11.18, + -11.54, + -11.86, + -12.2, + -12.55, + -12.91, + -13.24, + -13.58, + -13.89, + -14.26, + -14.58, + -14.89, + -15.21, + -15.6, + -15.89, + -16.22, + -16.54, + -16.88, + -17.21, + -17.52, + -17.85, + -18.2, + -18.5, + -18.82, + -19.14, + -19.47, + -19.78, + -20.09, + -20.38, + -20.72, + -21.03, + -21.33, + -21.62 + ], + "beam_azimuth_angles": + [ + 4.27, + 1.43, + -1.39, + -4.23, + 4.26, + 1.45, + -1.38, + -4.22, + 4.28, + 1.43, + -1.39, + -4.24, + 4.27, + 1.44, + -1.4, + -4.22, + 4.28, + 1.45, + -1.39, + -4.23, + 4.26, + 1.44, + -1.4, + -4.23, + 4.27, + 1.43, + -1.39, + -4.23, + 4.26, + 1.43, + -1.4, + -4.24, + 4.26, + 1.44, + -1.4, + -4.23, + 4.28, + 1.43, + -1.4, + -4.24, + 4.26, + 1.42, + -1.39, + -4.24, + 4.27, + 1.43, + -1.42, + -4.25, + 4.26, + 1.43, + -1.42, + -4.25, + 4.28, + 1.43, + -1.42, + -4.25, + 4.26, + 1.43, + -1.44, + -4.25, + 4.27, + 1.43, + -1.43, + -4.26, + 4.27, + 1.42, + -1.43, + -4.26, + 4.25, + 1.42, + -1.43, + -4.28, + 4.25, + 1.42, + -1.45, + -4.28, + 4.26, + 1.42, + -1.44, + -4.26, + 4.26, + 1.4, + -1.46, + -4.27, + 4.24, + 1.41, + -1.43, + -4.28, + 4.26, + 1.4, + -1.44, + -4.28, + 4.22, + 1.4, + -1.43, + -4.29, + 4.25, + 1.41, + -1.45, + -4.29, + 4.24, + 1.4, + -1.46, + -4.28, + 4.24, + 1.4, + -1.44, + -4.28, + 4.22, + 1.41, + -1.45, + -4.28, + 4.25, + 1.39, + -1.45, + -4.29, + 4.23, + 1.4, + -1.45, + -4.3, + 4.22, + 1.39, + -1.47, + -4.29, + 4.22, + 1.38, + -1.47, + -4.3 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 15.806, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "build_date": "2023-03-18T18:28:26Z", + "build_rev": "v2.5.0-omega.8", + "client_version": "ouster_client 0.8.0dev0", + "data_format": + { + "column_window": + [ + 912, + 1022 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16" + }, + "hostname": "", + "image_rev": "ousteros-image-prod-aries-v2.5.0-omega.8+20230318213158.staging", + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + 6.253, + 0, + 1, + 0, + -11.775, + 0, + 0, + 1, + 7.645, + 0, + 0, + 0, + 1 + ], + "initialization_id": 8247301, + "json_calibration_version": 4, + "lidar_mode": "1024x10", + "lidar_origin_to_beam_origin_mm": 15.806, + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 36.18, + 0, + 0, + 0, + 1 + ], + "prod_line": "OS-1-128", + "prod_pn": "840-103575-06", + "prod_sn": "992146000760", + "status": "RUNNING", + "udp_port_imu": 7503, + "udp_port_lidar": 7502 +} diff --git a/tests/metadata/3_0_1_os-122246000293-128.json b/tests/metadata/3_0_1_os-122246000293-128.json new file mode 100644 index 00000000..f58bbed0 --- /dev/null +++ b/tests/metadata/3_0_1_os-122246000293-128.json @@ -0,0 +1,525 @@ +{ + "beam_intrinsics": + { + "beam_altitude_angles": + [ + 20.47, + 20.21, + 19.89, + 19.54, + 19.23, + 18.97, + 18.66, + 18.31, + 18, + 17.72, + 17.4, + 17.05, + 16.74, + 16.45, + 16.13, + 15.77, + 15.45, + 15.15, + 14.83, + 14.47, + 14.15, + 13.85, + 13.52, + 13.17, + 12.83, + 12.52, + 12.19, + 11.83, + 11.5, + 11.18, + 10.85, + 10.5, + 10.14, + 9.82, + 9.49, + 9.14, + 8.79, + 8.45, + 8.13, + 7.76, + 7.42, + 7.08, + 6.75, + 6.39, + 6.04, + 5.7, + 5.35, + 5, + 4.64, + 4.32, + 3.98, + 3.61, + 3.25, + 2.92, + 2.57, + 2.22, + 1.85, + 1.52, + 1.17, + 0.82, + 0.45, + 0.12, + -0.24, + -0.58, + -0.95, + -1.28, + -1.64, + -1.97, + -2.35, + -2.69, + -3.03, + -3.37, + -3.74, + -4.1, + -4.43, + -4.77, + -5.14, + -5.49, + -5.82, + -6.17, + -6.53, + -6.88, + -7.22, + -7.55, + -7.91, + -8.27, + -8.6, + -8.92, + -9.28, + -9.64, + -9.97, + -10.29, + -10.65, + -11.01, + -11.33, + -11.64, + -12.01, + -12.35, + -12.69, + -12.98, + -13.34, + -13.69, + -14.03, + -14.32, + -14.68, + -15.02, + -15.35, + -15.63, + -15.98, + -16.34, + -16.65, + -16.93, + -17.27, + -17.63, + -17.93, + -18.21, + -18.55, + -18.9, + -19.2, + -19.48, + -19.81, + -20.16, + -20.46, + -20.72, + -21.05, + -21.39, + -21.68, + -21.95 + ], + "beam_azimuth_angles": + [ + 4.2, + 1.42, + -1.37, + -4.14, + 4.22, + 1.44, + -1.37, + -4.15, + 4.24, + 1.43, + -1.36, + -4.15, + 4.24, + 1.43, + -1.36, + -4.16, + 4.23, + 1.42, + -1.37, + -4.17, + 4.25, + 1.43, + -1.37, + -4.16, + 4.24, + 1.42, + -1.38, + -4.18, + 4.24, + 1.42, + -1.38, + -4.18, + 4.24, + 1.41, + -1.38, + -4.18, + 4.23, + 1.41, + -1.39, + -4.19, + 4.22, + 1.4, + -1.39, + -4.2, + 4.21, + 1.4, + -1.4, + -4.2, + 4.2, + 1.41, + -1.4, + -4.21, + 4.2, + 1.4, + -1.41, + -4.21, + 4.2, + 1.4, + -1.41, + -4.21, + 4.2, + 1.4, + -1.42, + -4.21, + 4.2, + 1.4, + -1.42, + -4.22, + 4.2, + 1.39, + -1.41, + -4.22, + 4.2, + 1.38, + -1.41, + -4.23, + 4.2, + 1.38, + -1.41, + -4.24, + 4.2, + 1.39, + -1.42, + -4.23, + 4.2, + 1.38, + -1.43, + -4.22, + 4.2, + 1.38, + -1.42, + -4.23, + 4.19, + 1.38, + -1.42, + -4.23, + 4.2, + 1.39, + -1.43, + -4.22, + 4.2, + 1.38, + -1.42, + -4.23, + 4.19, + 1.39, + -1.43, + -4.24, + 4.19, + 1.38, + -1.43, + -4.23, + 4.2, + 1.38, + -1.42, + -4.24, + 4.2, + 1.38, + -1.43, + -4.24, + 4.19, + 1.37, + -1.43, + -4.24, + 4.19, + 1.37, + -1.44, + -4.25 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 16.721, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "lidar_origin_to_beam_origin_mm": 16.721 + }, + "calibration_status": + { + "reflectivity": + { + "timestamp": "2022-11-29T02:00:08", + "valid": true + } + }, + "client_version": "ouster_client 0.7.1", + "config_params": + { + "azimuth_window": + [ + 0, + 360000 + ], + "columns_per_packet": 16, + "lidar_mode": "1024x10", + "multipurpose_io_mode": "OFF", + "nmea_baud_rate": "BAUD_9600", + "nmea_ignore_valid_char": 0, + "nmea_in_polarity": "ACTIVE_HIGH", + "nmea_leap_seconds": 0, + "operating_mode": "NORMAL", + "phase_lock_enable": false, + "phase_lock_offset": 0, + "signal_multiplier": 1, + "sync_pulse_in_polarity": "ACTIVE_HIGH", + "sync_pulse_out_angle": 360, + "sync_pulse_out_frequency": 1, + "sync_pulse_out_polarity": "ACTIVE_HIGH", + "sync_pulse_out_pulse_width": 10, + "timestamp_mode": "TIME_FROM_INTERNAL_OSC", + "udp_dest": "10.0.0.167", + "udp_port_imu": 7503, + "udp_port_lidar": 7502, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "imu_intrinsics": + { + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + -2.441, + 0, + 1, + 0, + -9.725, + 0, + 0, + 1, + 7.533, + 0, + 0, + 0, + 1 + ] + }, + "lidar_data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "lidar_intrinsics": + { + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 38.195, + 0, + 0, + 0, + 1 + ] + }, + "sensor_info": + { + "build_date": "2023-02-03T21:45:40Z", + "build_rev": "v3.0.1-rc.1", + "image_rev": "ousteros-image-prod-bootes-v3.0.1-rc.1+20230203215650.staging-2022", + "initialization_id": 1629346, + "prod_line": "OS-1-128", + "prod_pn": "840-104682-C", + "prod_sn": "122246000293", + "status": "RUNNING" + } +} diff --git a/tests/metadata/3_0_1_os-122246000293-128_legacy.json b/tests/metadata/3_0_1_os-122246000293-128_legacy.json new file mode 100644 index 00000000..a6fb9525 --- /dev/null +++ b/tests/metadata/3_0_1_os-122246000293-128_legacy.json @@ -0,0 +1,480 @@ +{ + "beam_altitude_angles": + [ + 20.47, + 20.21, + 19.89, + 19.54, + 19.23, + 18.97, + 18.66, + 18.31, + 18, + 17.72, + 17.4, + 17.05, + 16.74, + 16.45, + 16.13, + 15.77, + 15.45, + 15.15, + 14.83, + 14.47, + 14.15, + 13.85, + 13.52, + 13.17, + 12.83, + 12.52, + 12.19, + 11.83, + 11.5, + 11.18, + 10.85, + 10.5, + 10.14, + 9.82, + 9.49, + 9.14, + 8.79, + 8.45, + 8.13, + 7.76, + 7.42, + 7.08, + 6.75, + 6.39, + 6.04, + 5.7, + 5.35, + 5, + 4.64, + 4.32, + 3.98, + 3.61, + 3.25, + 2.92, + 2.57, + 2.22, + 1.85, + 1.52, + 1.17, + 0.82, + 0.45, + 0.12, + -0.24, + -0.58, + -0.95, + -1.28, + -1.64, + -1.97, + -2.35, + -2.69, + -3.03, + -3.37, + -3.74, + -4.1, + -4.43, + -4.77, + -5.14, + -5.49, + -5.82, + -6.17, + -6.53, + -6.88, + -7.22, + -7.55, + -7.91, + -8.27, + -8.6, + -8.92, + -9.28, + -9.64, + -9.97, + -10.29, + -10.65, + -11.01, + -11.33, + -11.64, + -12.01, + -12.35, + -12.69, + -12.98, + -13.34, + -13.69, + -14.03, + -14.32, + -14.68, + -15.02, + -15.35, + -15.63, + -15.98, + -16.34, + -16.65, + -16.93, + -17.27, + -17.63, + -17.93, + -18.21, + -18.55, + -18.9, + -19.2, + -19.48, + -19.81, + -20.16, + -20.46, + -20.72, + -21.05, + -21.39, + -21.68, + -21.95 + ], + "beam_azimuth_angles": + [ + 4.2, + 1.42, + -1.37, + -4.14, + 4.22, + 1.44, + -1.37, + -4.15, + 4.24, + 1.43, + -1.36, + -4.15, + 4.24, + 1.43, + -1.36, + -4.16, + 4.23, + 1.42, + -1.37, + -4.17, + 4.25, + 1.43, + -1.37, + -4.16, + 4.24, + 1.42, + -1.38, + -4.18, + 4.24, + 1.42, + -1.38, + -4.18, + 4.24, + 1.41, + -1.38, + -4.18, + 4.23, + 1.41, + -1.39, + -4.19, + 4.22, + 1.4, + -1.39, + -4.2, + 4.21, + 1.4, + -1.4, + -4.2, + 4.2, + 1.41, + -1.4, + -4.21, + 4.2, + 1.4, + -1.41, + -4.21, + 4.2, + 1.4, + -1.41, + -4.21, + 4.2, + 1.4, + -1.42, + -4.21, + 4.2, + 1.4, + -1.42, + -4.22, + 4.2, + 1.39, + -1.41, + -4.22, + 4.2, + 1.38, + -1.41, + -4.23, + 4.2, + 1.38, + -1.41, + -4.24, + 4.2, + 1.39, + -1.42, + -4.23, + 4.2, + 1.38, + -1.43, + -4.22, + 4.2, + 1.38, + -1.42, + -4.23, + 4.19, + 1.38, + -1.42, + -4.23, + 4.2, + 1.39, + -1.43, + -4.22, + 4.2, + 1.38, + -1.42, + -4.23, + 4.19, + 1.39, + -1.43, + -4.24, + 4.19, + 1.38, + -1.43, + -4.23, + 4.2, + 1.38, + -1.42, + -4.24, + 4.2, + 1.38, + -1.43, + -4.24, + 4.19, + 1.37, + -1.43, + -4.24, + 4.19, + 1.37, + -1.44, + -4.25 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 16.721, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "build_date": "2023-02-03T21:45:40Z", + "build_rev": "v3.0.1-rc.1", + "client_version": "ouster_client 0.7.1", + "data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12, + 12, + 4, + -4, + -12 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "hostname": "", + "image_rev": "ousteros-image-prod-bootes-v3.0.1-rc.1+20230203215650.staging-2022", + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + -2.441, + 0, + 1, + 0, + -9.725, + 0, + 0, + 1, + 7.533, + 0, + 0, + 0, + 1 + ], + "initialization_id": 1629346, + "json_calibration_version": 4, + "lidar_mode": "1024x10", + "lidar_origin_to_beam_origin_mm": 16.721, + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 38.195, + 0, + 0, + 0, + 1 + ], + "prod_line": "OS-1-128", + "prod_pn": "840-104682-C", + "prod_sn": "122246000293", + "status": "RUNNING", + "udp_port_imu": 7503, + "udp_port_lidar": 7502 +} diff --git a/tests/pcap_test.cpp b/tests/pcap_test.cpp new file mode 100644 index 00000000..fea7750d --- /dev/null +++ b/tests/pcap_test.cpp @@ -0,0 +1,160 @@ +/** + * Copyright (c) 2023, Ouster, Inc. + * All rights reserved. + */ + +#include +#include +#include "ouster/pcap.h" +#include "ouster/os_pcap.h" +#include "ouster/indexed_pcap_reader.h" + +namespace ouster { +namespace sensor_utils { + +inline std::string getenvs(const std::string& var) { + char* res = std::getenv(var.c_str()); + return res ? std::string{res} : std::string{}; +} + +/// it is equal to or occurs before the first byte in the packet +TEST(PcapReader, file_offset) { + // this file has one packet in it and is 8530 bytes long + auto data_dir = getenvs("DATA_DIR"); + PcapReader pcap(data_dir + "/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap"); + + // get offset of the first packet; + // since we haven't read any packets yet, call next_packet to read the first one + // then read the cached info + pcap.next_packet(); + packet_info current_info = pcap.current_info(); + EXPECT_EQ(current_info.file_offset, 24); // the length of a pcap file header + + size_t packet_size = pcap.next_packet(); + EXPECT_EQ(packet_size, 0); // no packets left + current_info = pcap.current_info(); + EXPECT_EQ(current_info.file_offset, 8530); // which is the end of the file +} + +/// it seeks to the specified offset +TEST(PcapReader, seek) { + auto data_dir = getenvs("DATA_DIR"); + PcapReader pcap(data_dir + "/OS-0-128-U1_v2.3.0_1024x10.pcap"); + + // get offset of the first packet; + // since we haven't read any packets yet, call next_packet to read the first one + // then read the cached info + pcap.next_packet(); + packet_info current_info = pcap.current_info(); + + // file_offset is supposed to be the position of the packet in the file + auto offset = current_info.file_offset; + + // so if we seek to that offset, we should be back at the beginning of the first packet + pcap.seek(offset); + + // read the packet again, read its info again + pcap.next_packet(); + current_info = pcap.current_info(); + + // it should have the same offset as the first packet we read + EXPECT_EQ(offset, current_info.file_offset); +} + +inline uint64_t file_size(const std::string& filename) { + FILE* f = fopen(filename.c_str(), "rb"); + fseek(f, 0, SEEK_END); + uint64_t size = ftell(f); + fclose(f); + return size; +} + +TEST(PcapReader, seek_to_0) { + // it should be able to read packets after an attempt to seek before the pcap header + // this file has one packet in it and is 8530 bytes long + auto data_dir = getenvs("DATA_DIR"); + std::string filename = data_dir + "/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap"; + PcapReader pcap(filename); + + EXPECT_EQ(pcap.next_packet(), 8448); + pcap.seek(0); + + // attempting to read past end of file is not an error, either + EXPECT_EQ(pcap.next_packet(), 8448); +} + +TEST(PcapReader, seek_middle_of_packet) { + // seeking to the middle of a packet results in undefined behavior when `next_packet()` is called again + auto data_dir = getenvs("DATA_DIR"); + std::string filename = data_dir + "/OS-0-128-U1_v2.3.0_1024x10.pcap"; + PcapReader pcap(filename); + + pcap.seek(4000); // roughly the middle of the first packet in the file + EXPECT_EQ(pcap.next_packet(), 0); + EXPECT_EQ(pcap.next_packet(), 0); +} + +TEST(PcapReader, seek_past_end_of_file) { + // it does not raise an exception if the caller seeks past the end of file + // this file has one packet in it and is 8530 bytes long + auto data_dir = getenvs("DATA_DIR"); + std::string filename = data_dir + "/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap"; + PcapReader pcap(filename); + pcap.seek(file_size(filename) + 1); + + // attempting to read past end of file is not an error, either + EXPECT_EQ(pcap.next_packet(), 0); +} + +void progress_callback(uint64_t, uint64_t, uint64_t) {} + + +TEST(IndexedPcapReader, constructor) { + // it should be constructed with the correct number of indices + // and previous frame counts (one for each metadata file) + auto data_dir = getenvs("DATA_DIR"); + std::string filename = data_dir + "/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap"; + std::string meta_filename = data_dir + "/OS-0-32-U1_v2.2.0_1024x10.json"; + + IndexedPcapReader pcap(filename, {meta_filename, meta_filename, meta_filename}, progress_callback); + EXPECT_EQ(pcap.frame_indices_.size(), 3); + EXPECT_EQ(pcap.previous_frame_ids_.size(), 3); +} + +TEST(IndexedPcapReader, frame_count) { + // it should raise std::out_of_range if there is no sensor at that position + auto data_dir = getenvs("DATA_DIR"); + std::string filename = data_dir + "/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap"; + IndexedPcapReader pcap(filename, {}, progress_callback); + pcap.frame_indices_.push_back(IndexedPcapReader::frame_index()); + pcap.frame_indices_.at(0).push_back(0); + + EXPECT_EQ(pcap.frame_count(0), 1); + EXPECT_THROW(pcap.frame_count(1), std::out_of_range); +} + +TEST(IndexedPcapReader, seek_to_frame) { + // it should raise std::out_of_range if there is no sensor or frame at that position + auto data_dir = getenvs("DATA_DIR"); + std::string filename = data_dir + "/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap"; + std::string meta_filename = data_dir + "/OS-0-32-U1_v2.2.0_1024x10.json"; + IndexedPcapReader pcap(filename, {meta_filename}, progress_callback); + pcap.frame_indices_.push_back(IndexedPcapReader::frame_index()); + + EXPECT_EQ(pcap.frame_count(0), 1); + EXPECT_NO_THROW(pcap.seek_to_frame(0, 0)); + EXPECT_THROW(pcap.seek_to_frame(0, 1), std::out_of_range); + EXPECT_THROW(pcap.seek_to_frame(1, 1), std::out_of_range); +} + +TEST(IndexedPcapReader, frame_id_rolled_over) { + EXPECT_TRUE(IndexedPcapReader::frame_id_rolled_over(65535, 0)); + EXPECT_TRUE(IndexedPcapReader::frame_id_rolled_over(65290, 100)); + EXPECT_FALSE(IndexedPcapReader::frame_id_rolled_over(65000, 65535)); + EXPECT_FALSE(IndexedPcapReader::frame_id_rolled_over(1, 0)); + EXPECT_FALSE(IndexedPcapReader::frame_id_rolled_over(12, 12)); +} + +} // namespace sensor_utils +} // namespace ouster + diff --git a/tests/pcaps/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap b/tests/pcaps/OS-0-32-U1_v2.2.0_1024x10-single-packet.pcap new file mode 100644 index 0000000000000000000000000000000000000000..52f086b75ea10029c2969360c9f59166575656d2 GIT binary patch literal 8530 zcmZvh3wTx4mB)7;H@N|lkee4F1QHT?Bp`?pP!LEUL>@*&h9X*^#rmpZwSZIw`OtAJ z3Mx2?BT%LEVJ$N)(@wR6MWtE`+FEQ?>JzKg+EN`~ZO7OA{%h?UPx$8WadQ86J@z?! z{rB2y?fct(yYJ2o`5}9K`e_I`fK%SM;tMUSdWGhOVE?mn{Y{6C&@e!1Ys+eXbu|Y1 zU)NRF)sWuMoD+7X9=>wZuUh}{=^v9;+0T21SJtdDkQ2_T4jFx177D`kc_Fk`g)-q) zj4>8`LS?8HzK17Vog2c+iclx~dPs%Y;2)NU0m5$~zX4oU9-4%w<)p%!;JY$msPO8X zypRUhXTliaJvl{T7XJ5_g(1S%puYw=?{=l zCOjVb+b*9eyeu~_lsN7bz9=^xmL&8iAV1ONy(GUZ6owZZ8#m_j{-=T$lrS+|C-UbN z-Y3)7@dWg524}f3im#VDwow|e?~VQVQYMOP1p1Gee<5)pKYU>Nj??Hrm5>`Zkhj^> z!F}`}19ucTxB7rjLjQ+p=Sm^=ihXs&^4}-nPu!Y`{eaQ^j0?G#bybHdjeZTfm-5st zRh}oVX0$@^kV;?o2f+V=uk$J#KZpKl;9W5<9AWJB;L^y2&k-A~+izsTG4g*NTzFq& zXTtHq4}q8B|JP+rs36gKk~)m6@oJMjGv zY)3^d{F-MM7cQ;{wR+lT80#GHx64C+;g`YULUH86*~Idj;5##+QSvi$axJI!k9px9 zV&BMk2g{r*tKh~q^qI@?y1z?aVOtUIjx}3pH?DvS`yEf!w;jZCgzL8nj|W#ewpcGB z_Ho@$k^Fe%kvr|ev+;Mb`?HnzRo-`_<3h>LU_Pz{kBeNG%=`X=x#amQ)?>hH9NVak z%>9`OTnBE#K2ypEu1Wa289brbV^hJ`18h7=L+FLSWL{9tzt{Y^JOvjHfln#o16LWb z9fSQZ((bPp`eoqeh?OUXfnQ`=r7x~s%cqK#TH``KU!M~9hzodk?VpWRprd6U9qbV|xZLWA75I;GxE9_+ih0UF}A6&(Xn|Q8v6F& z!Vp6C3Gy+e1dVG6e;&o(@?s{EYb1aEfIQR3V^xg*LpHvo+7t4^I`i*XIsYdK+z0&$ z=)c>|g@RC!uveZa27lO{7xKep*st3%z_^fN*tyjn`>!(E6}-j3xJO(8eg@m&-St9Y zI0L*Q?h%KOUk?6WtQX$pSr&uS<(>y75!)T$jd72-7%sfb*mW^4+zS_q7?0Y~xbYqE zJ?P_MqV7e+Htq?wXMBMDw_I*{BOs6Uz*yx{mRWfU}ok|L^ACzY%`sD6Bk?U*ud;9#Oqe zt0I$2^~75VeeE%)f@cpNXj~}3Y)-ZJ391#hVwL5>YMwF^+9S7y9E*g)(%@lWm8DElvgwWkmZ>kIw{ z_WjiB2gUk%mrs#aE3tgSvGpB)M!p1p*c%rvW}TNfo~CbS6Kmx)U#WHy)c=%UFq1?`DpZKB=87uCH9S_p;%To zWB(3#Z80CXqY}Ku{7LHdh2}GRsgIa~{>L#dsBb6*w?>@C z{|xYv#2&E&`!_ZZGA`)r>DAsh=-uyQZpva_5S{|=7x#z<@U{a!s;HZPpd=D&}zOJZNKlGrW8KU^3h{dM4V=rgyYb${?EE{Gd% zx;*BARCwR9aVJe|=Xq>9WIvC*Q0%x(Sbai+>$eNHAz$or!|iZop3CDNkstOrj(MS% z)kkw;UO0idkM)6Zp$$CGVw0^MyZZ=}w?YOL^ht1lAr_lOA{wyzC&U~nLW_;tqX~be0xFhBT#r|pcXY2S_ z=IizZRxNp{V_WwpVPB1XO{wq6zmq-qrecqE6IeZETjatB#(xocOUw&8k9gJm>6ujf zalVqkimmca_EOKNz9oB6J@n{VRXctW`|-C7HZF)llViPbCGT=0bCb;rSD~xgtU2xx zzXU%7rl+*CCdFRdpkA=^hCE{bGw?^TKIkG=isgow2Ud_54l;IatQ~J6Z)jcqtSmI? zd6$9Xx-_mF#Qq-aiAAg(Q{g`yTMznuo*g7W)&B-{Lst1+p~u zw|0If@-aPnP6v0KY=?s&v>BBxil3nsAnTq&ARUbKk3-G zP)01pHRKliXR+6Q3hu5Knrzn6?fP~y`fs^>n((a8*8LP=^^{+8eZy^VLHjNKtZ&eH z!u^h|?q9(BDi2-|`-rK`n+C^7ka~mqN)42Q(m$wPl;M#=!-z z$kVfg*C1cx`X(Pw-q56*&KImH>IF`9jo-GYyeqvsT zu}_75*gsvOSyi9V^Y;kleC71)rC96CKYJ;*-?j1l8|95;tffCXp|98{&oO0Rew2JV z{x;q;%(zet7iLw53VC0NFP%G9#or?~GgbziJ!^Uhxwx>PBGgM?eZ#rnyJ8=aL(h1X z#)@Z6b6BS@f&V$~5!ZtsVr=RK+XHmw{5kwT61nhoVtJqJW6!79e%G;`H}of`$6C_z zz)RQKoEI^|S};9os!a>lkZCXVALOo@F`Kx^7I! zmGfUoV8wc;d3Js60Xn_SSm|LoKp3ZGzTE4>E z<{9RU4*kdli-3-vhnqi*=6k}99yrzGoKWj{b3!hzcrBW>Pxa_46x-IL^z#!~wW503 z>?OPyJY{{0aiJ70jETQZ-N>_U@oMTPE<$31*;`r<=M1O_&vgY z#|*?y>xiDRLH47;Z-f6e){48?BecI!J6fzCCH6Wq{$=bb@8&n`SgV;_d&E=NGtags z=xk`aV_U~{#P$Wp#*G%_r{fR(IlAAB{3Dm!S<;uu3oQwJIr{2p(Kjw!fV|7)-L;*# zjNHy4uVX$|fd|AKuiu)s+T42W*nxb7W7{*Dk*{-X_hNqU@jgaA@yfre&A(&CK9#_d sKc2vf^-s-{>x&!u?J0Z7eq=&lu|AxTYY*~1DQlk>&qJ~v{{O%K2NyJmrT_o{ literal 0 HcmV?d00001