diff --git a/README.md b/README.md index e9244952..792c3e1b 100644 --- a/README.md +++ b/README.md @@ -251,7 +251,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/python/bindings.cc b/python/bindings.cc index c664fc74..06f7234f 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()), @@ -283,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") @@ -830,7 +836,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/CMakeLists.txt b/source/LibMultiSense/CMakeLists.txt index b1245c9b..f895107f 100644 --- a/source/LibMultiSense/CMakeLists.txt +++ b/source/LibMultiSense/CMakeLists.txt @@ -9,6 +9,11 @@ endif() 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/amb/webrtc.cc details/legacy/calibration.cc details/legacy/channel.cc details/legacy/configuration.cc @@ -69,11 +74,15 @@ target_include_directories(MultiSense PUBLIC $ + +#include "details/amb/channel.hh" +#include "details/amb/info.hh" +#include "details/amb/http.hh" +#include "details/amb/utilities.hh" + +#include +#include + +#include "multisense-webrtc-client.h" + +#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::OK; +} + +Status AmbChannel::stop_streams(const std::vector &sources) +{ + (void) sources; + return Status::OK; +} + +void AmbChannel::add_image_frame_callback(std::function callback) +{ + m_user_image_frame_callback = 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; + } + + if (config.receive_timeout) + { + m_http_client->set_read_timeout(config.receive_timeout.value()); + } + + // + // 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; + } + + // + // 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; + } + + // + // 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_calibration); + + 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() +{ + if (!m_connected) + { + return std::nullopt; + } + + return m_image_frame_notifier.wait(m_config.receive_timeout); +} + +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::OK; +} + +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 config_json = http_get(m_http_client, "/conf/conf.json"); + + if (config_json) + { + 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); + + 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; +} + +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; +} + +bool is_amb_camera(const std::string &ip_address) +{ + auto client = std::make_unique("https://" + ip_address); + + if (!client) + { + 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")); +} + +} +} diff --git a/source/LibMultiSense/details/amb/http.cc b/source/LibMultiSense/details/amb/http.cc new file mode 100644 index 00000000..79af4222 --- /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/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/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/amb/webrtc.cc b/source/LibMultiSense/details/amb/webrtc.cc new file mode 100644 index 00000000..4f2b92df --- /dev/null +++ b/source/LibMultiSense/details/amb/webrtc.cc @@ -0,0 +1,211 @@ +/** + * @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/utilities.hh" +#include "details/amb/webrtc.hh" + +#include +#include + +extern "C" +{ +#include "multisense-webrtc-client.h" +} + +namespace multisense { +namespace amb { + +WebRtcClient::WebRtcClient(const std::string& host, const StereoCalibration &calibration) + : m_impl(nullptr), + m_calibration(calibration) +{ + 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(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; + } + + WebRtcClient* client = static_cast(user_data); + if (!client->m_frame_callback) + { + return; + } + + ImageFrame frame; + + 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 = [](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; + } + + // + // 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::PixelFormat format = Image::PixelFormat::UNKNOWN; + if (data->channels == 1 && data->bytewidth == 1) + { + format = Image::PixelFormat::MONO8; + } + else if (data->channels == 1 && data->bytewidth == 2) + { + format = Image::PixelFormat::MONO16; + } + else if (data->channels == 3 && data->bytewidth == 1) + { + format = Image::PixelFormat::BGR8; + } + else + { + format = Image::PixelFormat::UNKNOWN; + } + + return Image{std::move(buffer), + format, + data->width, + data->height, + image_timestamp, + image_timestamp, + source, + calibration, + 1.0/static_cast(1 << data->precision)}; + }; + + if (auto left_image = create_image(left, + client->m_calibration.left, + 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, + 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, + image_time); disparity_image) + { + frame.add_image(disparity_image.value()); + } + + client->m_frame_callback(frame); +} + +} // namespace amb +} // namespace multisense diff --git a/source/LibMultiSense/details/factory.cc b/source/LibMultiSense/details/factory.cc index 252e8f5b..b854e56b 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,33 @@ std::unique_ptr Channel::create(const Config &config, { switch (impl) { + case ChannelImplementation::AUTO: + { + if (amb::is_amb_camera(config.ip_address)) + { + 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; + } + } + else + { + 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: { try @@ -56,6 +84,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/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/legacy/channel.cc b/source/LibMultiSense/details/legacy/channel.cc index 3831319a..3b7b6a3b 100644 --- a/source/LibMultiSense/details/legacy/channel.cc +++ b/source/LibMultiSense/details/legacy/channel.cc @@ -1464,16 +1464,16 @@ 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, 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, @@ -1533,16 +1533,17 @@ 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, 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, @@ -1583,11 +1584,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,16 +1598,16 @@ 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, capture_time_point, ptp_capture_time_point, source, - scale_calibration(select_calibration(cal, source), cal_x_scale, cal_y_scale)}; + scale_calibration(select_calibration(cal, source), cal_x_scale, cal_y_scale), + 1.0/16.0}; handle_and_dispatch(std::move(image), meta->second, diff --git a/source/LibMultiSense/details/utilities.cc b/source/LibMultiSense/details/utilities.cc index 5c07cfa5..874ff304 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 @@ -323,21 +322,17 @@ 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 = 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; 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)); @@ -377,16 +372,15 @@ 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, 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, @@ -421,15 +415,14 @@ 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}; + 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 // 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,10 +432,10 @@ 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; + static_cast(*reinterpret_cast(disparity.raw_data->data() + index)) * pixel_scale; if (d == 0) { @@ -466,7 +459,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 +472,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 +488,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, @@ -563,4 +554,133 @@ 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; +} + +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/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/MultiSense/MultiSenseSerialization.hh b/source/LibMultiSense/include/MultiSense/MultiSenseSerialization.hh index 257a9458..4610d058 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseSerialization.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseSerialization.hh @@ -210,6 +210,7 @@ NLOHMANN_JSON_SERIALIZE_ENUM(MultiSenseInfo::DeviceInfo::HardwareRevision, { {MultiSenseInfo::DeviceInfo::HardwareRevision::KS21_SILVER, "KS21_SILVER"}, {MultiSenseInfo::DeviceInfo::HardwareRevision::ST25, "ST25"}, {MultiSenseInfo::DeviceInfo::HardwareRevision::KS21i, "KS21i"}, + {MultiSenseInfo::DeviceInfo::HardwareRevision::AMB, "AMB"}, {MultiSenseInfo::DeviceInfo::HardwareRevision::STLC, "STLC"} }) diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index c549a88c..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])}; } }; @@ -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 @@ -265,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 @@ -289,7 +356,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; @@ -1540,7 +1607,8 @@ struct MultiSenseInfo KS21_SILVER, ST25, KS21i, - STLC + STLC, + AMB }; /// diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh b/source/LibMultiSense/include/MultiSense/MultiSenseUtilities.hh index e6a593a4..4f4b2d3a 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 @@ -104,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_(static_cast(matching_cal.P[0][3]) / static_cast(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_), @@ -264,6 +266,9 @@ std::optional> create_color_pointcloud(const Image &disparity, 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) @@ -281,10 +286,10 @@ std::optional> create_color_pointcloud(const Image &disparity, 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; } @@ -292,29 +297,29 @@ std::optional> create_color_pointcloud(const Image &disparity, 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; - } - constexpr double scale = 1.0 / 16.0; + scale_x = static_cast(color->width) / static_cast(disparity.width); + scale_y = static_cast(color->height) / static_cast(disparity.height); + } 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); + 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) { - 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 = - 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) + if (d == 0.0 || d >= 255) { continue; } @@ -332,14 +337,17 @@ std::optional> create_color_pointcloud(const Image &disparity, } 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_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}); @@ -468,4 +476,111 @@ bool write_pointcloud_ply(const PointCloud &point_cloud, const std::files 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/include/details/amb/channel.hh b/source/LibMultiSense/include/details/amb/channel.hh new file mode 100644 index 00000000..15a56e9c --- /dev/null +++ b/source/LibMultiSense/include/details/amb/channel.hh @@ -0,0 +1,229 @@ +/** + * @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 + +#include "details/utilities.hh" +#include "details/amb/webrtc.hh" + +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 Query the MultiSense Info + /// + std::optional query_info(); + + /// + /// @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 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; +}; + +/// +/// @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); + +} +} 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/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); + +} +} + 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); + +} +} diff --git a/source/LibMultiSense/include/details/amb/webrtc.hh b/source/LibMultiSense/include/details/amb/webrtc.hh new file mode 100644 index 00000000..12cafeec --- /dev/null +++ b/source/LibMultiSense/include/details/amb/webrtc.hh @@ -0,0 +1,115 @@ +/** + * @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" + +// Forward declarations in global namespace to match the C library types +struct ImageData; +struct MswebrtcImpl; + +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, const StereoCalibration &calibration); + ~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(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); + + /// + /// @brief Pointer to the base C WebRTC implementation + /// + struct MswebrtcImpl* 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 +} // namespace multisense diff --git a/source/LibMultiSense/include/details/legacy/calibration.hh b/source/LibMultiSense/include/details/legacy/calibration.hh index 2582daf1..ce57ef86 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/legacy/channel.hh b/source/LibMultiSense/include/details/legacy/channel.hh index 80c8371e..8911ac32 100644 --- a/source/LibMultiSense/include/details/legacy/channel.hh +++ b/source/LibMultiSense/include/details/legacy/channel.hh @@ -45,6 +45,7 @@ #include #include +#include "details/utilities.hh" #include "details/legacy/ip.hh" #include "details/legacy/message.hh" #include "details/legacy/storage.hh" @@ -54,72 +55,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..51447cca --- /dev/null +++ b/source/LibMultiSense/include/details/utilities.hh @@ -0,0 +1,122 @@ +/** + * @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 + +#include "MultiSense/MultiSenseTypes.hh" + +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; +}; + +/// +/// @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/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 e3feeff2..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) @@ -124,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; @@ -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); @@ -488,6 +485,56 @@ TEST(create_pointcloud, basic_tests) 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); +} + TEST(to_string, status) { EXPECT_EQ(to_string(Status::OK), "OK"); @@ -550,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)); @@ -580,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, @@ -611,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)); @@ -642,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)); @@ -673,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)); @@ -704,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")); } @@ -739,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); @@ -819,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); @@ -877,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)); @@ -937,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; @@ -952,4 +1026,3 @@ TEST(create_bgr_image, valid_images) EXPECT_TRUE(create_bgr_image(frame, DataSource::AUX_RECTIFIED_RAW)); } - 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 -