From e83281d7426512fe4c767fa9341d4ae0dc69413f Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Fri, 2 May 2025 17:32:04 -0400 Subject: [PATCH 1/2] Add support for stereo histograms --- python/bindings.cc | 26 +++++++++++++++- .../LibMultiSense/details/legacy/channel.cc | 6 +++- .../LibMultiSense/details/legacy/utilities.cc | 9 ++++++ .../include/MultiSense/MultiSenseTypes.hh | 30 +++++++++++++++++-- .../include/details/legacy/channel.hh | 9 +++--- .../include/details/legacy/utilities.hh | 6 ++++ 6 files changed, 78 insertions(+), 8 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index 10b75bd1..782984c2 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -214,6 +214,29 @@ PYBIND11_MODULE(_libmultisense, m) { .def_readonly("source", &multisense::Image::source) .def_readonly("calibration", &multisense::Image::calibration); + // ImageHistogram + py::class_(m, "ImageHistogram") + .def(py::init<>()) + .def_readonly("channels", &multisense::ImageHistogram::channels) + .def_readonly("bins", &multisense::ImageHistogram::bins) + .def_property_readonly("as_array", [](const multisense::ImageHistogram& histogram) + { + std::vector shape{histogram.data.size()}; + std::vector strides{sizeof(uint32_t)}; + size_t element_size = sizeof(uint32_t); + std::string format = py::format_descriptor::format(); + + // Map the cv::Mat to a NumPy array without copying the data + return py::array(py::buffer_info( + const_cast(reinterpret_cast(histogram.data.data())), + element_size, + format, + shape.size(), + shape, + strides)); + }); + + // ImageFrame py::class_(m, "ImageFrame") .def(py::init<>()) @@ -225,7 +248,8 @@ PYBIND11_MODULE(_libmultisense, m) { .def_readonly("calibration", &multisense::ImageFrame::calibration) .def_readonly("frame_time", &multisense::ImageFrame::frame_time) .def_readonly("ptp_frame_time", &multisense::ImageFrame::ptp_frame_time) - .def_readonly("aux_color_encoding", &multisense::ImageFrame::aux_color_encoding); + .def_readonly("aux_color_encoding", &multisense::ImageFrame::aux_color_encoding) + .def_readonly("stereo_histogram", &multisense::ImageFrame::stereo_histogram); // ImuRate py::class_(m, "ImuRate") diff --git a/source/LibMultiSense/details/legacy/channel.cc b/source/LibMultiSense/details/legacy/channel.cc index ab6518d2..30f2fbe1 100644 --- a/source/LibMultiSense/details/legacy/channel.cc +++ b/source/LibMultiSense/details/legacy/channel.cc @@ -1106,6 +1106,7 @@ void LegacyChannel::image_callback(std::shared_ptr> d scale_calibration(select_calibration(cal, source.front()), cal_x_scale, cal_y_scale)}; handle_and_dispatch(std::move(image), + get_histogram(meta->second), wire_image.frameId, scale_calibration(cal, cal_x_scale, cal_y_scale), capture_time_point, @@ -1174,6 +1175,7 @@ void LegacyChannel::disparity_callback(std::shared_ptrsecond), wire_image.frameId, scale_calibration(cal, cal_x_scale, cal_y_scale), capture_time_point, @@ -1232,6 +1234,7 @@ void LegacyChannel::imu_callback(std::shared_ptr> dat } void LegacyChannel::handle_and_dispatch(Image image, + ImageHistogram histogram, int64_t frame_id, const StereoCalibration &calibration, const TimeT &capture_time, @@ -1248,7 +1251,8 @@ void LegacyChannel::handle_and_dispatch(Image image, calibration, capture_time, ptp_capture_time, - m_calibration.aux ? ColorImageEncoding::YCBCR420 : ColorImageEncoding::NONE}; + m_calibration.aux ? ColorImageEncoding::YCBCR420 : ColorImageEncoding::NONE, + std::move(histogram)}; m_frame_buffer.emplace(frame_id, std::move(frame)); } diff --git a/source/LibMultiSense/details/legacy/utilities.cc b/source/LibMultiSense/details/legacy/utilities.cc index b1fde0ce..a0f99801 100644 --- a/source/LibMultiSense/details/legacy/utilities.cc +++ b/source/LibMultiSense/details/legacy/utilities.cc @@ -343,6 +343,15 @@ ImuSampleScalars get_imu_scalars(const crl::multisense::details::wire::ImuInfo & return output; } +ImageHistogram get_histogram(const crl::multisense::details::wire::ImageMeta &metadata) +{ + using namespace crl::multisense::details; + + std::vector data(wire::ImageMeta::HISTOGRAM_CHANNELS * wire::ImageMeta::HISTOGRAM_BINS, 0); + memcpy(data.data(), metadata.histogramP, wire::ImageMeta::HISTOGRAM_LENGTH); + + return ImageHistogram{wire::ImageMeta::HISTOGRAM_CHANNELS, wire::ImageMeta::HISTOGRAM_BINS, std::move(data)}; } } +} diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index aa30c36a..2b5c42c8 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -295,6 +295,27 @@ struct Image #endif }; +/// +/// @brief A object containing histogram data for a image +/// +struct ImageHistogram +{ + /// + /// @brief The number of channels per pixel + /// + uint32_t channels = 0; + + /// + /// @brief The number of possible pixel bins + /// + uint32_t bins = 0; + + /// + /// @brief The raw histogram counts whose size is equal to channels*bins + /// + std::vector data{}; +}; + /// /// @brief A frame containing multiple images (indexed by DataSource). /// @@ -337,12 +358,12 @@ struct ImageFrame /// /// @brief The images associated with each source in the frame /// - std::map images; + std::map images{}; /// /// @brief The scaled calibration for the entire camera /// - StereoCalibration calibration; + StereoCalibration calibration{}; /// /// @brief The MultiSense timestamp associated with the frame @@ -358,6 +379,11 @@ struct ImageFrame /// @brief The encoding of the aux color image(s) in the frame /// ColorImageEncoding aux_color_encoding = ColorImageEncoding::NONE; + + /// + /// @brief Corresponding histogram from the main stereo pair + /// + std::optional stereo_histogram = std::nullopt; }; /// diff --git a/source/LibMultiSense/include/details/legacy/channel.hh b/source/LibMultiSense/include/details/legacy/channel.hh index 60f56712..2982102f 100644 --- a/source/LibMultiSense/include/details/legacy/channel.hh +++ b/source/LibMultiSense/include/details/legacy/channel.hh @@ -291,10 +291,11 @@ private: /// @brief Handle internal process, and potentially dispatch a image /// void handle_and_dispatch(Image image, - int64_t frame_id, - const StereoCalibration &calibration, - const TimeT &capture_time, - const TimeT &ptp_capture_time); + ImageHistogram histogram, + int64_t frame_id, + const StereoCalibration &calibration, + const TimeT &capture_time, + const TimeT &ptp_capture_time); /// /// @brief Internal mutex used to handle updates from users diff --git a/source/LibMultiSense/include/details/legacy/utilities.hh b/source/LibMultiSense/include/details/legacy/utilities.hh index 694c6349..54b2d997 100644 --- a/source/LibMultiSense/include/details/legacy/utilities.hh +++ b/source/LibMultiSense/include/details/legacy/utilities.hh @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -168,6 +169,11 @@ double get_magnetometer_scale(const std::string &units); /// ImuSampleScalars get_imu_scalars(const crl::multisense::details::wire::ImuInfo &info); +/// +/// @brief Extract histogram data from the image metadata +/// +ImageHistogram get_histogram(const crl::multisense::details::wire::ImageMeta &metadata); + /// /// @brief Helper to wait for ack from the camera from a given query command. Once a query /// command is sent to the MultiSense, it Ack's the command before sending the response From 41823e8315a9133ef9bf71c6d0c8b871a9da1c52 Mon Sep 17 00:00:00 2001 From: Matt Alvarado Date: Sun, 4 May 2025 13:49:38 -0400 Subject: [PATCH 2/2] Add capture and exposure times to the image frame --- python/bindings.cc | 4 +++- source/LibMultiSense/details/legacy/channel.cc | 12 ++++++++---- .../include/MultiSense/MultiSenseTypes.hh | 10 ++++++++++ .../LibMultiSense/include/details/legacy/channel.hh | 2 +- 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/python/bindings.cc b/python/bindings.cc index 782984c2..3a16f4cc 100644 --- a/python/bindings.cc +++ b/python/bindings.cc @@ -249,7 +249,9 @@ PYBIND11_MODULE(_libmultisense, m) { .def_readonly("frame_time", &multisense::ImageFrame::frame_time) .def_readonly("ptp_frame_time", &multisense::ImageFrame::ptp_frame_time) .def_readonly("aux_color_encoding", &multisense::ImageFrame::aux_color_encoding) - .def_readonly("stereo_histogram", &multisense::ImageFrame::stereo_histogram); + .def_readonly("stereo_histogram", &multisense::ImageFrame::stereo_histogram) + .def_readonly("capture_exposure_time", &multisense::ImageFrame::capture_exposure_time) + .def_readonly("capture_gain", &multisense::ImageFrame::capture_gain); // ImuRate py::class_(m, "ImuRate") diff --git a/source/LibMultiSense/details/legacy/channel.cc b/source/LibMultiSense/details/legacy/channel.cc index 30f2fbe1..a6ed0b05 100644 --- a/source/LibMultiSense/details/legacy/channel.cc +++ b/source/LibMultiSense/details/legacy/channel.cc @@ -1106,7 +1106,7 @@ void LegacyChannel::image_callback(std::shared_ptr> d scale_calibration(select_calibration(cal, source.front()), cal_x_scale, cal_y_scale)}; handle_and_dispatch(std::move(image), - get_histogram(meta->second), + meta->second, wire_image.frameId, scale_calibration(cal, cal_x_scale, cal_y_scale), capture_time_point, @@ -1175,7 +1175,7 @@ void LegacyChannel::disparity_callback(std::shared_ptrsecond), + meta->second, wire_image.frameId, scale_calibration(cal, cal_x_scale, cal_y_scale), capture_time_point, @@ -1234,12 +1234,14 @@ void LegacyChannel::imu_callback(std::shared_ptr> dat } void LegacyChannel::handle_and_dispatch(Image image, - ImageHistogram histogram, + const crl::multisense::details::wire::ImageMeta &metadata, int64_t frame_id, const StereoCalibration &calibration, const TimeT &capture_time, const TimeT &ptp_capture_time) { + using namespace std::chrono; + // // Create a new frame if one does not exist, or add the input image to the corresponding frame // @@ -1252,7 +1254,9 @@ void LegacyChannel::handle_and_dispatch(Image image, capture_time, ptp_capture_time, m_calibration.aux ? ColorImageEncoding::YCBCR420 : ColorImageEncoding::NONE, - std::move(histogram)}; + get_histogram(metadata), + microseconds{metadata.exposureTime}, + metadata.gain}; m_frame_buffer.emplace(frame_id, std::move(frame)); } diff --git a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh index 2b5c42c8..0bb8fd0f 100644 --- a/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh +++ b/source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh @@ -384,6 +384,16 @@ struct ImageFrame /// @brief Corresponding histogram from the main stereo pair /// std::optional stereo_histogram = std::nullopt; + + /// + /// @brief The exposure time which was used to capture this frame + /// + std::chrono::microseconds capture_exposure_time{0}; + + /// + /// @brief The gain that was used to capture the stereo images in this frame + /// + float capture_gain = 0.0; }; /// diff --git a/source/LibMultiSense/include/details/legacy/channel.hh b/source/LibMultiSense/include/details/legacy/channel.hh index 2982102f..abe7a7b9 100644 --- a/source/LibMultiSense/include/details/legacy/channel.hh +++ b/source/LibMultiSense/include/details/legacy/channel.hh @@ -291,7 +291,7 @@ private: /// @brief Handle internal process, and potentially dispatch a image /// void handle_and_dispatch(Image image, - ImageHistogram histogram, + const crl::multisense::details::wire::ImageMeta &metadata, int64_t frame_id, const StereoCalibration &calibration, const TimeT &capture_time,