Added support for confidence map streaming.

marek-simonik · May 27, 2024 · 0c6e769 · 0c6e769
1 parent d8637f5
commit 0c6e769
Show file tree

Hide file tree

Showing 9 changed files with 183 additions and 29 deletions.
diff --git a/README.md b/README.md
@@ -1,5 +1,7 @@
 # [*Record3D – Point Cloud Animation and Streaming*](https://record3d.app/): the accompanying library 
 
+**2024/05/27 Update**: Added confidence map streaming (introduced breaking changes). **To be used with Record3D 1.10 and newer.**
+
 **2022/08/16 Update**: Added camera position streaming (introduced breaking changes). **To be used with Record3D 1.7.2 and newer.**
 
 **2021/07/28 Update**: Introduced support for higher-quality RGB LiDAR streaming. **To be used with Record3D 1.6 and newer.**

diff --git a/demo-main.py b/demo-main.py
@@ -49,6 +49,7 @@ def start_processing_stream(self):
             # Copy the newly arrived RGBD frame
             depth = self.session.get_depth_frame()
             rgb = self.session.get_rgb_frame()
+            confidence = self.session.get_confidence_frame()
             intrinsic_mat = self.get_intrinsic_mat_from_coeffs(self.session.get_intrinsic_mat())
             camera_pose = self.session.get_camera_pose()  # Quaternion + world position (accessible via camera_pose.[qx|qy|qz|qw|tx|ty|tz])
 
@@ -66,6 +67,10 @@ def start_processing_stream(self):
             # Show the RGBD Stream
             cv2.imshow('RGB', rgb)
             cv2.imshow('Depth', depth)
+
+            if confidence.shape[0] > 0 and confidence.shape[1] > 0:
+                cv2.imshow('Confidence', confidence * 100)
+
             cv2.waitKey(1)
 
             self.event.clear()

diff --git a/include/record3d/Record3DStream.h b/include/record3d/Record3DStream.h
@@ -9,6 +9,7 @@
 #include "Record3DStructs.h"
 #include <stdint.h>
 #include <functional>
+#include <string.h>
 
 #ifdef PYTHON_BINDINGS_BUILD
 #include <pybind11/pybind11.h>
@@ -21,6 +22,8 @@ namespace Record3D
 {
     using BufferRGB = std::vector<uint8_t>;
     using BufferDepth = std::vector<uint8_t>;
+    using BufferConfidence = std::vector<uint8_t>;
+    using BufferMisc = std::vector<uint8_t>;
 
     class Record3DStream
     {
@@ -74,12 +77,12 @@ namespace Record3D
         /**
          * Decompresses incoming compressed depth frame into $destinationBuffer of known size.
          *
-         * @param $compressedDepthBuffer the buffer containing LZFSE-compressed depth frame.
-         * @param $compressedDepthBufferSize size of the compressed buffer.
+         * @param $compressedBuffer the buffer containing LZFSE-compressed depth frame.
+         * @param $compressedBufferSize size of the compressed buffer.
          * @param $destinationBuffer buffer into which the decompressed depth frame is going to be written.
          * @returns pointer to the decompressed buffer. In case of decompression failure, `nullptr` is returned.
          */
-        uint8_t* DecompressDepthBuffer(const uint8_t* $compressedDepthBuffer, size_t $compressedDepthBufferSize, std::vector<uint8_t> &$destinationBuffer);
+        uint8_t* DecompressBuffer(const uint8_t* $compressedBuffer, size_t $compressedBufferSize, std::vector<uint8_t> &$destinationBuffer);
 
         /**
          * Wraps the standard `recv()` function to ensure the *exact* amount of bytes (`$numBytesToRead`) is read into the $outputBuffer.
@@ -116,17 +119,29 @@ namespace Record3D
          *
          * @param $rgbFrame RGB buffer.
          * @param $depthFrame Depth buffer.
-         * @param $frameWidth width od the RGB and Depth frames.
-         * @param $frameHeight height od the RGB and Depth frames.
-         * @param $K four coefficients of the intrinsic matrix corresponding to the Depth frame.
+         * @param $confFrame Confidence buffer.
+         * @param $miscData Misc buffer (reserved).
+         * @param $rgbWidth width of the RGB frame.
+         * @param $rgbHeight height of the RGB frame.
+         * @param $depthWidth width of the Depth frame.
+         * @param $depthHeight height of the Depth frame.
+         * @param $confWidth width of Confidence frame (0 if there is no confidence frame).
+         * @param $confHeight height of Confidence frame (0 if there is no confidence frame).
+         * @param $deviceType type of the camera used for streaming.
+         * @param $K four coefficients of the intrinsic matrix corresponding to the RGB frame.
+         * @param $cameraPose camera pose in world space.
          */
         std::function<void(const Record3D::BufferRGB &$rgbFrame,
                            const Record3D::BufferDepth &$depthFrame,
+                           const Record3D::BufferConfidence &$confFrame,
+                           const Record3D::BufferMisc &$miscData,
                            uint32_t   $rgbWidth,
                            uint32_t   $rgbHeight,
                            uint32_t   $depthWidth,
                            uint32_t   $depthHeight,
-                           DeviceType $deviceType,
+                           uint32_t   $confWidth,
+                           uint32_t   $confHeight,
+                           Record3D::DeviceType $deviceType,
                            Record3D::IntrinsicMatrixCoeffs $K,
                            Record3D::CameraPose $cameraPose)> onNewFrame{};
 #endif
@@ -157,6 +172,44 @@ namespace Record3D
             return result;
         }
 
+        /**
+         * NOTE: This is alternative API for Python.
+         *
+         * @returns the current contents of the Confidence buffer which holds the lastly received Depth frame.
+         */
+        py::array_t<uint8_t> GetCurrentConfidenceFrame()
+        {
+            size_t currentFrameWidth = currentFrameConfidenceWidth_;
+            size_t currentFrameHeight = currentFrameConfidenceHeight_;
+
+            size_t bufferSize  = currentFrameWidth * currentFrameHeight * sizeof(uint8_t);
+            auto result        = py::array_t<uint8_t>(currentFrameWidth * currentFrameHeight);
+            auto result_buffer = result.request();
+            uint8_t *result_ptr  = (uint8_t *) result_buffer.ptr;
+
+            std::memcpy(result_ptr, confidenceImageBuffer_.data(), bufferSize);
+            result.resize(std::vector<int>{static_cast<int>(currentFrameHeight), static_cast<int>(currentFrameWidth)});
+
+            return result;
+        }
+
+        /**
+         * NOTE: This is alternative API for Python.
+         *
+         * @returns the current contents of the Misc buffer (reserved).
+         */
+        py::array_t<uint8_t> GetCurrentMiscData()
+        {
+            size_t bufferSize  = miscBuffer_.size();
+            auto result        = py::array_t<uint8_t>( bufferSize );
+            auto result_buffer = result.request();
+            uint8_t *result_ptr  = (uint8_t *) result_buffer.ptr;
+
+            std::memcpy(result_ptr, miscBuffer_.data(), bufferSize);
+
+            return result;
+        }
+
         /**
          * NOTE: This is alternative API for Python.
          *
@@ -212,8 +265,13 @@ namespace Record3D
     private:
         size_t currentFrameRGBWidth_{ 0 };
         size_t currentFrameRGBHeight_{ 0 };
+
         size_t currentFrameDepthWidth_{ 0 };
         size_t currentFrameDepthHeight_{ 0 };
+
+        size_t currentFrameConfidenceWidth_{ 0 };
+        size_t currentFrameConfidenceHeight_{ 0 };
+
         DeviceType currentDeviceType_ {};
 
         uint8_t* lzfseScratchBuffer_{ nullptr }; /** Preallocated LZFSE scratch buffer. */
@@ -226,7 +284,9 @@ namespace Record3D
 
         std::vector<uint8_t> depthImageBuffer_{}; /** Holds the most recent Depth buffer. */
         std::vector<uint8_t> RGBImageBuffer_{}; /** Holds the most recent RGB buffer. */
-        IntrinsicMatrixCoeffs rgbIntrinsicMatrixCoeffs_{}; /** Holds the intrinsic matrix of the most recent Depth frame. */
+        std::vector<uint8_t> confidenceImageBuffer_{}; /** Holds the Confidence map of the most recent Depth buffer. Pixel values: Low: 0, Medium: 1, High: 2 */
+        std::vector<uint8_t> miscBuffer_{}; /** Reserved */
+        IntrinsicMatrixCoeffs rgbIntrinsicMatrixCoeffs_{}; /** Holds the intrinsic matrix of the most recent RGB frame. */
         CameraPose cameraPose_{}; /** Holds the world position of the most recent camera frame. */
     };
 }

diff --git a/include/record3d/Record3DStructs.h b/include/record3d/Record3DStructs.h
@@ -1,6 +1,8 @@
 #ifndef CPP_RECORD3DSTRUCTS_H
 #define CPP_RECORD3DSTRUCTS_H
 
+#include <cstring>
+
 namespace Record3D
 {
     struct IntrinsicMatrixCoeffs

diff --git a/pyproject.toml b/pyproject.toml
@@ -0,0 +1,6 @@
+[build-system]
+requires = [
+    "setuptools",
+    "numpy"
+]
+build-backend = "setuptools.build_meta"
diff --git a/python-bindings/src/PythonBindings.cpp b/python-bindings/src/PythonBindings.cpp
@@ -43,7 +43,9 @@ PYBIND11_MODULE( record3d, m )
             .def("connect", &Record3D::Record3DStream::ConnectToDevice, "Connect to iOS device and start receiving RGBD frames.")
             .def("get_depth_frame", &Record3D::Record3DStream::GetCurrentDepthFrame, "Returns the current Depth frame.")
             .def("get_rgb_frame", &Record3D::Record3DStream::GetCurrentRGBFrame, "Return the current RGB frame.")
-            .def("get_intrinsic_mat", &Record3D::Record3DStream::GetCurrentIntrinsicMatrix, "Returns the intrinsic matrix of current Depth frame.")
+            .def("get_confidence_frame", &Record3D::Record3DStream::GetCurrentConfidenceFrame, "Return the current Confidence frame (corresponding to the current Depth frame).")
+            .def("get_misc_data", &Record3D::Record3DStream::GetCurrentMiscData, "Return the current Misc data buffer (reserved).")
+            .def("get_intrinsic_mat", &Record3D::Record3DStream::GetCurrentIntrinsicMatrix, "Returns the intrinsic matrix of current RGB frame.")
             .def("get_camera_pose", &Record3D::Record3DStream::GetCurrentCameraPose, "Returns the camera pose of the current camera frame.")
             .def("get_device_type", &Record3D::Record3DStream::GetCurrentDeviceType, "Returns the type of camera (TrueDeph = 0, LiDAR = 1).")
             .def_readwrite("on_new_frame", &Record3D::Record3DStream::onNewFrame, "Method called upon receiving new frame.")

diff --git a/setup.py b/setup.py
@@ -67,7 +67,7 @@ def build_extension(self, ext):
 
 setup(
     name='record3d',
-    version='1.3.1',
+    version='1.4',
     license='lgpl-2.1',
     author='Marek Simonik',
     author_email='[email protected]',

diff --git a/src/DemoMain.cpp b/src/DemoMain.cpp
@@ -2,6 +2,7 @@
 #include <vector>
 #include <record3d/Record3DStream.h>
 #include <mutex>
+#include <cstring>
 
 #ifdef HAS_OPENCV
 #include <opencv2/opencv.hpp>
@@ -28,15 +29,19 @@ class Record3DDemoApp
         };
         stream.onNewFrame = [&]( const Record3D::BufferRGB &$rgbFrame,
                                  const Record3D::BufferDepth &$depthFrame,
-                                 uint32_t $rgbWidth,
-                                 uint32_t $rgbHeight,
-                                 uint32_t $depthWidth,
-                                 uint32_t $depthHeight,
+                                 const Record3D::BufferConfidence &$confFrame,
+                                 const Record3D::BufferMisc &$miscData,
+                                 uint32_t   $rgbWidth,
+                                 uint32_t   $rgbHeight,
+                                 uint32_t   $depthWidth,
+                                 uint32_t   $depthHeight,
+                                 uint32_t   $confWidth,
+                                 uint32_t   $confHeight,
                                  Record3D::DeviceType $deviceType,
                                  Record3D::IntrinsicMatrixCoeffs $K,
                                  Record3D::CameraPose $cameraPose )
         {
-            OnNewFrame( $rgbFrame, $depthFrame, $rgbWidth, $rgbHeight, $depthWidth, $depthHeight, $deviceType, $K, $cameraPose );
+            OnNewFrame( $rgbFrame, $depthFrame, $confFrame, $miscData, $rgbWidth, $rgbHeight, $depthWidth, $depthHeight, $confWidth, $confHeight, $deviceType, $K, $cameraPose );
         };
 
         // Try connecting to a device.
@@ -67,7 +72,7 @@ class Record3DDemoApp
             {
                 // Wait for the callback thread to receive new frame and unlock this thread
 #ifdef HAS_OPENCV
-                cv::Mat rgb, depth;
+                cv::Mat rgb, depth, confidence;
                 {
                     std::lock_guard<std::recursive_mutex> lock( mainThreadLock_ );
 
@@ -76,6 +81,11 @@ class Record3DDemoApp
                         continue;
                     }
 
+                    if ( imgConfidence_.cols > 0 && imgConfidence_.rows > 0 )
+                    {
+                        confidence = imgConfidence_.clone();
+                    }
+
                     rgb = imgRGB_.clone();
                     depth = imgDepth_.clone();
                 }
@@ -89,6 +99,11 @@ class Record3DDemoApp
                     cv::flip( depth, depth, 1 );
                 }
 
+                if ( imgConfidence_.cols > 0 && imgConfidence_.rows > 0 )
+                {
+                    cv::imshow( "Confidence", confidence * 100 );
+                }
+
                 // Show images
                 cv::imshow( "RGB", rgb );
                 cv::imshow( "Depth", depth );
@@ -111,10 +126,14 @@ class Record3DDemoApp
 
     void OnNewFrame( const Record3D::BufferRGB &$rgbFrame,
                      const Record3D::BufferDepth &$depthFrame,
-                     uint32_t $rgbWidth,
-                     uint32_t $rgbHeight,
-                     uint32_t $depthWidth,
-                     uint32_t $depthHeight,
+                     const Record3D::BufferConfidence &$confFrame,
+                     const Record3D::BufferMisc &$miscData,
+                     uint32_t   $rgbWidth,
+                     uint32_t   $rgbHeight,
+                     uint32_t   $depthWidth,
+                     uint32_t   $depthHeight,
+                     uint32_t   $confWidth,
+                     uint32_t   $confHeight,
                      Record3D::DeviceType $deviceType,
                      Record3D::IntrinsicMatrixCoeffs $K,
                      Record3D::CameraPose $cameraPose )
@@ -135,10 +154,27 @@ class Record3DDemoApp
             imgDepth_ = cv::Mat::zeros( $depthHeight, $depthWidth, CV_32F );
         }
 
+        bool isConfidenceMapIncluded = $confWidth > 0 && $confHeight > 0;
+
+        if ( imgConfidence_.rows != $confWidth || imgConfidence_.cols != $confHeight )
+        {
+            imgConfidence_.release();
+
+            if ( isConfidenceMapIncluded )
+            {
+                imgConfidence_ = cv::Mat::zeros( $confHeight, $confWidth, CV_8U );
+            }
+        }
+
         // The `BufferRGB` and `BufferDepth` may be larger than the actual payload, therefore the true frame size is computed.
         constexpr int numRGBChannels = 3;
         memcpy( imgRGB_.data, $rgbFrame.data(), $rgbWidth * $rgbHeight * numRGBChannels * sizeof( uint8_t ) );
         memcpy( imgDepth_.data, $depthFrame.data(), $depthWidth * $depthHeight * sizeof( float ) );
+
+        if ( isConfidenceMapIncluded )
+        {
+            memcpy( imgConfidence_.data, $confFrame.data(), $confWidth * $confHeight * sizeof(uint8_t) );
+        }
 #endif
     }
 
@@ -149,6 +185,7 @@ class Record3DDemoApp
 #ifdef HAS_OPENCV
     cv::Mat imgRGB_ { };
     cv::Mat imgDepth_ { };
+    cv::Mat imgConfidence_ { };
 #endif
 };