From 586632f3d8724367376ad432b425a38d70241873 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 17 Apr 2026 13:06:18 -0400 Subject: [PATCH 01/18] Initial impl with basic cal parsing --- README.md | 2 +- source/LibMultiSense/CMakeLists.txt | 8 +- source/LibMultiSense/details/amb/channel.cc | 189 +++++++++++++++++ source/LibMultiSense/details/amb/http.cc | 65 ++++++ source/LibMultiSense/details/amb/utilities.cc | 88 ++++++++ source/LibMultiSense/details/factory.cc | 25 +++ .../include/MultiSense/MultiSenseChannel.hh | 12 +- .../include/details/amb/channel.hh | 194 ++++++++++++++++++ .../LibMultiSense/include/details/amb/http.hh | 57 +++++ .../include/details/amb/utilities.hh | 47 +++++ 10 files changed, 683 insertions(+), 4 deletions(-) create mode 100644 source/LibMultiSense/details/amb/channel.cc create mode 100644 source/LibMultiSense/details/amb/http.cc create mode 100644 source/LibMultiSense/details/amb/utilities.cc create mode 100644 source/LibMultiSense/include/details/amb/channel.hh create mode 100644 source/LibMultiSense/include/details/amb/http.hh create mode 100644 source/LibMultiSense/include/details/amb/utilities.hh diff --git a/README.md b/README.md index 859930cc..6ed04a79 100644 --- a/README.md +++ b/README.md @@ -223,7 +223,7 @@ installed via pip into a Python virtual environment or a local Python project. To install the LibMultiSense Python bindings - > sudo apt install build-essential pybind11-dev nlohmann-json3-dev + > sudo apt install build-essential pybind11-dev nlohmann-json3-dev libcpp-httplib-dev > git clone https://github.com/carnegierobotics/LibMultiSense.git > cd LibMultiSense diff --git a/source/LibMultiSense/CMakeLists.txt b/source/LibMultiSense/CMakeLists.txt index 84f054b0..7e9c58f9 100644 --- a/source/LibMultiSense/CMakeLists.txt +++ b/source/LibMultiSense/CMakeLists.txt @@ -9,6 +9,9 @@ endif() set(DETAILS_SRC details/factory.cc details/multi_channel.cc details/utilities.cc + details/amb/channel.cc + details/amb/http.cc + details/amb/utilities.cc details/legacy/calibration.cc details/legacy/channel.cc details/legacy/configuration.cc @@ -63,8 +66,11 @@ set_target_properties(MultiSense PROPERTIES PUBLIC_HEADER "${MULTISENSE_HEADERS} # We want to link against our child libraries. # +find_package(PkgConfig REQUIRED) +pkg_check_modules(HTTPLIB REQUIRED IMPORTED_TARGET cpp-httplib) +find_package(OpenSSL REQUIRED) find_package(Threads REQUIRED) -target_link_libraries(MultiSense Threads::Threads) +target_link_libraries(MultiSense Threads::Threads PkgConfig::HTTPLIB OpenSSL::SSL OpenSSL::Crypto) if(CMAKE_SYSTEM_NAME STREQUAL "Windows" OR CMAKE_SYSTEM_NAME STREQUAL "WindowsStore") target_link_libraries(MultiSense ws2_32) elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux") diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc new file mode 100644 index 00000000..b8d21bd1 --- /dev/null +++ b/source/LibMultiSense/details/amb/channel.cc @@ -0,0 +1,189 @@ +/** + * @file channel.cc + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#include "details/amb/channel.hh" +#include "details/amb/http.hh" +#include "details/amb/utilities.hh" + +#include + +namespace multisense { +namespace amb { + +AmbChannel::AmbChannel(const Config &config): + m_config(config) +{ + if (config.connect_on_initialization && connect(config) != Status::OK) + { + CRL_EXCEPTION("Connection to MultiSense failed\n"); + } +} + +AmbChannel::~AmbChannel() = default; + +Status AmbChannel::start_streams(const std::vector &sources) +{ + (void) sources; + return Status::UNSUPPORTED; +} + +Status AmbChannel::stop_streams(const std::vector &sources) +{ + (void) sources; + return Status::UNSUPPORTED; +} + +void AmbChannel::add_image_frame_callback(std::function callback) +{ + (void) callback; + return; +} + +void AmbChannel::add_imu_frame_callback(std::function callback) +{ + (void) callback; + CRL_DEBUG("IMU callbacks are unsupported for the Ambarella camera"); + return; +} + +Status AmbChannel::connect(const Config &config) +{ + m_http_client = std::make_unique("https://" + config.ip_address); + + if (!m_http_client) + { + return Status::FAILED; + } + + // + // TODO (malvarado): Figure out the right way to resolve certs + // + m_http_client->enable_server_certificate_verification(false); + + // + // Update our cached calibration + // + if (auto calibration = query_calibration(); calibration) + { + m_calibration = std::move(calibration.value()); + } + else + { + CRL_DEBUG("Unable to query the camera's calibration"); + return Status::FAILED; + } + + return Status::OK; +}; + +void AmbChannel::disconnect() +{ + return; +}; + +std::optional AmbChannel::get_next_image_frame() +{ + return std::nullopt; +} + +std::optional AmbChannel::get_next_imu_frame() +{ + return std::nullopt; +} + +MultiSenseConfig AmbChannel::get_config() +{ + return m_multisense_config; +} + +Status AmbChannel::set_config(const MultiSenseConfig &config) +{ + (void) config; + return Status::UNSUPPORTED; +} + +StereoCalibration AmbChannel::get_calibration() +{ + return m_calibration; +} + +Status AmbChannel::set_calibration(const StereoCalibration &calibration) +{ + (void) calibration; + return Status::UNSUPPORTED; +} + +MultiSenseInfo AmbChannel::get_info() +{ + return m_info; +} + +Status AmbChannel::set_device_info(const MultiSenseInfo::DeviceInfo &device_info, const std::string &key) +{ + (void) device_info; + (void) key; + return Status::UNSUPPORTED; +} + +std::optional AmbChannel::get_system_status() +{ + return std::nullopt; +} + +Status AmbChannel::set_network_config(const MultiSenseInfo::NetworkInfo &config, + const std::optional &broadcast_interface) +{ + (void) config; + (void) broadcast_interface; + return Status::UNSUPPORTED; +} + +std::optional AmbChannel::query_calibration() +{ + const auto calibration_json = http_get(m_http_client, "/conf/conf.json"); + + if (calibration_json) + { + const auto intrinsics_yaml = base64_decode(calibration_json.value()["calibration"]["intrinsics"].get()); + const auto extrinsics_yaml = base64_decode(calibration_json.value()["calibration"]["intrinsics"].get()); + return m_calibration; + } + + return std::nullopt; +} + +} +} diff --git a/source/LibMultiSense/details/amb/http.cc b/source/LibMultiSense/details/amb/http.cc new file mode 100644 index 00000000..739a3153 --- /dev/null +++ b/source/LibMultiSense/details/amb/http.cc @@ -0,0 +1,65 @@ +/** + * @file http.cc + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#include "details/amb/http.hh" + +#include + +namespace multisense{ +namespace amb{ + +std::optional http_get(std::unique_ptr &client, const std::string &endpoint) +{ + auto result = client->Get(endpoint); + + if (result) + { + if (result->status == 200) + { + return nlohmann::json::parse(result->body); + } + else + { + CRL_DEBUG("HTTP GET request to %s returned error: %s\n", + endpoint.c_str(), std::to_string(result->status).c_str()); + } + } + + return std::nullopt; +} + +} +} diff --git a/source/LibMultiSense/details/amb/utilities.cc b/source/LibMultiSense/details/amb/utilities.cc new file mode 100644 index 00000000..670bd350 --- /dev/null +++ b/source/LibMultiSense/details/amb/utilities.cc @@ -0,0 +1,88 @@ +/** + * @file utilities.cc + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#include "details/amb/utilities.hh" + +#include + +namespace multisense{ +namespace amb{ + +std::string base64_decode(const std::string &input) +{ + std::string decoded_string; + + // Create a lookup table initialized to -1 + std::vector base64_lookup(256, -1); + + // Populate the lookup table with Base64 characters + const std::string base64_chars = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + + for (int i = 0; i < 64; i++) + { + base64_lookup[base64_chars[i]] = i; + } + + int val = 0; + int valb = -8; + + for (unsigned char c : input) + { + if (base64_lookup[c] == -1) + { + break; + } + + // + // Shift the current value over by 6 bits and add the new 6 bits + // + val = (val << 6) + base64_lookup[c]; + valb += 6; + + // + // Once we have at least 8 bits (a full byte), extract it + // + if (valb >= 0) + { + decoded_string.push_back(char((val >> valb) & 0xFF)); + valb -= 8; + } + } + return decoded_string; +} + +} +} diff --git a/source/LibMultiSense/details/factory.cc b/source/LibMultiSense/details/factory.cc index 252e8f5b..739f567e 100644 --- a/source/LibMultiSense/details/factory.cc +++ b/source/LibMultiSense/details/factory.cc @@ -34,6 +34,7 @@ * 2024-12-24, malvarado@carnegierobotics.com, IRAD, Created file. **/ +#include "details/amb/channel.hh" #include "details/legacy/channel.hh" namespace multisense @@ -44,6 +45,18 @@ std::unique_ptr Channel::create(const Config &config, { switch (impl) { + case ChannelImplementation::AUTO: + { + try + { + return std::unique_ptr(new amb::AmbChannel(config)); + } + catch(const std::exception &e) + { + CRL_DEBUG("Unable to create ambarella channel %s\n", e.what()); + return nullptr; + } + } case ChannelImplementation::LEGACY: { try @@ -56,6 +69,18 @@ std::unique_ptr Channel::create(const Config &config, return nullptr; } } + case ChannelImplementation::AMB: + { + try + { + return std::unique_ptr(new amb::AmbChannel(config)); + } + catch(const std::exception &e) + { + CRL_DEBUG("Unable to create ambarella channel %s\n", e.what()); + return nullptr; + } + } default: { return nullptr; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh index 9153a08d..08225664 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh @@ -51,10 +51,18 @@ public: /// enum class ChannelImplementation { + /// + /// @brief Attempt to automatically detect the camera type at connection time + /// + AUTO, /// /// @brief Use the Legacy MultiSense wire protocol implemented as part of LibMultiSense /// - LEGACY + LEGACY, + /// + /// @brief Use the Amberella MultiSense wire protocol implemented as part of LibMultiSense + /// + AMB }; /// @@ -130,7 +138,7 @@ public: /// implementations /// static std::unique_ptr create(const Config &config, - const ChannelImplementation &impl = ChannelImplementation::LEGACY); + const ChannelImplementation &impl = ChannelImplementation::AUTO); Channel() = default; diff --git a/source/LibMultiSense/include/details/amb/channel.hh b/source/LibMultiSense/include/details/amb/channel.hh new file mode 100644 index 00000000..bc724d45 --- /dev/null +++ b/source/LibMultiSense/include/details/amb/channel.hh @@ -0,0 +1,194 @@ +/** + * @file channel.hh + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#pragma once + +#include "MultiSense/MultiSenseChannel.hh" + +#ifndef CPPHTTPLIB_OPENSSL_SUPPORT +#define CPPHTTPLIB_OPENSSL_SUPPORT +#endif +#include + +namespace multisense{ +namespace amb{ + +class AmbChannel : public Channel +{ +public: + explicit AmbChannel(const Config &config); + + virtual ~AmbChannel(); + + /// + /// @brief Start a collection of image streams. Repeated calls to this function will not implicitly + /// stop the previously started streams. For example if a user started a left_raw stream in one + /// call and a disparity stream in a second call, both streams would be active until stop_streams + /// is called for either + /// + Status start_streams(const std::vector &sources) final override; + + /// + /// @brief Stop a collection of streams + /// + Status stop_streams(const std::vector &sources) final override; + + /// + /// @brief Add a image frame callback to get serviced inline with the receipt of a new frame. Only + /// a single image frame callback can be added to the channel + /// NOTE: Perform minimal work in this callback, and ideally copy the lightweight + /// ImageFrame object out to another processing thread + /// + void add_image_frame_callback(std::function callback) final override; + + /// + /// @brief Add a imu frame callback to get serviced inline with the receipt of a new frame. Only + /// a single imu frame callback can be added to the channel + /// NOTE: Perform minimal work in this callback, and ideally copy the lightweight + /// ImuFrame object out to another processing thread + /// + void add_imu_frame_callback(std::function callback) final override; + + /// + /// @brief Initialize the connection to the camera + /// + Status connect(const Config &config) final override; + + /// + /// @brief Disconnect from the camera + /// + void disconnect() final override; + + /// + /// @brief A blocking call that waits for one image frame from the camera. + /// + /// If you’ve set a receive timeout (via Config), it will block until that timeout expires; + /// otherwise, it blocks indefinitely until data arrives. + /// + /// @return The newly received ImageFrame, or std::nullopt if timed out (and you used a timeout). + /// + std::optional get_next_image_frame() final override; + + /// + /// @brief A blocking call that waits for one imu frame from the camera. + /// + /// If you’ve set a receive timeout (via Config), it will block until that timeout expires; + /// otherwise, it blocks indefinitely until data arrives. + /// + /// @return The newly received ImuFrame, or std::nullopt if timed out (and you used a timeout). + /// + std::optional get_next_imu_frame() final override; + + /// + /// @brief Get the current MultiSense configuration + /// + MultiSenseConfig get_config() final override; + + /// + /// @brief Get set the current MultiSense configuration + /// + Status set_config(const MultiSenseConfig &config) final override; + + /// + /// @brief Get the current stereo calibration. The output calibration will correspond to the full-resolution + /// operating mode of the camera + /// + StereoCalibration get_calibration() final override; + + /// + /// @brief Set the current stereo calibration. The calibration is expected to be or the full-resolution + /// operating mode of the camera + /// + Status set_calibration(const StereoCalibration &calibration) final override; + + /// + /// @brief Get the device info associated with the camera + /// + MultiSenseInfo get_info() final override; + + /// + /// @brief Set the camera's device info. This setting is protected via a key since invalid values in the + /// device info can result in internal camera failures + /// + Status set_device_info(const MultiSenseInfo::DeviceInfo &device_info, const std::string &key) final override; + + /// + /// @brief Query the current MultiSense status + /// + std::optional get_system_status() final override; + + /// + /// @brief Update the network configuration of the MultiSense. This will require a hardware reboot of the + /// MultiSense after it's been successfully applied + /// + Status set_network_config(const MultiSenseInfo::NetworkInfo &config, + const std::optional &broadcast_interface) final override; + +private: + + /// + /// @brief Query the calibration from the camera + /// + std::optional query_calibration(); + + /// + /// @brief Channel config + /// + Config m_config{}; + + /// + /// @brief The current cached calibration stored here for convenience + /// + StereoCalibration m_calibration{}; + + /// + /// @brief The current cached device info stored here for convenience + /// + MultiSenseInfo m_info{}; + + /// + /// @brief The current cached MultiSense configuration stored for convenience + /// + MultiSenseConfig m_multisense_config{}; + + /// + /// @brief HTTP client to handle https get/post requests + /// + std::unique_ptr m_http_client = nullptr; +}; + +} +} diff --git a/source/LibMultiSense/include/details/amb/http.hh b/source/LibMultiSense/include/details/amb/http.hh new file mode 100644 index 00000000..a7e2e537 --- /dev/null +++ b/source/LibMultiSense/include/details/amb/http.hh @@ -0,0 +1,57 @@ +/** + * @file http.hh + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#pragma once + +#include + +#ifndef CPPHTTPLIB_OPENSSL_SUPPORT +#define CPPHTTPLIB_OPENSSL_SUPPORT +#endif + +#include + +#include +#include + +namespace multisense{ +namespace amb{ + +std::optional http_get(std::unique_ptr &client, const std::string &endpoint); + +} +} + diff --git a/source/LibMultiSense/include/details/amb/utilities.hh b/source/LibMultiSense/include/details/amb/utilities.hh new file mode 100644 index 00000000..f353fbdf --- /dev/null +++ b/source/LibMultiSense/include/details/amb/utilities.hh @@ -0,0 +1,47 @@ +/** + * @file utilities.hh + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#pragma once + +#include + +namespace multisense{ +namespace amb{ + +std::string base64_decode(const std::string &input); + +} +} From cb90fe4a99f6ac4dc42a5ecefe63c071c8193679 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 17 Apr 2026 14:18:11 -0400 Subject: [PATCH 02/18] Reuse yaml parsing. Working cal parsing --- source/LibMultiSense/details/amb/channel.cc | 25 ++- source/LibMultiSense/details/utilities.cc | 97 ++++++++ .../include/MultiSense/MultiSenseUtilities.hh | 111 +++++++++ .../test/multisense_utilities_test.cc | 50 +++++ .../ImageCalUtility/ImageCalUtility.cc | 16 +- .../Utilities/portability/CalibrationYaml.hh | 210 ------------------ 6 files changed, 287 insertions(+), 222 deletions(-) delete mode 100644 source/Utilities/portability/CalibrationYaml.hh diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index b8d21bd1..5128ab89 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -34,10 +34,14 @@ * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. **/ +#include + #include "details/amb/channel.hh" #include "details/amb/http.hh" #include "details/amb/utilities.hh" +#include + #include namespace multisense { @@ -177,9 +181,24 @@ std::optional AmbChannel::query_calibration() if (calibration_json) { - const auto intrinsics_yaml = base64_decode(calibration_json.value()["calibration"]["intrinsics"].get()); - const auto extrinsics_yaml = base64_decode(calibration_json.value()["calibration"]["intrinsics"].get()); - return m_calibration; + auto intrinsics_yaml = std::stringstream(base64_decode(calibration_json.value()["calibration"]["intrinsics"].get())); + auto extrinsics_yaml = std::stringstream(base64_decode(calibration_json.value()["calibration"]["extrinsics"].get())); + + const auto intrinsics = parse_yaml(intrinsics_yaml); + const auto extrinsics = parse_yaml(extrinsics_yaml); + + StereoCalibration output; + std::memcpy(&output.left.K, intrinsics.at("M1").data(), 9 * sizeof(float)); + output.left.D = intrinsics.at("D1"); + std::memcpy(&output.left.R, extrinsics.at("R1").data(), 9 * sizeof(float)); + std::memcpy(&output.left.P, extrinsics.at("P1").data(), 12 * sizeof(float)); + + std::memcpy(&output.right.K, intrinsics.at("M2").data(), 9 * sizeof(float)); + output.right.D = intrinsics.at("D2"); + std::memcpy(&output.right.R, extrinsics.at("R2").data(), 9 * sizeof(float)); + std::memcpy(&output.right.P, extrinsics.at("P2").data(), 12 * sizeof(float)); + + return output; } return std::nullopt; diff --git a/source/LibMultiSense/details/utilities.cc b/source/LibMultiSense/details/utilities.cc index 7ca321c3..440099a0 100644 --- a/source/LibMultiSense/details/utilities.cc +++ b/source/LibMultiSense/details/utilities.cc @@ -503,4 +503,101 @@ std::optional> create_pointcloud(const ImageFrame &frame, return create_color_pointcloud(frame, max_range, DataSource::UNKNOWN, disparity_source); } +std::map> parse_yaml(std::istream& stream) +{ + std::map> data; + std::string token; + while (stream >> token) + { + // + // Skip comments or YAML headers + // + if (token.front() == '%' || token.front() == '-') + { + stream.ignore(std::numeric_limits::max(), '\n'); + continue; + } + + // + // We expect "key:" + // + size_t colon_pos = token.find(':'); + if (colon_pos == std::string::npos) + { + continue; + } + + std::string key = token.substr(0, colon_pos); + std::vector values; + + // + // If there was something after the colon in the same token, we don't handle it here + // for simplicity, we assume standard YAML spacing for keys. + // + stream >> std::ws; + int next_char = stream.peek(); + + if (next_char == '[') + { + stream.ignore(); // skip '[' + stream >> values; + } + else + { + std::string value_indicator; + if (stream >> value_indicator) + { + if (value_indicator == "!!opencv-matrix") + { + // + // For an OpenCV matrix, we look for the "data:" key and its associated array + // + while (stream >> token) + { + if (token == "data:") + { + stream >> std::ws; + if (stream.peek() == '[') + { + stream.ignore(); + stream >> values; + } + break; + } + else if (token.back() == ':') + { + // + // Some other sub-key (rows, cols, dt), skip its value + // + std::string dummy; + stream >> dummy; + } + } + } + else + { + // + // Try to parse as a single scalar value + // + try + { + values.push_back(std::stof(value_indicator)); + } + catch (...) + { + // Not a scalar, ignore + } + } + } + } + + if (!key.empty() && !values.empty()) + { + data[key] = values; + } + } + + return data; +} + } diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh index f2200822..d30a1685 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh @@ -37,8 +37,12 @@ #pragma once #include +#include #include #include +#include +#include +#include #include #include @@ -446,4 +450,111 @@ MULTISENSE_API bool write_pointcloud_ply(const PointCloud &point_cloud, c return true; } +/// +/// @brief Parse a YAML stream into a map of vectors. This implementation is designed to handle basic YAML structures +/// and OpenCV's matrix serialization format. +/// +/// @param stream Input stream to parse yaml data from +/// @return The parsed mapping between keys and values +/// +MULTISENSE_API std::map> parse_yaml(std::istream& stream); + +/// +/// @brief Write a matrix to a YAML stream in OpenCV format +/// +/// @param stream The output stream to write to +/// @param name The name of the YAML matrix +/// @param rows The number of rows in the matrix +/// @param columns The number of columns in the matrix +/// @param data A pointer to the raw data to write +/// +template +std::ostream& write_yaml_matrix(std::ostream& stream, const std::string& name, uint32_t rows, uint32_t columns, const T* data) +{ + stream << name << ": !!opencv-matrix\n"; + stream << " rows: " << rows << "\n"; + stream << " cols: " << columns << "\n"; + stream << " dt: d\n"; + stream << " data: [ "; + + stream.precision(17); + stream << std::scientific; + for (uint32_t i = 0; i < rows; i++) + { + if (i != 0) + { + stream << ",\n"; + stream << " "; + } + for (uint32_t j = 0; j < columns; j++) + { + if (j != 0) + { + stream << ", "; + } + stream << std::setw(22) << data[i * columns + j]; + } + } + stream << " ]\n"; + return stream; +} + +/// +/// @brief Overload the >> operator for std::vector to parse YAML-style arrays. +/// This will flatten nested arrays into a single vector. +/// +template +std::istream& operator>>(std::istream& stream, std::vector& data) +{ + char c; + while (stream >> c) + { + if (c == '[') + { + stream >> data; + } + else if (c == ']') + { + break; + } + else if (c == ',') + { + continue; + } + else + { + stream.putback(c); + T value; + if (stream >> value) + { + data.push_back(value); + } + else + { + // + // Clear error state to recover from invalid scalar (e.g., NaN or typo) + // + stream.clear(); + + // + // Consume the bad token up to the next delimiter or whitespace + // + while (stream.get(c)) + { + if (c == ',' || c == ']') + { + stream.putback(c); + break; + } + if (std::isspace(c)) + { + break; + } + } + } + } + } + return stream; +} + } diff --git a/source/LibMultiSense/test/multisense_utilities_test.cc b/source/LibMultiSense/test/multisense_utilities_test.cc index efa5ac0c..012534dd 100644 --- a/source/LibMultiSense/test/multisense_utilities_test.cc +++ b/source/LibMultiSense/test/multisense_utilities_test.cc @@ -389,3 +389,53 @@ TEST(create_pointcloud, basic_tests) ASSERT_TRUE(point_cloud_empty); ASSERT_TRUE(point_cloud_empty->cloud.empty()); } + +TEST(YAML, write_yaml_and_parse) +{ + const float data[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0}; + const uint32_t rows = 2; + const uint32_t cols = 3; + + std::stringstream ss; + write_yaml_matrix(ss, "test_matrix", rows, cols, data); + + const auto parsed_data = parse_yaml(ss); + + ASSERT_EQ(parsed_data.count("test_matrix"), 1); + ASSERT_EQ(parsed_data.at("test_matrix").size(), rows * cols); + for (size_t i = 0 ; i < rows * cols ; ++i) + { + EXPECT_FLOAT_EQ(parsed_data.at("test_matrix").at(i), data[i]); + } +} + +TEST(YAML, robustness) +{ + std::stringstream ss; + ss << "%YAML:1.0\n"; + ss << "scalar_val: 42.5\n"; + ss << "list_val: [10, invalid_float, 30]\n"; + ss << "matrix_val: !!opencv-matrix\n"; + ss << " rows: 1\n"; + ss << " cols: 2\n"; + ss << " dt: d\n"; + ss << " data: [1.1, 2.2]\n"; + ss << "another_scalar: 100\n"; + + const auto parsed_data = parse_yaml(ss); + + ASSERT_EQ(parsed_data.count("scalar_val"), 1); + EXPECT_FLOAT_EQ(parsed_data.at("scalar_val").at(0), 42.5); + + ASSERT_EQ(parsed_data.count("list_val"), 1); + ASSERT_EQ(parsed_data.at("list_val").size(), 2); + EXPECT_FLOAT_EQ(parsed_data.at("list_val")[0], 10.0); + EXPECT_FLOAT_EQ(parsed_data.at("list_val")[1], 30.0); + + ASSERT_EQ(parsed_data.count("matrix_val"), 1); + ASSERT_EQ(parsed_data.at("matrix_val").size(), 2); + EXPECT_FLOAT_EQ(parsed_data.at("matrix_val")[0], 1.1); + + ASSERT_EQ(parsed_data.count("another_scalar"), 1); + EXPECT_FLOAT_EQ(parsed_data.at("another_scalar")[0], 100.0); +} diff --git a/source/Utilities/LibMultiSense/ImageCalUtility/ImageCalUtility.cc b/source/Utilities/LibMultiSense/ImageCalUtility/ImageCalUtility.cc index 0af761ca..1e9e4b46 100644 --- a/source/Utilities/LibMultiSense/ImageCalUtility/ImageCalUtility.cc +++ b/source/Utilities/LibMultiSense/ImageCalUtility/ImageCalUtility.cc @@ -53,8 +53,8 @@ #include #include +#include -#include "CalibrationYaml.hh" #include "getopt/getopt.h" namespace lms = multisense; @@ -120,10 +120,10 @@ lms::CameraCalibration read_cal(const std::map> void write_cal(std::ofstream &intrinsics, std::ofstream &extrinsics, const lms::CameraCalibration &cal, size_t index) { - writeMatrix(intrinsics, "M" + std::to_string(index), 3, 3, &cal.K[0][0]); - writeMatrix(intrinsics, "D" + std::to_string(index), 1, cal.D.size(), cal.D.data()); - writeMatrix(extrinsics, "P" + std::to_string(index), 3, 4, &cal.P[0][0]); - writeMatrix(extrinsics, "R" + std::to_string(index), 3, 3, &cal.R[0][0]); + lms::write_yaml_matrix(intrinsics, "M" + std::to_string(index), 3, 3, &cal.K[0][0]); + lms::write_yaml_matrix(intrinsics, "D" + std::to_string(index), 1, cal.D.size(), cal.D.data()); + lms::write_yaml_matrix(extrinsics, "P" + std::to_string(index), 3, 4, &cal.P[0][0]); + lms::write_yaml_matrix(extrinsics, "R" + std::to_string(index), 3, 3, &cal.R[0][0]); } } @@ -197,11 +197,9 @@ int main(int argc, char** argv) return 1; } - std::map> intrinsics_data{}; - parseYaml(intrinsics, intrinsics_data); + const auto intrinsics_data = lms::parse_yaml(intrinsics); - std::map> extrinsics_data{}; - parseYaml(extrinsics, extrinsics_data); + const auto extrinsics_data = lms::parse_yaml(extrinsics); calibration.left = read_cal(intrinsics_data, extrinsics_data, 1); calibration.right = read_cal(intrinsics_data, extrinsics_data, 2); diff --git a/source/Utilities/portability/CalibrationYaml.hh b/source/Utilities/portability/CalibrationYaml.hh deleted file mode 100644 index 54061be9..00000000 --- a/source/Utilities/portability/CalibrationYaml.hh +++ /dev/null @@ -1,210 +0,0 @@ -/** - * @file shared/CalibrationYaml.hh - * - * Copyright 2013-2025 - * Carnegie Robotics, LLC - * 4501 Hatfield Street, Pittsburgh, PA 15201 - * http://www.carnegierobotics.com - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Carnegie Robotics, LLC nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - **/ - -#ifndef CALIBRATION_YAML_HH -#define CALIBRATION_YAML_HH - -#include -#include -#include -#include -#include - -template -std::ostream& writeMatrix (std::ostream& stream, std::string const& name, uint32_t rows, uint32_t columns, T const* data) -{ - stream << name << ": !!opencv-matrix\n"; - stream << " rows: " << rows << "\n"; - stream << " cols: " << columns << "\n"; - stream << " dt: d\n"; - stream << " data: [ "; - - stream.precision (17); - stream << std::scientific; - for (uint32_t i = 0; i < rows; i++) { - if (i != 0) { - stream << ",\n"; - stream << " "; - } - for (uint32_t j = 0; j < columns; j++) { - if (j != 0) { - stream << ", "; - } - stream << std::setw(22) << data[i * columns + j]; - } - } - stream << " ]\n"; - return stream; -} - -class Expect -{ -private: - std::string m_value; - -public: - Expect (std::string const& value) : - m_value (value) - { - } - - std::string const& value () const - { - return this->m_value; - } -}; - -std::istream& operator >> (std::istream& stream, Expect const& expect) -{ - stream >> std::ws; - - for (std::string::const_iterator iter = expect.value ().begin (); iter != expect.value ().end (); ++iter) - { - if (*iter == ' ') - { - stream >> std::ws; - continue; - } - if (stream.get () != *iter) - { - stream.clear (std::ios_base::failbit); - break; - } - } - - return stream; -} - -template -std::istream& operator >> (std::istream& stream, std::vector& data) -{ - char input; - while (stream.good ()) - { - input = 0; - stream >> input; - if (input == '[') - { - stream >> data; - } - else - { - stream.putback (input); - - T value; - stream >> value; - data.push_back (value); - } - - input = 0; - stream >> input; - if (input == ']') - { - break; - } - else if (input != ',') - { - stream.clear (std::ios_base::failbit); - break; - } - } - - return stream; -} - -std::istream& parseYaml (std::istream& stream, std::map >& data) -{ - char input; - while (stream.good ()) - { - input = 0; - stream >> input; - if (input == '%' || input == '-') - { - std::string comment; - std::getline (stream, comment); - continue; - } - stream.putback (input); - - std::string name; - stream >> name; - if (name.empty ()) - { - break; - } - if (name[name.size () - 1] != ':') - { - stream.clear (std::ios_base::failbit); - break; - } - name.resize (name.size () - 1); - - std::vector arrayContents; - arrayContents.clear (); - - input = 0; - stream >> input; - if (input == '[') - { - stream >> arrayContents; - } - else - { - stream.putback (input); - - uint32_t rows = 0; - uint32_t columns = 0; - stream >> Expect ("!!opencv-matrix"); - stream >> Expect ("rows:") >> rows; - stream >> Expect ("cols:") >> columns; - stream >> Expect ("dt: d"); - stream >> Expect ("data: [") >> arrayContents; - } - - if (stream.good()) - { - data.insert (std::make_pair(name, arrayContents)); - } - else - { - fprintf (stderr, "Error parsing data near array \"%s\"", name.c_str()); - } - } - - return stream; -} - -#endif //CALIBRATION_YAML_HH - From 054f16b244f4e1f02d5ba4d7ce3313254d93c306 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 17 Apr 2026 15:23:11 -0400 Subject: [PATCH 03/18] Basic device info --- source/LibMultiSense/CMakeLists.txt | 1 + source/LibMultiSense/details/amb/channel.cc | 64 ++++++++++++++++-- source/LibMultiSense/details/amb/info.cc | 65 +++++++++++++++++++ .../MultiSense/MultiSenseSerialization.hh | 3 +- .../include/MultiSense/MultiSenseTypes.hh | 3 +- .../include/details/amb/channel.hh | 5 ++ .../LibMultiSense/include/details/amb/info.hh | 55 ++++++++++++++++ 7 files changed, 190 insertions(+), 6 deletions(-) create mode 100644 source/LibMultiSense/details/amb/info.cc create mode 100644 source/LibMultiSense/include/details/amb/info.hh diff --git a/source/LibMultiSense/CMakeLists.txt b/source/LibMultiSense/CMakeLists.txt index 7e9c58f9..24c113be 100644 --- a/source/LibMultiSense/CMakeLists.txt +++ b/source/LibMultiSense/CMakeLists.txt @@ -10,6 +10,7 @@ set(DETAILS_SRC details/factory.cc details/multi_channel.cc details/utilities.cc details/amb/channel.cc + details/amb/info.cc details/amb/http.cc details/amb/utilities.cc details/legacy/calibration.cc diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index 5128ab89..b53be3f5 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -37,9 +37,11 @@ #include #include "details/amb/channel.hh" +#include "details/amb/info.hh" #include "details/amb/http.hh" #include "details/amb/utilities.hh" +#include #include #include @@ -110,6 +112,19 @@ Status AmbChannel::connect(const Config &config) return Status::FAILED; } + // + // Update our cached info + // + if (auto info = query_info(); info) + { + m_info = std::move(info.value()); + } + else + { + CRL_DEBUG("Unable to query the camera's device info"); + return Status::FAILED; + } + return Status::OK; }; @@ -177,12 +192,12 @@ Status AmbChannel::set_network_config(const MultiSenseInfo::NetworkInfo &config, std::optional AmbChannel::query_calibration() { - const auto calibration_json = http_get(m_http_client, "/conf/conf.json"); + const auto config_json = http_get(m_http_client, "/conf/conf.json"); - if (calibration_json) + if (config_json) { - auto intrinsics_yaml = std::stringstream(base64_decode(calibration_json.value()["calibration"]["intrinsics"].get())); - auto extrinsics_yaml = std::stringstream(base64_decode(calibration_json.value()["calibration"]["extrinsics"].get())); + auto intrinsics_yaml = std::stringstream(base64_decode(config_json.value()["calibration"]["intrinsics"].get())); + auto extrinsics_yaml = std::stringstream(base64_decode(config_json.value()["calibration"]["extrinsics"].get())); const auto intrinsics = parse_yaml(intrinsics_yaml); const auto extrinsics = parse_yaml(extrinsics_yaml); @@ -204,5 +219,46 @@ std::optional AmbChannel::query_calibration() return std::nullopt; } +std::optional AmbChannel::query_info() +{ + const auto info_json = http_get(m_http_client, "/conf/info.json"); + const auto config_json = http_get(m_http_client, "/conf/conf.json"); + + if (info_json && config_json) + { + const auto device_info_json = info_json.value()["device"]; + + MultiSenseInfo::DeviceInfo device_info; + // TODO (malvarado): Handle the rest of this conversion for more complex types + device_info.build_date = device_info_json["builddate"].get(); + device_info.serial_number = device_info_json["serialnum"].get(); + device_info.imager_name = device_info_json["imagername"].get(); + device_info.imager_width = device_info_json["imagerwidth"].get(); + device_info.imager_height = device_info_json["imagerheight"].get(); + device_info.lens_name = device_info_json["lensname"].get(); + //device_info.nominal_stereo_baseline = device_info_json["nominalbaseline"]; + //device_info.nominal_focal_length = device_info_json["nominalfocallength"]; + //device_info.nominal_relative_aperture = device_info_json["nominalrelativeaperture"]; + device_info.number_of_lights = device_info_json["lightingnumber"].get(); + + MultiSenseInfo::NetworkInfo network_info; + network_info.ip_address = config_json.value()["network"]["ip-address"].get(); + network_info.netmask = cidr_to_netmask(config_json.value()["network"]["netmask"].get()); + + MultiSenseInfo::SensorVersion version_info; + std::vector operating_modes; + + MultiSenseInfo info{std::move(device_info), + std::move(version_info), + std::move(operating_modes), + std::nullopt, + std::move(network_info)}; + return info; + } + + + return std::nullopt; +} + } } diff --git a/source/LibMultiSense/details/amb/info.cc b/source/LibMultiSense/details/amb/info.cc new file mode 100644 index 00000000..1d1b0eac --- /dev/null +++ b/source/LibMultiSense/details/amb/info.cc @@ -0,0 +1,65 @@ +/** + * @file info.cc + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#include "details/amb/info.hh" + +namespace multisense { +namespace amb { + +std::string cidr_to_netmask(uint32_t cidr) +{ + if (cidr > 32) + { + return "Invalid CIDR"; + } + + // + // A 32-bit mask with all 1s + // We handle cidr == 0 explicitly because shifting a 32-bit int by 32 + // results in undefined behavior in C++. + // + uint32_t mask = (cidr == 0) ? 0 : (0xFFFFFFFF << (32 - cidr)); + + uint8_t octet1 = (mask >> 24) & 0xFF; + uint8_t octet2 = (mask >> 16) & 0xFF; + uint8_t octet3 = (mask >> 8) & 0xFF; + uint8_t octet4 = mask & 0xFF; + + return std::to_string(octet1) + "." + std::to_string(octet2) + "." + std::to_string(octet3) + "." + std::to_string(octet4); +} + +} +} diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseSerialization.hh b/source/LibMultiSense/include/MultiSense/MultiSenseSerialization.hh index 03607c20..cec26dd4 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseSerialization.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseSerialization.hh @@ -203,7 +203,8 @@ NLOHMANN_JSON_SERIALIZE_ENUM(MultiSenseInfo::DeviceInfo::HardwareRevision, { {MultiSenseInfo::DeviceInfo::HardwareRevision::MONOCAM, "MONOCAM"}, {MultiSenseInfo::DeviceInfo::HardwareRevision::KS21_SILVER, "KS21_SILVER"}, {MultiSenseInfo::DeviceInfo::HardwareRevision::ST25, "ST25"}, - {MultiSenseInfo::DeviceInfo::HardwareRevision::KS21i, "KS21i"} + {MultiSenseInfo::DeviceInfo::HardwareRevision::KS21i, "KS21i"}, + {MultiSenseInfo::DeviceInfo::HardwareRevision::AMB, "AMB"} }) NLOHMANN_JSON_SERIALIZE_ENUM(MultiSenseInfo::DeviceInfo::ImagerType, { diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index fb05aa33..ae10cc1d 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -1362,7 +1362,8 @@ struct MultiSenseInfo MONOCAM, KS21_SILVER, ST25, - KS21i + KS21i, + AMB }; /// diff --git a/source/LibMultiSense/include/details/amb/channel.hh b/source/LibMultiSense/include/details/amb/channel.hh index bc724d45..30c2e162 100644 --- a/source/LibMultiSense/include/details/amb/channel.hh +++ b/source/LibMultiSense/include/details/amb/channel.hh @@ -164,6 +164,11 @@ private: /// std::optional query_calibration(); + /// + /// @brief Query the MultiSense Info + /// + std::optional query_info(); + /// /// @brief Channel config /// diff --git a/source/LibMultiSense/include/details/amb/info.hh b/source/LibMultiSense/include/details/amb/info.hh new file mode 100644 index 00000000..84c5ba66 --- /dev/null +++ b/source/LibMultiSense/include/details/amb/info.hh @@ -0,0 +1,55 @@ +/** + * @file info.hh + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#pragma once + +#include +#include + +namespace multisense { +namespace amb { + +/// +/// @brief Convert a CIDR block like /24 or /16 to a dotted-decimal netamsk string like 255.255.255.0 +/// +/// @param cidr The CIDR block to convert to a netmask +/// @return Return the netmask string +/// +std::string cidr_to_netmask(uint32_t cidr); + +} +} + From c8623a83c8b3f64b13b9170babbd7adc538c638f Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 17 Apr 2026 15:39:47 -0400 Subject: [PATCH 04/18] Find/build against the multisense rtsp lib --- source/LibMultiSense/CMakeLists.txt | 5 +++-- source/LibMultiSense/details/amb/channel.cc | 2 ++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/source/LibMultiSense/CMakeLists.txt b/source/LibMultiSense/CMakeLists.txt index 24c113be..0f1fc14e 100644 --- a/source/LibMultiSense/CMakeLists.txt +++ b/source/LibMultiSense/CMakeLists.txt @@ -64,14 +64,15 @@ target_include_directories(MultiSense PUBLIC $ #include +#include "multisense-webrtc-client.h" + #include namespace multisense { From 9834a9cf3dc1a364969eca730730a9db02cc811a Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 17 Apr 2026 15:41:00 -0400 Subject: [PATCH 05/18] Link against webrtc library --- source/LibMultiSense/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/LibMultiSense/CMakeLists.txt b/source/LibMultiSense/CMakeLists.txt index 0f1fc14e..3b202909 100644 --- a/source/LibMultiSense/CMakeLists.txt +++ b/source/LibMultiSense/CMakeLists.txt @@ -71,7 +71,7 @@ find_package(PkgConfig REQUIRED) pkg_check_modules(HTTPLIB REQUIRED IMPORTED_TARGET cpp-httplib) find_package(OpenSSL REQUIRED) find_package(Threads REQUIRED) -target_link_libraries(MultiSense Threads::Threads PkgConfig::HTTPLIB OpenSSL::SSL OpenSSL::Crypto) +target_link_libraries(MultiSense Threads::Threads PkgConfig::HTTPLIB OpenSSL::SSL OpenSSL::Crypto multisense-webrtc-client) if(CMAKE_SYSTEM_NAME STREQUAL "Windows" OR CMAKE_SYSTEM_NAME STREQUAL "WindowsStore") target_link_libraries(MultiSense ws2_32) From b158e9eee84902dc50ee1e6c272b419f2ce9b8d2 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 17 Apr 2026 17:31:38 -0400 Subject: [PATCH 06/18] Crude impl of image streaming --- source/LibMultiSense/CMakeLists.txt | 1 + source/LibMultiSense/details/amb/channel.cc | 50 ++++- source/LibMultiSense/details/amb/webrtc.cc | 177 ++++++++++++++++++ .../include/details/amb/channel.hh | 23 +++ .../include/details/amb/webrtc.hh | 103 ++++++++++ .../include/details/legacy/channel.hh | 67 +------ .../include/details/utilities.hh | 110 +++++++++++ 7 files changed, 460 insertions(+), 71 deletions(-) create mode 100644 source/LibMultiSense/details/amb/webrtc.cc create mode 100644 source/LibMultiSense/include/details/amb/webrtc.hh create mode 100644 source/LibMultiSense/include/details/utilities.hh diff --git a/source/LibMultiSense/CMakeLists.txt b/source/LibMultiSense/CMakeLists.txt index 3b202909..5a2c75d4 100644 --- a/source/LibMultiSense/CMakeLists.txt +++ b/source/LibMultiSense/CMakeLists.txt @@ -13,6 +13,7 @@ set(DETAILS_SRC details/factory.cc details/amb/info.cc details/amb/http.cc details/amb/utilities.cc + details/amb/webrtc.cc details/legacy/calibration.cc details/legacy/channel.cc details/legacy/configuration.cc diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index cd652878..296053b2 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -65,18 +65,18 @@ AmbChannel::~AmbChannel() = default; Status AmbChannel::start_streams(const std::vector &sources) { (void) sources; - return Status::UNSUPPORTED; + return Status::OK; } Status AmbChannel::stop_streams(const std::vector &sources) { (void) sources; - return Status::UNSUPPORTED; + return Status::OK; } void AmbChannel::add_image_frame_callback(std::function callback) { - (void) callback; + m_user_image_frame_callback = callback; return; } @@ -127,17 +127,57 @@ Status AmbChannel::connect(const Config &config) return Status::FAILED; } + // + // Connect our WebRTC client + // + m_webtrc = std::make_unique("https://" + config.ip_address); + + if (!m_webtrc) + { + return Status::FAILED; + } + + // + // TODO (malvarado): Handle this better + // + m_webtrc->connect(true, true, true, true); + + m_webtrc->set_frame_callback([this](ImageFrame& frame) + { + frame.calibration = m_calibration; + if (m_user_image_frame_callback) + { + m_user_image_frame_callback(frame); + } + + m_image_frame_notifier.set_and_notify(frame); + }); + + m_connected = true; + return Status::OK; }; void AmbChannel::disconnect() { + if (m_webtrc) + { + m_webtrc->disconnect(); + } + + m_connected = false; + return; }; std::optional AmbChannel::get_next_image_frame() { - return std::nullopt; + if (!m_connected) + { + return std::nullopt; + } + + return m_image_frame_notifier.wait(m_config.receive_timeout); } std::optional AmbChannel::get_next_imu_frame() @@ -153,7 +193,7 @@ MultiSenseConfig AmbChannel::get_config() Status AmbChannel::set_config(const MultiSenseConfig &config) { (void) config; - return Status::UNSUPPORTED; + return Status::OK; } StereoCalibration AmbChannel::get_calibration() diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc new file mode 100644 index 00000000..1aaa352f --- /dev/null +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -0,0 +1,177 @@ +/** + * @file webrtc_client.cc + * + * Copyright 2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#include "details/amb/webrtc.hh" + +#include +#include + +// Declare the extern "C" functions exposed by the Rust FFI library +extern "C" +{ + typedef void (*mswebrtc_frame_callback)(void* left_data, int left_size, + void* right_data, int right_size, + void* disparity_data, int disparity_size, + void* user_data); + + void* create_mswebrtc_impl(const char* host); + bool connect_mswebrtc_impl(void* obj, bool left, bool right, bool disparity, bool nndata); + void disconnect_mswebrtc_impl(void* obj); + void destroy_mswebrtc_impl(void* obj); + void set_frame_callback_mswebrtc_impl(void* obj, mswebrtc_frame_callback cb, void* user_data); + void set_bitrate_mswebrtc_impl(void* obj, double bitrate); +} + +namespace multisense { +namespace amb { + +WebRtcClient::WebRtcClient(const std::string& host) + : m_impl(nullptr) +{ + m_impl = create_mswebrtc_impl(host.c_str()); +} + +WebRtcClient::~WebRtcClient() +{ + if (m_impl) { + destroy_mswebrtc_impl(m_impl); + m_impl = nullptr; + } +} + +bool WebRtcClient::connect(bool left, bool right, bool disparity, bool nndata) +{ + if (!m_impl) + { + return false; + } + + return connect_mswebrtc_impl(m_impl, left, right, disparity, nndata); +} + +void WebRtcClient::disconnect() +{ + if (m_impl) + { + disconnect_mswebrtc_impl(m_impl); + } +} + +void WebRtcClient::set_bitrate(double bitrate) +{ + if (m_impl) + { + set_bitrate_mswebrtc_impl(m_impl, bitrate); + } +} + +void WebRtcClient::set_frame_callback(std::function callback) +{ + m_frame_callback = std::move(callback); + + if (m_impl) + { + set_frame_callback_mswebrtc_impl(m_impl, &WebRtcClient::c_frame_callback, this); + } +} + +void WebRtcClient::c_frame_callback(void* left_data, + int left_size, + void* right_data, + int right_size, + void* disparity_data, + int disparity_size, + void* user_data) +{ + if (!user_data) + { + return; + } + + WebRtcClient* client = static_cast(user_data); + if (!client->m_frame_callback) + { + return; + } + + // TODO (malvarado): do this better + static size_t frame_id = 0; + ImageFrame frame; + frame.frame_id = ++frame_id; + + auto create_image = [](void* data, int size, DataSource source, Image::PixelFormat format) -> std::optional + { + if (data == nullptr || size <= 0) + { + return std::nullopt; + } + + Image image; + auto buffer = std::make_shared>(size); + std::memcpy(buffer->data(), data, size); + + image.raw_data = buffer; + image.image_data_offset = 0; + image.image_data_length = size; + image.source = source; + image.format = format; + // TODO (malvarado) : fix this + image.width = 1920; + image.height = 1200; + + return image; + }; + + if (auto left_image = create_image(left_data, left_size, DataSource::LEFT_RECTIFIED_RAW, Image::PixelFormat::MONO8); left_image) + { + frame.add_image(left_image.value()); + } + + if (auto right_image = create_image(right_data, right_size, DataSource::RIGHT_RECTIFIED_RAW, Image::PixelFormat::MONO8); right_image) + { + frame.add_image(right_image.value()); + } + + if (auto disparity_image = create_image(disparity_data, disparity_size, DataSource::LEFT_DISPARITY_RAW, Image::PixelFormat::MONO16); disparity_image) + { + frame.add_image(disparity_image.value()); + } + + client->m_frame_callback(frame); +} + +} // namespace amb +} // namespace multisense diff --git a/source/LibMultiSense/include/details/amb/channel.hh b/source/LibMultiSense/include/details/amb/channel.hh index 30c2e162..fe39261b 100644 --- a/source/LibMultiSense/include/details/amb/channel.hh +++ b/source/LibMultiSense/include/details/amb/channel.hh @@ -43,6 +43,9 @@ #endif #include +#include "details/utilities.hh" +#include "details/amb/webrtc.hh" + namespace multisense{ namespace amb{ @@ -189,10 +192,30 @@ private: /// MultiSenseConfig m_multisense_config{}; + /// + /// @brief The currently active image frame user callback + /// + std::function m_user_image_frame_callback{}; + + /// + /// @brief Notifier used to service the get_next_image_frame member function + /// + FrameNotifier m_image_frame_notifier{}; + /// /// @brief HTTP client to handle https get/post requests /// std::unique_ptr m_http_client = nullptr; + + /// + /// @brief WebRTC client for querying images + /// + std::unique_ptr m_webtrc = nullptr; + + /// + /// @brief Atomic flag to determine if we are connected to an active camera + /// + std::atomic_bool m_connected = false; }; } diff --git a/source/LibMultiSense/include/details/amb/webrtc.hh b/source/LibMultiSense/include/details/amb/webrtc.hh new file mode 100644 index 00000000..9bb72afa --- /dev/null +++ b/source/LibMultiSense/include/details/amb/webrtc.hh @@ -0,0 +1,103 @@ +/** + * @file webrtc.hh + * + * Copyright 2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#pragma once + +#include +#include + +#include "MultiSense/MultiSenseTypes.hh" + +namespace multisense { +namespace amb { + +/// +/// @brief A WebRTC client to connect to the camera +/// +class WebRtcClient { +public: + /// + /// @brief Create a new WebRTC client which attaches to a server at a specific hostname + /// + explicit WebRtcClient(const std::string& host); + ~WebRtcClient(); + + WebRtcClient(const WebRtcClient&) = delete; + WebRtcClient& operator=(const WebRtcClient&) = delete; + + /// + /// @brief Connect to the WebRTC server and start streaming available topics + /// + bool connect(bool left, bool right, bool disparity, bool nndata); + + /// + /// @brief Disconnect from the WebRTC sever + /// + void disconnect(); + + /// + /// @brief Set the bitrate used to encode left/right rectified image + /// + void set_bitrate(double bitrate); + + /// + /// @brief Attach a frame callback to the client which returns whenever a valid ImageFrame is received + /// + void set_frame_callback(std::function callback); + +private: + + /// + /// @brief Private callback signature which the c client WebRTC uses to return images + /// + static void c_frame_callback(void* left_data, + int left_size, + void* right_data, + int right_size, + void* disparity_data, + int disparity_size, + void* user_data); + + /// + /// @param Pointer to the base C WebRTC implementation + /// + void* m_impl = nullptr; + + std::function m_frame_callback; +}; + +} // namespace amb +} // namespace multisense diff --git a/source/LibMultiSense/include/details/legacy/channel.hh b/source/LibMultiSense/include/details/legacy/channel.hh index b060fd5b..4866710b 100644 --- a/source/LibMultiSense/include/details/legacy/channel.hh +++ b/source/LibMultiSense/include/details/legacy/channel.hh @@ -44,6 +44,7 @@ #include #include +#include "details/utilities.hh" #include "details/legacy/ip.hh" #include "details/legacy/message.hh" #include "details/legacy/storage.hh" @@ -53,72 +54,6 @@ namespace multisense{ namespace legacy{ -template -class FrameNotifier -{ -public: - - FrameNotifier() = default; - - ~FrameNotifier() - { - m_cv.notify_all(); - } - - /// - /// @brief Copy a frame into the local storage, and notify all waiters that the frame is valid - /// - void set_and_notify(const T &in_frame) - { - std::lock_guard lock(m_mutex); - m_frame = in_frame; - m_cv.notify_all(); - } - - /// - /// @brief Wait for the notifier to be valid. If the timeout is invalid, will wait forever - /// - template - std::optional wait(const std::optional>& timeout) - { - std::unique_lock lock(m_mutex); - - std::optional output_frame = std::nullopt; - if (timeout) - { - if (std::cv_status::no_timeout == m_cv.wait_for(lock, timeout.value())) - { - output_frame = std::move(m_frame); - } - } - else - { - m_cv.wait(lock); - output_frame = std::move(m_frame); - } - - // - // Reset the frame - // - m_frame = std::nullopt; - - return output_frame; - } - - std::optional wait() - { - const std::optional timeout = std::nullopt; - return wait(timeout); - } - -private: - - std::mutex m_mutex; - std::condition_variable m_cv; - std::optional m_frame; -}; - - class LegacyChannel : public Channel { public: diff --git a/source/LibMultiSense/include/details/utilities.hh b/source/LibMultiSense/include/details/utilities.hh new file mode 100644 index 00000000..c759fdf2 --- /dev/null +++ b/source/LibMultiSense/include/details/utilities.hh @@ -0,0 +1,110 @@ +/** + * @file utilities.hh + * + * Copyright 2013-2026 + * Carnegie Robotics, LLC + * 4501 Hatfield Street, Pittsburgh, PA 15201 + * http://www.carnegierobotics.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Carnegie Robotics, LLC nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL CARNEGIE ROBOTICS, LLC BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Significant history (date, user, job code, action): + * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. + **/ + +#pragma once + +#include +#include +#include + +namespace multisense{ + +template +class FrameNotifier +{ +public: + + FrameNotifier() = default; + + ~FrameNotifier() + { + m_cv.notify_all(); + } + + /// + /// @brief Copy a frame into the local storage, and notify all waiters that the frame is valid + /// + void set_and_notify(const T &in_frame) + { + std::lock_guard lock(m_mutex); + m_frame = in_frame; + m_cv.notify_all(); + } + + /// + /// @brief Wait for the notifier to be valid. If the timeout is invalid, will wait forever + /// + template + std::optional wait(const std::optional>& timeout) + { + std::unique_lock lock(m_mutex); + + std::optional output_frame = std::nullopt; + if (timeout) + { + if (std::cv_status::no_timeout == m_cv.wait_for(lock, timeout.value())) + { + output_frame = std::move(m_frame); + } + } + else + { + m_cv.wait(lock); + output_frame = std::move(m_frame); + } + + // + // Reset the frame + // + m_frame = std::nullopt; + + return output_frame; + } + + std::optional wait() + { + const std::optional timeout = std::nullopt; + return wait(timeout); + } + +private: + + std::mutex m_mutex; + std::condition_variable m_cv; + std::optional m_frame; +}; + +} From a175b68b92dc2180dcbcf560aafbf385e81879ea Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 21 Apr 2026 12:45:15 -0400 Subject: [PATCH 07/18] Handle auto connection --- source/LibMultiSense/details/amb/channel.cc | 15 +++++++++++ source/LibMultiSense/details/factory.cc | 25 +++++++++++++++---- .../include/details/amb/channel.hh | 7 ++++++ 3 files changed, 42 insertions(+), 5 deletions(-) diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index 296053b2..ba69f7b1 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -144,6 +144,7 @@ Status AmbChannel::connect(const Config &config) m_webtrc->set_frame_callback([this](ImageFrame& frame) { + std::cout << "got new frame" << std::endl; frame.calibration = m_calibration; if (m_user_image_frame_callback) { @@ -302,5 +303,19 @@ std::optional AmbChannel::query_info() return std::nullopt; } +bool is_amb_camera(const std::string &ip_address) +{ + auto client = std::make_unique("https://" + ip_address); + + if (!client) + { + return false; + } + + client->enable_server_certificate_verification(false); + + return static_cast(http_get(client, "/conf/info.json")); +} + } } diff --git a/source/LibMultiSense/details/factory.cc b/source/LibMultiSense/details/factory.cc index 739f567e..b854e56b 100644 --- a/source/LibMultiSense/details/factory.cc +++ b/source/LibMultiSense/details/factory.cc @@ -47,14 +47,29 @@ std::unique_ptr Channel::create(const Config &config, { case ChannelImplementation::AUTO: { - try + if (amb::is_amb_camera(config.ip_address)) { - return std::unique_ptr(new amb::AmbChannel(config)); + try + { + return std::unique_ptr(new amb::AmbChannel(config)); + } + catch(const std::exception &e) + { + CRL_DEBUG("Unable to create ambarella channel %s\n", e.what()); + return nullptr; + } } - catch(const std::exception &e) + else { - CRL_DEBUG("Unable to create ambarella channel %s\n", e.what()); - return nullptr; + try + { + return std::unique_ptr(new legacy::LegacyChannel(config)); + } + catch(const std::exception &e) + { + CRL_DEBUG("Unable to create legacy channel %s\n", e.what()); + return nullptr; + } } } case ChannelImplementation::LEGACY: diff --git a/source/LibMultiSense/include/details/amb/channel.hh b/source/LibMultiSense/include/details/amb/channel.hh index fe39261b..15a56e9c 100644 --- a/source/LibMultiSense/include/details/amb/channel.hh +++ b/source/LibMultiSense/include/details/amb/channel.hh @@ -218,5 +218,12 @@ private: std::atomic_bool m_connected = false; }; +/// +/// @brief Helper function to test if an amb camera is connected to a specific IP address +/// +/// @param ip_address The ip address where the amb camera should be connected +/// +bool is_amb_camera(const std::string &ip_address); + } } From ccc025d0f7fb2df6b8147d28f8b11612e6f96fe7 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 21 Apr 2026 14:31:13 -0400 Subject: [PATCH 08/18] Add channel timeouts --- source/LibMultiSense/details/amb/channel.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index ba69f7b1..9ffa606b 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -96,6 +96,11 @@ Status AmbChannel::connect(const Config &config) return Status::FAILED; } + if (config.receive_timeout) + { + m_http_client->set_read_timeout(config.receive_timeout.value()); + } + // // TODO (malvarado): Figure out the right way to resolve certs // @@ -312,6 +317,8 @@ bool is_amb_camera(const std::string &ip_address) return false; } + client->set_read_timeout(std::chrono::seconds(1)); + client->enable_server_certificate_verification(false); return static_cast(http_get(client, "/conf/info.json")); From c74a467c2549427d170ccfda4bfa36904d43a17f Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 21 Apr 2026 15:55:54 -0400 Subject: [PATCH 09/18] Export dependencies --- source/LibMultiSense/MultiSenseConfig.cmake.in | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/source/LibMultiSense/MultiSenseConfig.cmake.in b/source/LibMultiSense/MultiSenseConfig.cmake.in index 02a39482..42aec567 100644 --- a/source/LibMultiSense/MultiSenseConfig.cmake.in +++ b/source/LibMultiSense/MultiSenseConfig.cmake.in @@ -6,6 +6,10 @@ if (@BUILD_JSON_SERIALIZATION@) find_dependency(nlohmann_json 3.9) endif () find_dependency(Threads) +find_dependency(multisense-webrtc-client) +find_dependency(OpenSSL) +find_dependency(PkgConfig) +pkg_check_modules(HTTPLIB REQUIRED IMPORTED_TARGET cpp-httplib) include("${CMAKE_CURRENT_LIST_DIR}/MultiSenseTargets.cmake") From 03384e7543182e089c765c81ff3be54f452c0f11 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 21 Apr 2026 16:03:59 -0400 Subject: [PATCH 10/18] Publish valid times --- source/LibMultiSense/MultiSenseConfig.cmake.in | 2 ++ source/LibMultiSense/details/amb/webrtc.cc | 3 +++ 2 files changed, 5 insertions(+) diff --git a/source/LibMultiSense/MultiSenseConfig.cmake.in b/source/LibMultiSense/MultiSenseConfig.cmake.in index 42aec567..f4783888 100644 --- a/source/LibMultiSense/MultiSenseConfig.cmake.in +++ b/source/LibMultiSense/MultiSenseConfig.cmake.in @@ -6,6 +6,8 @@ if (@BUILD_JSON_SERIALIZATION@) find_dependency(nlohmann_json 3.9) endif () find_dependency(Threads) + +# TODO (malvarado): Make this conditional find_dependency(multisense-webrtc-client) find_dependency(OpenSSL) find_dependency(PkgConfig) diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc index 1aaa352f..d80edce0 100644 --- a/source/LibMultiSense/details/amb/webrtc.cc +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -131,6 +131,9 @@ void WebRtcClient::c_frame_callback(void* left_data, static size_t frame_id = 0; ImageFrame frame; frame.frame_id = ++frame_id; + const auto now = std::chrono::time_point_cast(std::chrono::system_clock::now()); + frame.frame_time = now; + frame.ptp_frame_time = now; auto create_image = [](void* data, int size, DataSource source, Image::PixelFormat format) -> std::optional { From c29b1e864453ff55e1fe86992884a0e5a106c2dd Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 21 Apr 2026 16:17:52 -0400 Subject: [PATCH 11/18] Fix disparity size --- source/LibMultiSense/details/amb/webrtc.cc | 28 +++++++++++++++++----- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc index d80edce0..2eebd47d 100644 --- a/source/LibMultiSense/details/amb/webrtc.cc +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -127,6 +127,8 @@ void WebRtcClient::c_frame_callback(void* left_data, return; } + std::cout << left_size << " " << right_size << " " << disparity_size << std::endl; + // TODO (malvarado): do this better static size_t frame_id = 0; ImageFrame frame; @@ -135,7 +137,7 @@ void WebRtcClient::c_frame_callback(void* left_data, frame.frame_time = now; frame.ptp_frame_time = now; - auto create_image = [](void* data, int size, DataSource source, Image::PixelFormat format) -> std::optional + auto create_image = [](void* data, int size, size_t width, size_t height, DataSource source, Image::PixelFormat format) -> std::optional { if (data == nullptr || size <= 0) { @@ -152,23 +154,37 @@ void WebRtcClient::c_frame_callback(void* left_data, image.source = source; image.format = format; // TODO (malvarado) : fix this - image.width = 1920; - image.height = 1200; + image.width = width; + image.height = height; return image; }; - if (auto left_image = create_image(left_data, left_size, DataSource::LEFT_RECTIFIED_RAW, Image::PixelFormat::MONO8); left_image) + if (auto left_image = create_image(left_data, + left_size, + 1920, + 1200, + DataSource::LEFT_RECTIFIED_RAW, + Image::PixelFormat::MONO8); left_image) { frame.add_image(left_image.value()); } - if (auto right_image = create_image(right_data, right_size, DataSource::RIGHT_RECTIFIED_RAW, Image::PixelFormat::MONO8); right_image) + if (auto right_image = create_image(right_data, + right_size, + 1920, + 1200, + DataSource::RIGHT_RECTIFIED_RAW, + Image::PixelFormat::MONO8); right_image) { frame.add_image(right_image.value()); } - if (auto disparity_image = create_image(disparity_data, disparity_size, DataSource::LEFT_DISPARITY_RAW, Image::PixelFormat::MONO16); disparity_image) + if (auto disparity_image = create_image(disparity_data, + disparity_size, + 960, + 600, + DataSource::LEFT_DISPARITY_RAW, Image::PixelFormat::MONO16); disparity_image) { frame.add_image(disparity_image.value()); } From 9235f224be2c4e9dc12bb4dddb5e5618af2dfdda Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Wed, 22 Apr 2026 17:08:25 -0400 Subject: [PATCH 12/18] Set the image calibration properly --- source/LibMultiSense/details/amb/channel.cc | 10 +++++- source/LibMultiSense/details/amb/webrtc.cc | 23 ++++++++++--- .../details/legacy/calibration.cc | 32 ------------------- source/LibMultiSense/details/utilities.cc | 32 +++++++++++++++++++ .../include/details/amb/webrtc.hh | 12 +++++-- .../include/details/legacy/calibration.hh | 11 ------- .../include/details/utilities.hh | 12 +++++++ 7 files changed, 81 insertions(+), 51 deletions(-) diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index 9ffa606b..2a5163dd 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -132,10 +132,18 @@ Status AmbChannel::connect(const Config &config) return Status::FAILED; } + // + // TODO (malvarado): Fix this + // + m_multisense_config = MultiSenseConfig(); + m_multisense_config.width = 1920; + m_multisense_config.height = 1200; + m_multisense_config.frames_per_second = 30.0f; + // // Connect our WebRTC client // - m_webtrc = std::make_unique("https://" + config.ip_address); + m_webtrc = std::make_unique("https://" + config.ip_address, m_calibration); if (!m_webtrc) { diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc index 2eebd47d..8c58d86e 100644 --- a/source/LibMultiSense/details/amb/webrtc.cc +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -34,6 +34,7 @@ * 2026-04-17, malvarado@carnegierobotics.com, IRAD, Created file. **/ +#include "details/utilities.hh" #include "details/amb/webrtc.hh" #include @@ -58,8 +59,9 @@ extern "C" namespace multisense { namespace amb { -WebRtcClient::WebRtcClient(const std::string& host) - : m_impl(nullptr) +WebRtcClient::WebRtcClient(const std::string& host, const StereoCalibration &calibration) + : m_impl(nullptr), + m_calibration(calibration) { m_impl = create_mswebrtc_impl(host.c_str()); } @@ -127,8 +129,6 @@ void WebRtcClient::c_frame_callback(void* left_data, return; } - std::cout << left_size << " " << right_size << " " << disparity_size << std::endl; - // TODO (malvarado): do this better static size_t frame_id = 0; ImageFrame frame; @@ -137,7 +137,13 @@ void WebRtcClient::c_frame_callback(void* left_data, frame.frame_time = now; frame.ptp_frame_time = now; - auto create_image = [](void* data, int size, size_t width, size_t height, DataSource source, Image::PixelFormat format) -> std::optional + auto create_image = [](void* data, + int size, + size_t width, + size_t height, + const CameraCalibration &calibration, + DataSource source, + Image::PixelFormat format) -> std::optional { if (data == nullptr || size <= 0) { @@ -156,14 +162,19 @@ void WebRtcClient::c_frame_callback(void* left_data, // TODO (malvarado) : fix this image.width = width; image.height = height; + image.calibration = calibration; return image; }; + // + // @TODO (malvarado) fix image sizes + // if (auto left_image = create_image(left_data, left_size, 1920, 1200, + client->m_calibration.left, DataSource::LEFT_RECTIFIED_RAW, Image::PixelFormat::MONO8); left_image) { @@ -174,6 +185,7 @@ void WebRtcClient::c_frame_callback(void* left_data, right_size, 1920, 1200, + client->m_calibration.right, DataSource::RIGHT_RECTIFIED_RAW, Image::PixelFormat::MONO8); right_image) { @@ -184,6 +196,7 @@ void WebRtcClient::c_frame_callback(void* left_data, disparity_size, 960, 600, + scale_calibration(client->m_calibration.left, 0.5, 0.5), DataSource::LEFT_DISPARITY_RAW, Image::PixelFormat::MONO16); disparity_image) { frame.add_image(disparity_image.value()); diff --git a/source/LibMultiSense/details/legacy/calibration.cc b/source/LibMultiSense/details/legacy/calibration.cc index 2731cdce..51a224ce 100644 --- a/source/LibMultiSense/details/legacy/calibration.cc +++ b/source/LibMultiSense/details/legacy/calibration.cc @@ -180,37 +180,5 @@ CameraCalibration select_calibration(const StereoCalibration &input, const DataS } } -CameraCalibration scale_calibration(const CameraCalibration &input, double x_scale, double y_scale) -{ - auto output = input; - - output.K[0][0] = static_cast(static_cast(output.K[0][0]) * x_scale); // fx - output.K[0][2] = static_cast(static_cast(output.K[0][2]) * x_scale); // cx - output.K[1][1] = static_cast(static_cast(output.K[1][1]) * y_scale); // fy - output.K[1][2] = static_cast(static_cast(output.K[1][2]) * y_scale); // cy - - output.P[0][0] = static_cast(static_cast(output.P[0][0]) * x_scale); // fx - output.P[0][2] = static_cast(static_cast(output.P[0][2]) * x_scale); // cx - output.P[0][3] = static_cast(static_cast(output.P[0][3]) * x_scale); // fx * tx - output.P[1][1] = static_cast(static_cast(output.P[1][1]) * y_scale); // fy - output.P[1][2] = static_cast(static_cast(output.P[1][2]) * y_scale); // cy - - return output; -} - -StereoCalibration scale_calibration(const StereoCalibration &input, double x_scale, double y_scale) -{ - auto output = input; - - output.left = scale_calibration(input.left, x_scale, y_scale); - output.right = scale_calibration(input.right, x_scale, y_scale); - if (input.aux) - { - output.aux = scale_calibration(input.aux.value(), x_scale, y_scale); - } - - return output; -} - } } diff --git a/source/LibMultiSense/details/utilities.cc b/source/LibMultiSense/details/utilities.cc index 440099a0..72304b04 100644 --- a/source/LibMultiSense/details/utilities.cc +++ b/source/LibMultiSense/details/utilities.cc @@ -600,4 +600,36 @@ std::map> parse_yaml(std::istream& stream) return data; } +CameraCalibration scale_calibration(const CameraCalibration &input, double x_scale, double y_scale) +{ + auto output = input; + + output.K[0][0] = static_cast(static_cast(output.K[0][0]) * x_scale); // fx + output.K[0][2] = static_cast(static_cast(output.K[0][2]) * x_scale); // cx + output.K[1][1] = static_cast(static_cast(output.K[1][1]) * y_scale); // fy + output.K[1][2] = static_cast(static_cast(output.K[1][2]) * y_scale); // cy + + output.P[0][0] = static_cast(static_cast(output.P[0][0]) * x_scale); // fx + output.P[0][2] = static_cast(static_cast(output.P[0][2]) * x_scale); // cx + output.P[0][3] = static_cast(static_cast(output.P[0][3]) * x_scale); // fx * tx + output.P[1][1] = static_cast(static_cast(output.P[1][1]) * y_scale); // fy + output.P[1][2] = static_cast(static_cast(output.P[1][2]) * y_scale); // cy + + return output; +} + +StereoCalibration scale_calibration(const StereoCalibration &input, double x_scale, double y_scale) +{ + auto output = input; + + output.left = scale_calibration(input.left, x_scale, y_scale); + output.right = scale_calibration(input.right, x_scale, y_scale); + if (input.aux) + { + output.aux = scale_calibration(input.aux.value(), x_scale, y_scale); + } + + return output; +} + } diff --git a/source/LibMultiSense/include/details/amb/webrtc.hh b/source/LibMultiSense/include/details/amb/webrtc.hh index 9bb72afa..161c16e1 100644 --- a/source/LibMultiSense/include/details/amb/webrtc.hh +++ b/source/LibMultiSense/include/details/amb/webrtc.hh @@ -52,7 +52,7 @@ public: /// /// @brief Create a new WebRTC client which attaches to a server at a specific hostname /// - explicit WebRtcClient(const std::string& host); + explicit WebRtcClient(const std::string& host, const StereoCalibration &calibration); ~WebRtcClient(); WebRtcClient(const WebRtcClient&) = delete; @@ -92,11 +92,19 @@ private: void* user_data); /// - /// @param Pointer to the base C WebRTC implementation + /// @brief Pointer to the base C WebRTC implementation /// void* m_impl = nullptr; + /// + /// @brief The user supplied frame callback to call when a new frame arrives + /// std::function m_frame_callback; + + /// + /// @brief Calibration to associate with the incoming images + /// + StereoCalibration m_calibration; }; } // namespace amb diff --git a/source/LibMultiSense/include/details/legacy/calibration.hh b/source/LibMultiSense/include/details/legacy/calibration.hh index d2c6269a..012bc4bf 100644 --- a/source/LibMultiSense/include/details/legacy/calibration.hh +++ b/source/LibMultiSense/include/details/legacy/calibration.hh @@ -77,16 +77,5 @@ crl::multisense::details::wire::SysCameraCalibration convert(const StereoCalibra /// CameraCalibration select_calibration(const StereoCalibration &input, const DataSource &source); -/// -/// @brief Scale a calibration used to update a full-res calibration based on the current operating resolution -/// -CameraCalibration scale_calibration(const CameraCalibration &input, double x_scale, double y_scale); - -/// -/// @brief Scale a calibration used to update a full-res calibration based on the current operating resolution -/// -StereoCalibration scale_calibration(const StereoCalibration &input, double x_scale, double y_scale); - - } } diff --git a/source/LibMultiSense/include/details/utilities.hh b/source/LibMultiSense/include/details/utilities.hh index c759fdf2..51447cca 100644 --- a/source/LibMultiSense/include/details/utilities.hh +++ b/source/LibMultiSense/include/details/utilities.hh @@ -40,6 +40,8 @@ #include #include +#include "MultiSense/MultiSenseTypes.hh" + namespace multisense{ template @@ -107,4 +109,14 @@ private: std::optional m_frame; }; +/// +/// @brief Scale a calibration used to update a full-res calibration based on the current operating resolution +/// +CameraCalibration scale_calibration(const CameraCalibration &input, double x_scale, double y_scale); + +/// +/// @brief Scale a calibration used to update a full-res calibration based on the current operating resolution +/// +StereoCalibration scale_calibration(const StereoCalibration &input, double x_scale, double y_scale); + } From 4f7b392772e5faf08c62c002204c27f24ee64098 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Wed, 22 Apr 2026 17:23:50 -0400 Subject: [PATCH 13/18] Update pointcloud generation to handle different resolution images --- .../include/MultiSense/MultiSenseUtilities.hh | 35 ++++++++++++------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh index d30a1685..ae7ab395 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh @@ -246,6 +246,9 @@ MULTISENSE_API std::optional> create_color_pointcloud(const Im size_t color_step = 0; double color_disparity_scale = 0.0; + double scale_x = 1.0; + double scale_y = 1.0; + if constexpr (std::is_same_v) { if (disparity.format != Image::PixelFormat::MONO16 || disparity.width < 0 || disparity.height < 0) @@ -263,10 +266,10 @@ MULTISENSE_API std::optional> create_color_pointcloud(const Im color_step = sizeof(Color); if (disparity.format != Image::PixelFormat::MONO16 || - color->width != disparity.width || - color->height != disparity.height || - disparity.width < 0 || - disparity.height < 0) + disparity.width <= 0 || + disparity.height <= 0 || + color->width <= 0 || + color->height <= 0) { return std::nullopt; } @@ -274,12 +277,13 @@ MULTISENSE_API std::optional> create_color_pointcloud(const Im const double tx = calibration.right.P[0][3] / calibration.right.P[0][0]; const double color_tx = color->calibration.P[0][3] / color->calibration.P[0][0]; color_disparity_scale = color_tx / tx; + + scale_x = static_cast(color->width) / static_cast(disparity.width); + scale_y = static_cast(color->height) / static_cast(disparity.height); } constexpr double scale = 1.0 / 16.0; - const double squared_range = max_range * max_range; - const QMatrix Q(disparity.calibration, calibration.right); PointCloud output; @@ -314,14 +318,19 @@ MULTISENSE_API std::optional> create_color_pointcloud(const Im } else { - // - // Use the approximation that color_pixel_u = disp_u - (tx_color/ tx) * d - // - const size_t color_index = color->image_data_offset + - (h * color->width * color_step) + - static_cast((static_cast(w) - (color_disparity_scale * d))) * color_step; + const int color_u = static_cast((static_cast(w) - (color_disparity_scale * d)) * scale_x); + const int color_v = static_cast(static_cast(h) * scale_y); + + Color color_pixel{}; - const Color color_pixel = *reinterpret_cast(color->raw_data->data() + color_index); + if (color_u >= 0 && color_u < color->width && color_v >= 0 && color_v < color->height) + { + const size_t color_index = color->image_data_offset + + (color_v * color->width * color_step) + + (color_u * color_step); + + color_pixel = *reinterpret_cast(color->raw_data->data() + color_index); + } output.cloud.push_back(Point{static_cast(x), static_cast(y), static_cast(z), color_pixel}); From d52bc034b28fdffc9b89e8f8d3725d824a6f1f95 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Wed, 22 Apr 2026 17:52:59 -0400 Subject: [PATCH 14/18] Properly scale pointclouds from different resolutions --- source/LibMultiSense/details/utilities.cc | 2 +- .../include/MultiSense/MultiSenseUtilities.hh | 12 +++++------- .../LibMultiSense/test/multisense_utilities_test.cc | 2 +- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/source/LibMultiSense/details/utilities.cc b/source/LibMultiSense/details/utilities.cc index 72304b04..f837e578 100644 --- a/source/LibMultiSense/details/utilities.cc +++ b/source/LibMultiSense/details/utilities.cc @@ -380,7 +380,7 @@ std::optional> get_aux_3d_point(const ImageFrame &frame, constexpr double scale = 1.0 / 16.0; - const QMatrix Q{disparity.calibration, frame.calibration.right}; + const QMatrix Q(disparity.calibration, frame.calibration.right.rectified_translation()[0], frame.calibration.right.P[0][2]); // // Search through our configured pixel window testing to see if the disparity value projected into the aux diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh index ae7ab395..310ba541 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh @@ -108,16 +108,14 @@ public: /// @brief Construct a minimal Q matrix from calibrations /// /// @param reference_cal The calibration corresponding to the image where disaprities are computed with - /// @param matching_cal The calibration corresponding to the image where pixels is the disparity image are matched - /// against /// - QMatrix(const CameraCalibration &reference_cal, const CameraCalibration &matching_cal): + QMatrix(const CameraCalibration &reference_cal, double tx, double cx_prime): fx_(reference_cal.P[0][0]), fy_(reference_cal.P[1][1]), cx_(reference_cal.P[0][2]), cy_(reference_cal.P[1][2]), - tx_(matching_cal.P[0][3] / matching_cal.P[0][0]), - cx_prime_(matching_cal.P[0][2]), + tx_(tx), + cx_prime_(cx_prime), fytx_(fy_ * tx_), fxtx_(fx_ * tx_), fycxtx_(fy_ * cx_ * tx_), @@ -284,7 +282,7 @@ MULTISENSE_API std::optional> create_color_pointcloud(const Im constexpr double scale = 1.0 / 16.0; const double squared_range = max_range * max_range; - const QMatrix Q(disparity.calibration, calibration.right); + const QMatrix Q(disparity.calibration, calibration.right.rectified_translation()[0], calibration.right.P[0][2] / scale_x); PointCloud output; output.cloud.reserve(disparity.width * disparity.height); @@ -300,7 +298,7 @@ MULTISENSE_API std::optional> create_color_pointcloud(const Im const double d = static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * scale; - if (d == 0.0) + if (d == 0.0 || d >= 255) { continue; } diff --git a/source/LibMultiSense/test/multisense_utilities_test.cc b/source/LibMultiSense/test/multisense_utilities_test.cc index 012534dd..0fdb4038 100644 --- a/source/LibMultiSense/test/multisense_utilities_test.cc +++ b/source/LibMultiSense/test/multisense_utilities_test.cc @@ -123,7 +123,7 @@ TEST(QMatrix, reproject) CameraCalibration::DistortionType::NONE, {}}; - QMatrix q{left_calibration, right_calibration}; + QMatrix q{left_calibration, right_calibration.rectified_translation()[0], right_calibration.P[0][2]}; // Project a dummy point into the left/right camera const double x = 0.5; From e6f432d43be5c3b6cfbbf1c88f9e8a92cd3e54f4 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 26 Jun 2026 17:51:23 -0400 Subject: [PATCH 15/18] Updates for new client API --- source/LibMultiSense/details/amb/channel.cc | 2 +- source/LibMultiSense/details/amb/http.cc | 2 +- source/LibMultiSense/details/amb/webrtc.cc | 68 ++++++++++--------- .../include/details/amb/webrtc.hh | 15 ++-- 4 files changed, 45 insertions(+), 42 deletions(-) diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index 9ffa606b..fc492834 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -46,7 +46,7 @@ #include "multisense-webrtc-client.h" -#include +#include namespace multisense { namespace amb { diff --git a/source/LibMultiSense/details/amb/http.cc b/source/LibMultiSense/details/amb/http.cc index 739a3153..79af4222 100644 --- a/source/LibMultiSense/details/amb/http.cc +++ b/source/LibMultiSense/details/amb/http.cc @@ -36,7 +36,7 @@ #include "details/amb/http.hh" -#include +#include namespace multisense{ namespace amb{ diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc index 1aaa352f..664c218e 100644 --- a/source/LibMultiSense/details/amb/webrtc.cc +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -39,20 +39,9 @@ #include #include -// Declare the extern "C" functions exposed by the Rust FFI library extern "C" { - typedef void (*mswebrtc_frame_callback)(void* left_data, int left_size, - void* right_data, int right_size, - void* disparity_data, int disparity_size, - void* user_data); - - void* create_mswebrtc_impl(const char* host); - bool connect_mswebrtc_impl(void* obj, bool left, bool right, bool disparity, bool nndata); - void disconnect_mswebrtc_impl(void* obj); - void destroy_mswebrtc_impl(void* obj); - void set_frame_callback_mswebrtc_impl(void* obj, mswebrtc_frame_callback cb, void* user_data); - void set_bitrate_mswebrtc_impl(void* obj, double bitrate); +#include "multisense-webrtc-client.h" } namespace multisense { @@ -108,12 +97,9 @@ void WebRtcClient::set_frame_callback(std::function callback) } } -void WebRtcClient::c_frame_callback(void* left_data, - int left_size, - void* right_data, - int right_size, - void* disparity_data, - int disparity_size, +void WebRtcClient::c_frame_callback(struct ImageData* left, + struct ImageData* right, + struct ImageData* disparity, void* user_data) { if (!user_data) @@ -127,45 +113,61 @@ void WebRtcClient::c_frame_callback(void* left_data, return; } - // TODO (malvarado): do this better - static size_t frame_id = 0; ImageFrame frame; - frame.frame_id = ++frame_id; - auto create_image = [](void* data, int size, DataSource source, Image::PixelFormat format) -> std::optional + auto get_pixel_format = [](struct ImageData* data) -> Image::PixelFormat { - if (data == nullptr || size <= 0) + if (data->channels == 1 && data->bytewidth == 1) + { + return Image::PixelFormat::MONO8; + } + else if (data->channels == 1 && data->bytewidth == 2) + { + return Image::PixelFormat::MONO16; + } + else if (data->channels == 3 && data->bytewidth == 1) + { + return Image::PixelFormat::BGR8; + } + + return Image::PixelFormat::UNKNOWN; + }; + + auto create_image = [get_pixel_format](struct ImageData* data, DataSource source) -> std::optional + { + if (data == nullptr || data->size <= 0 || data->data == nullptr) { return std::nullopt; } Image image; - auto buffer = std::make_shared>(size); - std::memcpy(buffer->data(), data, size); + auto buffer = std::make_shared>(data->size, 0); + std::memcpy(buffer->data(), data->data, 0); image.raw_data = buffer; image.image_data_offset = 0; - image.image_data_length = size; + image.image_data_length = data->size; image.source = source; - image.format = format; - // TODO (malvarado) : fix this - image.width = 1920; - image.height = 1200; + + image.format = get_pixel_format(data); + + image.width = data->width; + image.height = data->height; return image; }; - if (auto left_image = create_image(left_data, left_size, DataSource::LEFT_RECTIFIED_RAW, Image::PixelFormat::MONO8); left_image) + if (auto left_image = create_image(left, DataSource::LEFT_RECTIFIED_RAW); left_image) { frame.add_image(left_image.value()); } - if (auto right_image = create_image(right_data, right_size, DataSource::RIGHT_RECTIFIED_RAW, Image::PixelFormat::MONO8); right_image) + if (auto right_image = create_image(right, DataSource::RIGHT_RECTIFIED_RAW); right_image) { frame.add_image(right_image.value()); } - if (auto disparity_image = create_image(disparity_data, disparity_size, DataSource::LEFT_DISPARITY_RAW, Image::PixelFormat::MONO16); disparity_image) + if (auto disparity_image = create_image(disparity, DataSource::LEFT_DISPARITY_RAW); disparity_image) { frame.add_image(disparity_image.value()); } diff --git a/source/LibMultiSense/include/details/amb/webrtc.hh b/source/LibMultiSense/include/details/amb/webrtc.hh index 9bb72afa..92072f03 100644 --- a/source/LibMultiSense/include/details/amb/webrtc.hh +++ b/source/LibMultiSense/include/details/amb/webrtc.hh @@ -41,6 +41,10 @@ #include "MultiSense/MultiSenseTypes.hh" +// Forward declarations in global namespace to match the C library types +struct ImageData; +struct MswebrtcImpl; + namespace multisense { namespace amb { @@ -83,18 +87,15 @@ private: /// /// @brief Private callback signature which the c client WebRTC uses to return images /// - static void c_frame_callback(void* left_data, - int left_size, - void* right_data, - int right_size, - void* disparity_data, - int disparity_size, + static void c_frame_callback(struct ImageData* left, + struct ImageData* right, + struct ImageData* disparity, void* user_data); /// /// @param Pointer to the base C WebRTC implementation /// - void* m_impl = nullptr; + struct MswebrtcImpl* m_impl = nullptr; std::function m_frame_callback; }; From c770148bfc7abd84dec339c26d412970cd3be799 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 26 Jun 2026 18:05:52 -0400 Subject: [PATCH 16/18] Copy the image --- source/LibMultiSense/details/amb/channel.cc | 1 - source/LibMultiSense/details/amb/webrtc.cc | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/source/LibMultiSense/details/amb/channel.cc b/source/LibMultiSense/details/amb/channel.cc index ffb69955..f3dd08cf 100644 --- a/source/LibMultiSense/details/amb/channel.cc +++ b/source/LibMultiSense/details/amb/channel.cc @@ -157,7 +157,6 @@ Status AmbChannel::connect(const Config &config) m_webtrc->set_frame_callback([this](ImageFrame& frame) { - std::cout << "got new frame" << std::endl; frame.calibration = m_calibration; if (m_user_image_frame_callback) { diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc index 6299b26b..e4bd45e1 100644 --- a/source/LibMultiSense/details/amb/webrtc.cc +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -132,7 +132,7 @@ void WebRtcClient::c_frame_callback(struct ImageData* left, Image image; auto buffer = std::make_shared>(data->size, 0); - std::memcpy(buffer->data(), data->data, 0); + std::memcpy(buffer->data(), data->data, data->size); image.raw_data = buffer; image.image_data_offset = 0; From c857c6883b82b0636e9038175ffc9d82912d0b4a Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 30 Jun 2026 11:43:25 -0400 Subject: [PATCH 17/18] Use the latest webrtc api --- python/bindings.cc | 17 ++-- source/LibMultiSense/details/amb/webrtc.cc | 77 ++++++++++------- .../LibMultiSense/details/legacy/channel.cc | 21 ++--- source/LibMultiSense/details/utilities.cc | 29 +++---- .../include/MultiSense/MultiSenseTypes.hh | 85 ++++++++++++++++--- .../include/MultiSense/MultiSenseUtilities.hh | 7 +- .../include/details/amb/webrtc.hh | 9 +- 7 files changed, 159 insertions(+), 86 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index c664fc74..6760354f 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -202,6 +202,13 @@ PYBIND11_MODULE(_libmultisense, m) { strides)); }); + // BufferWrapper + py::class_(m, "BufferWrapper") + .def(py::init> , size_t>()) + .def(py::init()) + .def("data", &multisense::BufferWrapper::data) + .def("size", &multisense::BufferWrapper::data); + // Image py::class_(m, "Image") .def(py::init<>()) @@ -211,12 +218,10 @@ PYBIND11_MODULE(_libmultisense, m) { if (image.format == multisense::Image::PixelFormat::H264 || image.format == multisense::Image::PixelFormat::JPEG) { - const SSizeVector shape = { - static_cast(image.raw_data->size() - image.image_data_offset) - }; + const SSizeVector shape = { static_cast(image.raw_data->size()) }; const SSizeVector strides = {static_cast(sizeof(uint8_t))}; return py::array(py::buffer_info( - const_cast(image.raw_data->data() + image.image_data_offset), + const_cast(image.raw_data->data()), static_cast(sizeof(uint8_t)), py::format_descriptor::format(), 1, @@ -270,7 +275,7 @@ PYBIND11_MODULE(_libmultisense, m) { // Map the cv::Mat to a NumPy array without copying the data return py::array(py::buffer_info( - const_cast(image.raw_data->data() + image.image_data_offset), + const_cast(image.raw_data->data()), element_size, format, static_cast(shape.size()), @@ -830,7 +835,7 @@ PYBIND11_MODULE(_libmultisense, m) { // Utilities py::class_(m, "QMatrix") - .def(py::init()) + .def(py::init()) .def("reproject", &multisense::QMatrix::reproject) .def("matrix", &multisense::QMatrix::matrix); diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc index e4bd45e1..a86a55ba 100644 --- a/source/LibMultiSense/details/amb/webrtc.cc +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -99,11 +99,15 @@ void WebRtcClient::set_frame_callback(std::function callback) } } -void WebRtcClient::c_frame_callback(struct ImageData* left, - struct ImageData* right, - struct ImageData* disparity, +void WebRtcClient::c_frame_callback(uint64_t timestamp, + uint32_t frame_id, + const struct ImageData* left, + const struct ImageData* right, + const struct ImageData* disparity, + const struct ImageData* nndata, void* user_data) { + (void) nndata; if (!user_data) { return; @@ -117,69 +121,84 @@ void WebRtcClient::c_frame_callback(struct ImageData* left, ImageFrame frame; - static size_t frame_id = 0; - frame.frame_id = ++frame_id; - const auto now = std::chrono::time_point_cast(std::chrono::system_clock::now()); - frame.frame_time = now; - frame.ptp_frame_time = now; + frame.frame_id = frame_id; + const std::chrono::nanoseconds timestamp_ns(timestamp); + const TimeT image_time{timestamp_ns}; + frame.frame_time = image_time; + frame.ptp_frame_time = image_time; - auto create_image = [](struct ImageData* data, const CameraCalibration &calibration, DataSource source) -> std::optional + auto create_image = [](const struct ImageData* data, + const CameraCalibration &calibration, + DataSource source, + const TimeT &image_timestamp) -> std::optional { if (data == nullptr || data->size <= 0 || data->data == nullptr) { return std::nullopt; } - Image image; - auto buffer = std::make_shared>(data->size, 0); - std::memcpy(buffer->data(), data->data, data->size); + // + // Increment our reference count, and convert the raw data ptr into a shared_ptr which will + // decrement the reference count once destructed + // + incref_imagedata(const_cast(data)); + auto data_p = static_cast(data->data); + std::shared_ptr buffer(new BufferWrapper(data_p, static_cast(data->size)), + [data](const BufferWrapper*) + { + decref_imagedata(const_cast(data)); + }); - image.raw_data = buffer; - image.image_data_offset = 0; - image.image_data_length = data->size; - image.source = source; + Image::PixelFormat format = Image::PixelFormat::UNKNOWN; if (data->channels == 1 && data->bytewidth == 1) { - image.format = Image::PixelFormat::MONO8; + format = Image::PixelFormat::MONO8; } else if (data->channels == 1 && data->bytewidth == 2) { - image.format = Image::PixelFormat::MONO16; + format = Image::PixelFormat::MONO16; } else if (data->channels == 3 && data->bytewidth == 1) { - image.format = Image::PixelFormat::BGR8; + format = Image::PixelFormat::BGR8; } else { - image.format = Image::PixelFormat::UNKNOWN; + format = Image::PixelFormat::UNKNOWN; } - image.width = data->width; - image.height = data->height; - image.calibration = calibration; - - return image; + return Image{std::move(buffer), + format, + data->width, + data->height, + image_timestamp, + image_timestamp, + source, + calibration}; }; if (auto left_image = create_image(left, client->m_calibration.left, - DataSource::LEFT_RECTIFIED_RAW); left_image) + DataSource::LEFT_RECTIFIED_RAW, + image_time); left_image) { frame.add_image(left_image.value()); } if (auto right_image = create_image(right, client->m_calibration.right, - DataSource::RIGHT_RECTIFIED_RAW); right_image) + DataSource::RIGHT_RECTIFIED_RAW, + image_time); right_image) { frame.add_image(right_image.value()); } + // TODO (malvarado): Scale this calibration if (auto disparity_image = create_image(disparity, - scale_calibration(client->m_calibration.left, 0.5, 0.5), - DataSource::LEFT_DISPARITY_RAW); disparity_image) + client->m_calibration.left, + DataSource::LEFT_DISPARITY_RAW, + image_time); disparity_image) { frame.add_image(disparity_image.value()); } diff --git a/source/LibMultiSense/details/legacy/channel.cc b/source/LibMultiSense/details/legacy/channel.cc index 3831319a..9593d1bc 100644 --- a/source/LibMultiSense/details/legacy/channel.cc +++ b/source/LibMultiSense/details/legacy/channel.cc @@ -1464,9 +1464,8 @@ void LegacyChannel::image_callback(std::shared_ptr> d const auto cal_x_scale = static_cast(wire_image.width) / static_cast(info.imager_width); const auto cal_y_scale = static_cast(wire_image.height) / static_cast(info.imager_height); - Image image{data, - static_cast(wire_image.dataP) - data->data(), - ((wire_image.bitsPerPixel / 8) * wire_image.width * wire_image.height), + const size_t offset = static_cast(wire_image.dataP) - data->data(); + Image image{std::make_shared(std::move(data), offset), pixel_format, wire_image.width, wire_image.height, @@ -1533,9 +1532,9 @@ void LegacyChannel::compressed_image_callback(std::shared_ptr(wire_image.width) / static_cast(info.imager_width); const auto cal_y_scale = static_cast(wire_image.height) / static_cast(info.imager_height); - Image image{data, - static_cast(wire_image.dataP) - data->data(), - ((wire_image.bitsPerPixel / 8) * wire_image.width * wire_image.height), + + const size_t offset = static_cast(wire_image.dataP) - data->data(); + Image image{std::make_shared(std::move(data), offset), pixel_format, wire_image.width, wire_image.height, @@ -1583,11 +1582,6 @@ void LegacyChannel::disparity_callback(std::shared_ptr(((static_cast(wire::Disparity::API_BITS_PER_PIXEL) / 8.0) * - wire_image.width * - wire_image.height)); - // // Copy our calibration and device info locally to make this thread safe // @@ -1602,9 +1596,8 @@ void LegacyChannel::disparity_callback(std::shared_ptr(wire_image.width) / static_cast(info.imager_width); const auto cal_y_scale = static_cast(wire_image.height) / static_cast(info.imager_height); - Image image{data, - static_cast(wire_image.dataP) - data->data(), - disparity_length, + const size_t offset = static_cast(wire_image.dataP) - data->data(); + Image image{std::make_shared(std::move(data), offset), pixel_format, wire_image.width, wire_image.height, diff --git a/source/LibMultiSense/details/utilities.cc b/source/LibMultiSense/details/utilities.cc index df733976..e14664f6 100644 --- a/source/LibMultiSense/details/utilities.cc +++ b/source/LibMultiSense/details/utilities.cc @@ -84,8 +84,7 @@ bool write_binary_image(const Image &image, const std::filesystem::path &path) << image.width << " " << image.height << "\n" << 0xFF << "\n"; - output.write(reinterpret_cast(image.raw_data->data()) + image.image_data_offset, - image.image_data_length); + output.write(reinterpret_cast(image.raw_data->data()), image.raw_data->size()); break; } case Image::PixelFormat::MONO16: @@ -97,7 +96,7 @@ bool write_binary_image(const Image &image, const std::filesystem::path &path) // // Make sure we swap our byte order if needed // - const uint16_t* raw_data = reinterpret_cast(image.raw_data->data() + image.image_data_offset); + const uint16_t* raw_data = reinterpret_cast(image.raw_data->data()); for (int i = 0 ; i < (image.width * image.height) ; ++i) { const uint16_t o = htons(raw_data[i]); @@ -117,7 +116,7 @@ bool write_binary_image(const Image &image, const std::filesystem::path &path) // for (int i = 0 ; i < (image.width * image.height) ; ++i) { - const auto bgr = reinterpret_cast*>(image.raw_data->data() + image.image_data_offset + (i * 3)); + const auto bgr = reinterpret_cast*>(image.raw_data->data() + (i * 3)); const std::array rgb{bgr->at(2), bgr->at(1), bgr->at(0)}; output.write(reinterpret_cast(rgb.data()), sizeof(rgb)); } @@ -209,7 +208,7 @@ cv::Mat Image::cv_mat() const return cv::Mat{height, width, cv_type, - const_cast(raw_data->data() + image_data_offset)}; + const_cast(raw_data->data())}; } std::vector FeatureMessage::cv_keypoints() const @@ -330,7 +329,7 @@ std::optional create_depth_image(const ImageFrame &frame, for (size_t i = 0 ; i < static_cast(disparity.width * disparity.height) ; ++i) { - const size_t index = disparity.image_data_offset + (i * sizeof(uint16_t)); + const size_t index = (i * sizeof(uint16_t)); const size_t u = i % disparity.width; const size_t v = i / disparity.width; @@ -377,9 +376,7 @@ std::optional create_depth_image(const ImageFrame &frame, } } - return Image{data, - 0, - data->size(), + return Image{std::make_shared(std::move(data), 0), depth_format, disparity.width, disparity.height, @@ -439,7 +436,7 @@ std::optional> get_aux_3d_point(const ImageFrame &frame, break; } - const size_t index = disparity.image_data_offset + ((search_u + (rectified_aux_pixel.v * disparity.width)) * sizeof(uint16_t)); + const size_t index = (search_u + (rectified_aux_pixel.v * disparity.width)) * sizeof(uint16_t); const double d = static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * scale; @@ -466,7 +463,7 @@ std::optional create_bgr_from_ycbcr420(const Image &luma, const Image &ch return std::nullopt; } - const size_t color_length = luma.image_data_length * 3; + const size_t color_length = luma.raw_data->size() * 3; std::vector raw_data(color_length, static_cast(0)); @@ -479,9 +476,9 @@ std::optional create_bgr_from_ycbcr420(const Image &luma, const Image &ch const size_t luma_offset = (h * luma.width) + w; const size_t chroma_offset = 2 * (((h / 2) * (luma.width / 2)) + (w / 2)); - const float px_y = static_cast(*(luma.raw_data->data() + luma.image_data_offset + luma_offset)); - const float px_cb = static_cast(*(chroma.raw_data->data() + chroma.image_data_offset + chroma_offset)) - 128.0f; - const float px_cr = static_cast(*(chroma.raw_data->data() + chroma.image_data_offset + chroma_offset + 1)) - 128.0f; + const float px_y = static_cast(*(luma.raw_data->data() + luma_offset)); + const float px_cb = static_cast(*(chroma.raw_data->data() + chroma_offset)) - 128.0f; + const float px_cr = static_cast(*(chroma.raw_data->data() + chroma_offset + 1)) - 128.0f; const float px_r = std::clamp(px_y + 1.13983f * px_cr, 0.0f, 255.0f); const float px_g = std::clamp(px_y - 0.39465f * px_cb - 0.58060f * px_cr, 0.0f, 255.0f); @@ -495,9 +492,7 @@ std::optional create_bgr_from_ycbcr420(const Image &luma, const Image &ch } } - return Image{std::make_shared>(std::move(raw_data)), - 0, - color_length, + return Image{std::make_shared(std::make_shared>(std::move(raw_data)), 0), Image::PixelFormat::BGR8, luma.width, luma.height, diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 47bd9fd7..c0be4701 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -194,6 +194,77 @@ struct StereoCalibration std::optional aux = std::nullopt; }; +/// +/// @brief Helper class for handling shared_ptrs to vectors or raw data. This allows us +/// to wrap arbitrary incoming data to avoid memcpy. +/// +/// Note when interfacing with raw data the user is responsable for cleaning up the memory +/// outside the scope of this class +/// +class BufferWrapper +{ +public: + + /// + /// @brief Construct from a shared_ptr. Keep the shared ptr valid throughout the duration of this + /// object lifetime + /// + BufferWrapper(std::shared_ptr> data, size_t offset): + m_data(std::move(data)), + m_raw_data(m_data->data() + offset), + m_size(m_data->size() - offset) + { + if (offset >= m_data->size()) + { + throw std::runtime_error("Invalid pointer offset"); + } + } + + /// + /// @brief Wrap a raw buffer with no associated shared_ptr. The user is responsible for freeing this memory once + /// this BufferWrapper object goes out of scope + /// + BufferWrapper(const uint8_t* data, size_t size): + m_data(nullptr), + m_raw_data(data), + m_size(size) + { + } + + /// + /// @brief Access the size of the raw data buffer in bytes + /// + size_t size() const + { + return m_size; + } + + /// + /// @brief Access the raw data buffer + /// + const uint8_t* data() const + { + return m_raw_data; + }; + +private: + + /// + /// @brief Keep a local copy of a shared pointer to ensure the associated raw ptr memory stays valid + /// + const std::shared_ptr> m_data{nullptr}; + + /// + /// @brief Raw buffer wrapped + /// + const uint8_t* m_raw_data{nullptr}; + + /// + /// @brief Size of the raw buffer in bytes + /// + const size_t m_size{0}; +}; + /// /// @brief Represents a single image plus metadata /// @@ -216,17 +287,7 @@ struct Image /// /// @brief A pointer to the raw image data sent from the camera /// - std::shared_ptr> raw_data = nullptr; - - /// - /// @brief An offset into the raw_data pointer where the image data starts - /// - int64_t image_data_offset = 0; - - /// - /// @brief The length of the image data after the image_data_offset has been applied - /// - size_t image_data_length = 0; + std::shared_ptr raw_data = nullptr; /// /// @brief The format of the image data stored in the raw_data stored in the raw_data buffer @@ -289,7 +350,7 @@ struct Image { const size_t offset = sizeof(T) * ((width * h) + w); - return *reinterpret_cast(raw_data->data() + image_data_offset + offset); + return *reinterpret_cast(raw_data->data() + offset); } return std::nullopt; diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh index e449c7fb..65fc89bb 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh @@ -313,8 +313,7 @@ std::optional> create_color_pointcloud(const Image &disparity, { for (size_t w = 0 ; w < static_cast(disparity.width) ; ++w) { - const size_t index = disparity.image_data_offset + - (h * disparity.width * sizeof(uint16_t)) + + const size_t index = (h * disparity.width * sizeof(uint16_t)) + (w * sizeof(uint16_t)); const double d = @@ -345,9 +344,7 @@ std::optional> create_color_pointcloud(const Image &disparity, if (color_u >= 0 && color_u < color->width && color_v >= 0 && color_v < color->height) { - const size_t color_index = color->image_data_offset + - (color_v * color->width * color_step) + - (color_u * color_step); + const size_t color_index = (color_v * color->width * color_step) + (color_u * color_step); color_pixel = *reinterpret_cast(color->raw_data->data() + color_index); } diff --git a/source/LibMultiSense/include/details/amb/webrtc.hh b/source/LibMultiSense/include/details/amb/webrtc.hh index 0f1c1eb4..12cafeec 100644 --- a/source/LibMultiSense/include/details/amb/webrtc.hh +++ b/source/LibMultiSense/include/details/amb/webrtc.hh @@ -87,9 +87,12 @@ private: /// /// @brief Private callback signature which the c client WebRTC uses to return images /// - static void c_frame_callback(struct ImageData* left, - struct ImageData* right, - struct ImageData* disparity, + static void c_frame_callback(uint64_t timestamp, + uint32_t frame_id, + const struct ImageData* left, + const struct ImageData* right, + const struct ImageData* disparity, + const struct ImageData* nndata, void* user_data); /// From 38bed50d52d601d5e3cc6c595b6a90580116b638 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Tue, 30 Jun 2026 13:03:14 -0400 Subject: [PATCH 18/18] Fix unit tests --- python/bindings.cc | 3 +- source/LibMultiSense/details/amb/webrtc.cc | 5 +- .../LibMultiSense/details/legacy/channel.cc | 9 +- source/LibMultiSense/details/utilities.cc | 16 +- .../include/MultiSense/MultiSenseTypes.hh | 14 +- .../include/MultiSense/MultiSenseUtilities.hh | 4 +- source/LibMultiSense/test/calibration_test.cc | 1 + .../test/multisense_utilities_test.cc | 174 ++++++++++-------- 8 files changed, 129 insertions(+), 97 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index 6760354f..06f7234f 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -288,7 +288,8 @@ PYBIND11_MODULE(_libmultisense, m) { .def_readonly("camera_timestamp", &multisense::Image::camera_timestamp) .def_readonly("ptp_timestamp", &multisense::Image::ptp_timestamp) .def_readonly("source", &multisense::Image::source) - .def_readonly("calibration", &multisense::Image::calibration); + .def_readonly("calibration", &multisense::Image::calibration) + .def_readonly("pixel_scale", &multisense::Image::pixel_scale); // ImageHistogram py::class_(m, "ImageHistogram") diff --git a/source/LibMultiSense/details/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc index a86a55ba..4f2b92df 100644 --- a/source/LibMultiSense/details/amb/webrtc.cc +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -175,7 +175,8 @@ void WebRtcClient::c_frame_callback(uint64_t timestamp, image_timestamp, image_timestamp, source, - calibration}; + calibration, + 1.0/static_cast(1 << data->precision)}; }; if (auto left_image = create_image(left, @@ -196,7 +197,7 @@ void WebRtcClient::c_frame_callback(uint64_t timestamp, // TODO (malvarado): Scale this calibration if (auto disparity_image = create_image(disparity, - client->m_calibration.left, + scale_calibration(client->m_calibration.left, 0.5, 0.5), DataSource::LEFT_DISPARITY_RAW, image_time); disparity_image) { diff --git a/source/LibMultiSense/details/legacy/channel.cc b/source/LibMultiSense/details/legacy/channel.cc index 9593d1bc..3b7b6a3b 100644 --- a/source/LibMultiSense/details/legacy/channel.cc +++ b/source/LibMultiSense/details/legacy/channel.cc @@ -1472,7 +1472,8 @@ void LegacyChannel::image_callback(std::shared_ptr> d capture_time_point, ptp_capture_time_point, source.front(), - scale_calibration(select_calibration(cal, source.front()), cal_x_scale, cal_y_scale)}; + scale_calibration(select_calibration(cal, source.front()), cal_x_scale, cal_y_scale), + 1.0}; handle_and_dispatch(std::move(image), meta->second, @@ -1541,7 +1542,8 @@ void LegacyChannel::compressed_image_callback(std::shared_ptrsecond, @@ -1604,7 +1606,8 @@ void LegacyChannel::disparity_callback(std::shared_ptrsecond, diff --git a/source/LibMultiSense/details/utilities.cc b/source/LibMultiSense/details/utilities.cc index e14664f6..874ff304 100644 --- a/source/LibMultiSense/details/utilities.cc +++ b/source/LibMultiSense/details/utilities.cc @@ -322,11 +322,7 @@ std::optional create_depth_image(const ImageFrame &frame, } } - // - // MONO16 disparity images are quantized to 1/16th of a pixel - // - constexpr double scale = 1.0 / 16.0; - + const auto pixel_scale = disparity.pixel_scale ? disparity.pixel_scale.value() : 1.0; for (size_t i = 0 ; i < static_cast(disparity.width * disparity.height) ; ++i) { const size_t index = (i * sizeof(uint16_t)); @@ -336,7 +332,7 @@ std::optional create_depth_image(const ImageFrame &frame, const double d = - static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * scale; + static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * pixel_scale; const double scaled_u = std::round(u - (baseline_ratio * d)); @@ -383,7 +379,8 @@ std::optional create_depth_image(const ImageFrame &frame, disparity.camera_timestamp, disparity.ptp_timestamp, disparity.source, - disparity.calibration}; + disparity.calibration, + depth_format == Image::PixelFormat::MONO16 ? 1.0/1000.0 : 1.0}; } std::optional> get_aux_3d_point(const ImageFrame &frame, @@ -418,8 +415,6 @@ std::optional> get_aux_3d_point(const ImageFrame &frame, const double tx_aux = frame.calibration.aux->P[0][3] / frame.calibration.aux->P[0][0]; const double baseline_ratio = tx_aux / tx; - constexpr double scale = 1.0 / 16.0; - const QMatrix Q(disparity.calibration, frame.calibration.right.rectified_translation()[0], frame.calibration.right.P[0][2]); // @@ -427,6 +422,7 @@ std::optional> get_aux_3d_point(const ImageFrame &frame, // image aligns with our query pixel. See: // https://docs.carnegierobotics.com/cookbook/overview.html#approximation-for-execution-speed // + const auto pixel_scale = disparity.pixel_scale ? disparity.pixel_scale.value() : 1.0; for (size_t i = 0 ; i < max_pixel_search_window ; ++i) { const size_t search_u = rectified_aux_pixel.u + i; @@ -439,7 +435,7 @@ std::optional> get_aux_3d_point(const ImageFrame &frame, const size_t index = (search_u + (rectified_aux_pixel.v * disparity.width)) * sizeof(uint16_t); const double d = - static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * scale; + static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * pixel_scale; if (d == 0) { diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index c0be4701..6b0ba4ea 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -168,11 +168,11 @@ struct CameraCalibration /// @brief Get the translation vector in meters which translates points in the current CameraCalibration frame /// to the origin left camera frame /// - std::array rectified_translation() const + std::array rectified_translation() const { - return std::array{(P[0][0] == 0.0f ? 0.0f : P[0][3] / P[0][0]), - (P[1][1] == 0.0f ? 0.0f : P[1][3] / P[1][1]), - P[2][3]}; + return std::array{(P[0][0] == 0.0f ? 0.0f : static_cast(P[0][3]) / static_cast(P[0][0])), + (P[1][1] == 0.0f ? 0.0f : static_cast(P[1][3]) / static_cast(P[1][1])), + static_cast(P[2][3])}; } }; @@ -326,6 +326,12 @@ struct Image /// CameraCalibration calibration{}; + /// + /// @brief A scale we should optionally apply to each pixel. This is used to handle quantization for data + /// sources like disparity + /// + std::optional pixel_scale = std::nullopt; + /// /// @brief Get a pixel at a certain width/height location in the image. /// NOTE: This check is slower since it checks to make sure the request is safe. It diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh index 65fc89bb..4f4b2d3a 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh @@ -302,13 +302,13 @@ std::optional> create_color_pointcloud(const Image &disparity, scale_y = static_cast(color->height) / static_cast(disparity.height); } - constexpr double scale = 1.0 / 16.0; const double squared_range = max_range * max_range; const QMatrix Q(disparity.calibration, calibration.right.rectified_translation()[0], calibration.right.P[0][2] / scale_x); PointCloud output; output.cloud.reserve(disparity.width * disparity.height); + const auto pixel_scale = disparity.pixel_scale ? disparity.pixel_scale.value() : 1.0; for (size_t h = 0 ; h < static_cast(disparity.height) ; ++h) { for (size_t w = 0 ; w < static_cast(disparity.width) ; ++w) @@ -317,7 +317,7 @@ std::optional> create_color_pointcloud(const Image &disparity, (w * sizeof(uint16_t)); const double d = - static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * scale; + static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * pixel_scale; if (d == 0.0 || d >= 255) { diff --git a/source/LibMultiSense/test/calibration_test.cc b/source/LibMultiSense/test/calibration_test.cc index ebd6f2c3..ce4ba324 100644 --- a/source/LibMultiSense/test/calibration_test.cc +++ b/source/LibMultiSense/test/calibration_test.cc @@ -37,6 +37,7 @@ #include #include
+#include
using namespace multisense::legacy; diff --git a/source/LibMultiSense/test/multisense_utilities_test.cc b/source/LibMultiSense/test/multisense_utilities_test.cc index 0a36a908..e25de741 100644 --- a/source/LibMultiSense/test/multisense_utilities_test.cc +++ b/source/LibMultiSense/test/multisense_utilities_test.cc @@ -90,16 +90,15 @@ Image create_example_disparity_image(const CameraCalibration &left_calibration, } } - return Image{std::make_shared>(data), - 0, - width * height * sizeof(uint16_t), + return Image{std::make_shared(std::make_shared>(std::move(data)), 0), Image::PixelFormat::MONO16, static_cast(width), static_cast(height), {}, {}, DataSource::LEFT_DISPARITY_RAW, - left_calibration}; + left_calibration, + 1.0/16.0}; } TEST(QMatrix, reproject) @@ -173,7 +172,7 @@ TEST(QMatrix, matrix) CameraCalibration::DistortionType::NONE, {}}; - QMatrix q{left_calibration, right_calibration}; + QMatrix q{left_calibration, right_calibration.rectified_translation()[0], right_calibration.P[0][2]}; const auto matrix = q.matrix(); @@ -402,27 +401,25 @@ TEST(create_bgr_from_ycbcr420, gray_image) // Values of 128 for cb/cr result in 0 values for the corresponding color pixels std::vector cbcr_data(width * height/2, 128); - const Image y{std::make_shared>(std::move(y_data)), - 0, - width * height, + const Image y{std::make_shared(y_data.data(), y_data.size()), Image::PixelFormat::MONO8, static_cast(width), static_cast(height), {}, {}, DataSource::AUX_LUMA_RAW, - aux_calibration}; + aux_calibration, + std::nullopt}; - const Image cbcr{std::make_shared>(std::move(cbcr_data)), - 0, - width / 2 * height / 2, + const Image cbcr{std::make_shared(cbcr_data.data(), cbcr_data.size()), Image::PixelFormat::MONO16, static_cast(width/2), static_cast(height/2), {}, {}, DataSource::AUX_CHROMA_RAW, - aux_calibration}; + aux_calibration, + std::nullopt}; const auto bgr_image = create_bgr_from_ycbcr420(y, cbcr, DataSource::AUX_RAW); @@ -600,16 +597,15 @@ TEST(write_image, unsupported_extension) {}}; std::vector data(10 * 10, 0); - Image img{std::make_shared>(data), - 0, - 100, + Image img{std::make_shared(data.data(), data.size()), Image::PixelFormat::MONO8, 10, 10, {}, {}, DataSource::LEFT_MONO_RAW, - aux_calibration}; + aux_calibration, + std::nullopt}; const auto path = std::filesystem::temp_directory_path() / "test.unsupported"; EXPECT_FALSE(write_image(img, path)); @@ -630,9 +626,7 @@ TEST(write_image, pgm_mono8) {}}; std::vector data(10 * 10, 128); - Image img{std::make_shared>(data), - 0, - 100, + Image img{std::make_shared(data.data(), data.size()), Image::PixelFormat::MONO8, 10, 10, @@ -661,16 +655,15 @@ TEST(write_image, pgm_mono16) {}}; std::vector data(10 * 10 * 2, 128); - Image img{std::make_shared>(data), - 0, - 200, + Image img{std::make_shared(data.data(), data.size()), Image::PixelFormat::MONO16, 10, 10, {}, {}, DataSource::LEFT_DISPARITY_RAW, - aux_calibration}; + aux_calibration, + std::nullopt}; const auto path = std::filesystem::temp_directory_path() / "test.pgm"; EXPECT_TRUE(write_image(img, path)); @@ -692,16 +685,15 @@ TEST(write_image, ppm_bgr8) {}}; std::vector data(10 * 10 * 3, 128); - Image img{std::make_shared>(data), - 0, - 300, + Image img{std::make_shared(data.data(), data.size()), Image::PixelFormat::BGR8, 10, 10, {}, {}, DataSource::LEFT_MONO_RAW, - aux_calibration}; + aux_calibration, + std::nullopt}; const auto path = std::filesystem::temp_directory_path() / "test.ppm"; EXPECT_TRUE(write_image(img, path)); @@ -723,16 +715,15 @@ TEST(write_image, unhandled_format) {}}; std::vector data(10 * 10 * 4, 128); - Image img{std::make_shared>(data), - 0, - 400, + Image img{std::make_shared(data.data(), data.size()), Image::PixelFormat::FLOAT32, 10, 10, {}, {}, DataSource::LEFT_MONO_RAW, - aux_calibration}; + aux_calibration, + std::nullopt}; const auto path = std::filesystem::temp_directory_path() / "test.ppm"; EXPECT_FALSE(write_image(img, path)); @@ -754,16 +745,15 @@ TEST(write_image, invalid_file_path) {}}; std::vector data(10 * 10, 128); - Image img{std::make_shared>(data), - 0, - 100, + Image img{std::make_shared(data.data(), data.size()), Image::PixelFormat::MONO8, 10, 10, {}, {}, DataSource::LEFT_MONO_RAW, - aux_calibration}; + aux_calibration, + std::nullopt}; EXPECT_FALSE(write_image(img, "/invalid_path_that_does_not_exist/test.pgm")); } @@ -789,16 +779,15 @@ TEST(create_depth_image, invalid_disparity_format) {}}; std::vector data(10 * 10, 0); - Image disparity{std::make_shared>(data), - 0, - 100, - Image::PixelFormat::MONO8, // Should be MONO16 - 10, - 10, - {}, - {}, - DataSource::LEFT_DISPARITY_RAW, - cal}; + Image disparity{std::make_shared(data.data(), data.size()), + Image::PixelFormat::MONO8, // Should be MONO16 + 10, + 10, + {}, + {}, + DataSource::LEFT_DISPARITY_RAW, + cal, + 1.0/16.0}; ImageFrame frame{0, {}, {}, StereoCalibration{cal, cal, std::nullopt}, {}, {}, {}, {}, {}, {}}; frame.add_image(disparity); @@ -869,16 +858,15 @@ TEST(get_aux_3d_point, invalid_disparity_format) {}}; std::vector data(10 * 10, 0); - Image disparity{std::make_shared>(data), - 0, - 100, - Image::PixelFormat::MONO8, // Should be MONO16 - 10, - 10, - {}, - {}, - DataSource::LEFT_DISPARITY_RAW, - cal}; + Image disparity{std::make_shared(data.data(), data.size()), + Image::PixelFormat::MONO8, // Should be MONO16 + 10, + 10, + {}, + {}, + DataSource::LEFT_DISPARITY_RAW, + cal, + 1.0/16.0}; ImageFrame frame{0, {}, {}, StereoCalibration{cal, cal, cal}, {}, {}, {}, {}, {}, {}}; frame.add_image(disparity); @@ -927,19 +915,44 @@ TEST(create_bgr_from_ycbcr420, invalid_formats) std::vector y_data(width * height, 42); std::vector cbcr_data(width * height/2, 128); - Image y_valid{std::make_shared>(y_data), - 0, width * height, Image::PixelFormat::MONO8, - static_cast(width), static_cast(height), {}, {}, DataSource::AUX_LUMA_RAW, aux_calibration}; - Image y_invalid{std::make_shared>(y_data), - 0, width * height, Image::PixelFormat::MONO16, // INVALID - static_cast(width), static_cast(height), {}, {}, DataSource::AUX_LUMA_RAW, aux_calibration}; - - Image cbcr_valid{std::make_shared>(cbcr_data), - 0, width / 2 * height / 2, Image::PixelFormat::MONO16, - static_cast(width/2), static_cast(height/2), {}, {}, DataSource::AUX_CHROMA_RAW, aux_calibration}; - Image cbcr_invalid{std::make_shared>(cbcr_data), - 0, width / 2 * height / 2, Image::PixelFormat::MONO8, // INVALID - static_cast(width/2), static_cast(height/2), {}, {}, DataSource::AUX_CHROMA_RAW, aux_calibration}; + Image y_valid{std::make_shared(y_data.data(), y_data.size()), + Image::PixelFormat::MONO8, + static_cast(width), + static_cast(height), + {}, + {}, + DataSource::AUX_LUMA_RAW, + aux_calibration, + std::nullopt}; + + Image y_invalid{std::make_shared(y_data.data(), y_data.size()), + Image::PixelFormat::MONO16, //INVALID + static_cast(width), + static_cast(height), + {}, + {}, + DataSource::AUX_LUMA_RAW, + aux_calibration, + std::nullopt}; + + Image cbcr_valid{std::make_shared(cbcr_data.data(), cbcr_data.size()), + Image::PixelFormat::MONO16, + static_cast(width/2), + static_cast(height/2), + {}, {}, + DataSource::AUX_CHROMA_RAW, + aux_calibration, + std::nullopt}; + + Image cbcr_invalid{std::make_shared(cbcr_data.data(), cbcr_data.size()), + Image::PixelFormat::MONO8, //INVALID + static_cast(width/2), + static_cast(height/2), + {}, + {}, + DataSource::AUX_CHROMA_RAW, + aux_calibration, + std::nullopt}; EXPECT_FALSE(create_bgr_from_ycbcr420(y_invalid, cbcr_valid, DataSource::AUX_RAW)); EXPECT_FALSE(create_bgr_from_ycbcr420(y_valid, cbcr_invalid, DataSource::AUX_RAW)); @@ -987,13 +1000,24 @@ TEST(create_bgr_image, valid_images) std::vector y_data(width * height, 42); std::vector cbcr_data(width * height/2, 128); - Image y_valid{std::make_shared>(y_data), - 0, width * height, Image::PixelFormat::MONO8, - static_cast(width), static_cast(height), {}, {}, DataSource::AUX_LUMA_RECTIFIED_RAW, aux_calibration}; + Image y_valid{std::make_shared(y_data.data(), y_data.size()), + Image::PixelFormat::MONO8, + static_cast(width), + static_cast(height), + {}, + {}, + DataSource::AUX_LUMA_RECTIFIED_RAW, + aux_calibration, + std::nullopt}; - Image cbcr_valid{std::make_shared>(cbcr_data), - 0, width / 2 * height / 2, Image::PixelFormat::MONO16, - static_cast(width/2), static_cast(height/2), {}, {}, DataSource::AUX_CHROMA_RECTIFIED_RAW, aux_calibration}; + Image cbcr_valid{std::make_shared(cbcr_data.data(), cbcr_data.size()), + Image::PixelFormat::MONO16, + static_cast(width/2), + static_cast(height/2), + {}, + {}, + DataSource::AUX_CHROMA_RECTIFIED_RAW, + aux_calibration}; ImageFrame frame{0, {}, {}, StereoCalibration{aux_calibration, aux_calibration, aux_calibration}, {}, {}, {}, {}, {}, {}}; frame.aux_color_encoding = ColorImageEncoding::YCBCR420;