Added support for HQ LiDAR RGB streaming

marek-simonik · Jul 28, 2021 · 62f8778 · 62f8778
1 parent 53b4824
commit 62f8778
Show file tree

Hide file tree

Showing 8 changed files with 113 additions and 58 deletions.
diff --git a/README.md b/README.md
@@ -1,5 +1,6 @@
 # [*Record3D – Point Cloud Animation and Streaming*](https://record3d.app/): the accompanying library 
 
+**2021/07/28 Update**: Introduced support for higher-quality RGB LiDAR streaming. **To be used with Record3D 1.6 and newer.**
 **2020/09/17 Update**: Introduced LiDAR support. To be used with Record3D 1.4 and newer.
 
 This project provides C++ and Python libraries for the [iOS Record3D app](https://record3d.app/) which allows you (among other features) to 

diff --git a/demo-main.py b/demo-main.py
@@ -8,6 +8,8 @@ class DemoApp:
     def __init__(self):
         self.event = Event()
         self.session = None
+        self.DEVICE_TYPE__TRUEDEPTH = 0
+        self.DEVICE_TYPE__LIDAR = 1
 
     def on_new_frame(self):
         """
@@ -40,7 +42,6 @@ def get_intrinsic_mat_from_coeffs(self, coeffs):
                          [        0, coeffs.fy, coeffs.ty],
                          [        0,         0,         1]])
 
-
     def start_processing_stream(self):
         while True:
             self.event.wait()  # Wait for new frame to arrive
@@ -49,11 +50,11 @@ def start_processing_stream(self):
             depth = self.session.get_depth_frame()
             rgb = self.session.get_rgb_frame()
             intrinsic_mat = self.get_intrinsic_mat_from_coeffs(self.session.get_intrinsic_mat())
+            print(intrinsic_mat)
             # You can now e.g. create point cloud by projecting the depth map using the intrinsic matrix.
 
             # Postprocess it
-            are_truedepth_camera_data_being_streamed = depth.shape[0] == 640
-            if are_truedepth_camera_data_being_streamed:
+            if self.session.get_device_type() == self.DEVICE_TYPE__TRUEDEPTH:
                 depth = cv2.flip(depth, 1)
                 rgb = cv2.flip(rgb, 1)
 

diff --git a/include/record3d/Record3DStream.h b/include/record3d/Record3DStream.h
@@ -79,7 +79,7 @@ namespace Record3D
          * @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, uint8_t* $destinationBuffer);
+        uint8_t* DecompressDepthBuffer(const uint8_t* $compressedDepthBuffer, size_t $compressedDepthBufferSize, std::vector<uint8_t> &$destinationBuffer);
 
         /**
          * Wraps the standard `recv()` function to ensure the *exact* amount of bytes (`$numBytesToRead`) is read into the $outputBuffer.
@@ -94,8 +94,6 @@ namespace Record3D
     public:
         // Constants
         static constexpr uint16_t DEVICE_PORT{ 1337 }; /** Port on iDevice that we are listening to for RGBD stream. */
-        static constexpr uint32_t MAXIMUM_FRAME_WIDTH{ 480 }; /** Maximum width of the RGB and Depth components of the RGBD stream. */
-        static constexpr uint32_t MAXIMUM_FRAME_HEIGHT{ 640 }; /** Maximum height of the RGB and Depth components of the RGBD stream. */
 
 #ifdef PYTHON_BINDINGS_BUILD
         /**
@@ -124,8 +122,11 @@ namespace Record3D
          */
         std::function<void(const Record3D::BufferRGB &$rgbFrame,
                            const Record3D::BufferDepth &$depthFrame,
-                           uint32_t $frameWidth,
-                           uint32_t $frameHeight,
+                           uint32_t   $rgbWidth,
+                           uint32_t   $rgbHeight,
+                           uint32_t   $depthWidth,
+                           uint32_t   $depthHeight,
+                           DeviceType $deviceType,
                            Record3D::IntrinsicMatrixCoeffs $K)> onNewFrame{};
 #endif
         /**
@@ -141,8 +142,8 @@ namespace Record3D
          */
         py::array_t<float> GetCurrentDepthFrame()
         {
-            size_t currentFrameWidth = currentFrameWidth_;
-            size_t currentFrameHeight = currentFrameHeight_;
+            size_t currentFrameWidth = currentFrameDepthWidth_;
+            size_t currentFrameHeight = currentFrameDepthHeight_;
 
             size_t bufferSize  = currentFrameWidth * currentFrameHeight * sizeof(float);
             auto result        = py::array_t<float>(currentFrameWidth * currentFrameHeight);
@@ -162,8 +163,8 @@ namespace Record3D
          */
         py::array_t<uint8_t> GetCurrentRGBFrame()
         {
-            size_t currentFrameWidth = currentFrameWidth_;
-            size_t currentFrameHeight = currentFrameHeight_;
+            size_t currentFrameWidth = currentFrameRGBWidth_;
+            size_t currentFrameHeight = currentFrameRGBHeight_;
 
             constexpr int numChannels = 3;
             size_t bufferSize  = currentFrameWidth * currentFrameHeight * numChannels * sizeof(uint8_t);
@@ -184,16 +185,26 @@ namespace Record3D
          */
         IntrinsicMatrixCoeffs GetCurrentIntrinsicMatrix()
         {
-            return intrinsicMatrixCoeffs_;
+            return rgbIntrinsicMatrixCoeffs_;
+        }
+
+        /**
+         * NOTE: This is alternative API for Python.
+         *
+         * @returns the type of camera (TrueDeph = 0, LiDAR = 1).
+         */
+        uint32_t GetCurrentDeviceType()
+        {
+            return (uint32_t) currentDeviceType_;
         }
 #endif
     private:
-        static constexpr size_t depthBufferSize_{MAXIMUM_FRAME_WIDTH * MAXIMUM_FRAME_HEIGHT * sizeof( float ) }; /** Size in bytes of decompressed Depth frame. */
-
-        size_t currentFrameWidth_{ MAXIMUM_FRAME_WIDTH };
-        size_t currentFrameHeight_{ MAXIMUM_FRAME_HEIGHT };
+        size_t currentFrameRGBWidth_{ 0 };
+        size_t currentFrameRGBHeight_{ 0 };
+        size_t currentFrameDepthWidth_{ 0 };
+        size_t currentFrameDepthHeight_{ 0 };
+        DeviceType currentDeviceType_ {};
 
-        uint8_t* compressedDepthBuffer_{ nullptr }; /** Preallocated buffer holding decompressed depth data. */
         uint8_t* lzfseScratchBuffer_{ nullptr }; /** Preallocated LZFSE scratch buffer. */
 
         int socketHandle_{ -1 }; /** Socket handle representing connection to iDevice. */
@@ -204,7 +215,7 @@ 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 intrinsicMatrixCoeffs_{}; /** Holds the intrinsic matrix of the most recent Depth frame. */
+        IntrinsicMatrixCoeffs rgbIntrinsicMatrixCoeffs_{}; /** Holds the intrinsic matrix of the most recent Depth frame. */
     };
 }
 #endif //CPP_RECORD3DSTREAM_H
diff --git a/include/record3d/Record3DStructs.h b/include/record3d/Record3DStructs.h
@@ -17,6 +17,12 @@ namespace Record3D
         std::string udid{ "" };
         uint32_t handle{ 0 };
     };
+
+    enum DeviceType
+    {
+        R3D_DEVICE_TYPE__FACEID = 0,
+        R3D_DEVICE_TYPE__LIDAR
+    };
 }
 
 #endif //CPP_RECORD3DSTRUCTS_H
diff --git a/python-bindings/src/PythonBindings.cpp b/python-bindings/src/PythonBindings.cpp
@@ -34,6 +34,7 @@ PYBIND11_MODULE( record3d, m )
             .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_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.")
             .def_readwrite("on_stream_stopped", &Record3D::Record3DStream::onStreamStopped, "Method called when stream is interrupted.")
             ;

diff --git a/setup.py b/setup.py
@@ -67,14 +67,14 @@ def build_extension(self, ext):
 
 setup(
     name='record3d',
-    version='1.2.0',
+    version='1.3.0',
     license='lgpl-2.1',
     author='Marek Simonik',
     author_email='admin@record3d.app',
     url='https://github.com/marek-simonik/record3d',
     install_requires=[ 'numpy' ],
     keywords=['record3d', 'iOS', 'TrueDepth', 'streaming', 'pointcloud', 'point', 'cloud'],
-    description='Accompanying library for the Record3D iOS app (https://record3d.app/). Allows you to receive RGBD stream from iOS devices with TrueDepth camera(s).',
+    description='Accompanying library for the Record3D iOS app (https://record3d.app/). Allows you to receive RGBD stream from iOS devices with TrueDepth and/or LiDAR camera(s).',
     long_description=long_description,
     long_description_content_type='text/markdown',
     ext_modules=[CMakeExtension('record3d')],

diff --git a/src/DemoMain.cpp b/src/DemoMain.cpp
@@ -28,11 +28,14 @@ class Record3DDemoApp
         };
         stream.onNewFrame = [&](const Record3D::BufferRGB &$rgbFrame,
                                 const Record3D::BufferDepth &$depthFrame,
-                                uint32_t $frameWidth,
-                                uint32_t $frameHeight,
+                                uint32_t $rgbWidth,
+                                uint32_t $rgbHeight,
+                                uint32_t $depthWidth,
+                                uint32_t $depthHeight,
+                                Record3D::DeviceType $deviceType,
                                 Record3D::IntrinsicMatrixCoeffs $K)
         {
-            OnNewFrame( $rgbFrame, $depthFrame, $frameWidth, $frameHeight, $K );
+            OnNewFrame( $rgbFrame, $depthFrame, $rgbWidth, $rgbHeight, $depthWidth, $depthHeight, $deviceType, $K );
         };
 
         // Try connecting to a device.
@@ -63,6 +66,11 @@ class Record3DDemoApp
             {
                 // Wait for the callback thread to receive new frame and unlock this thread
 #ifdef HAS_OPENCV
+                if ( imgRGB_.cols == 0 || imgRGB_.rows == 0 || imgDepth_.cols == 0 || imgDepth_.rows == 0 )
+                {
+                    continue;
+                }
+
                 cv::Mat rgb, depth;
                 {
                     std::lock_guard<std::recursive_mutex> lock(mainThreadLock_);
@@ -73,8 +81,7 @@ class Record3DDemoApp
                 cv::cvtColor( rgb, rgb, cv::COLOR_RGB2BGR );
 
                 // The TrueDepth camera is a selfie camera; we mirror the RGBD frame so it looks plausible.
-                bool areTrueDepthDataBeingStreamed = depth.rows == Record3D::Record3DStream::MAXIMUM_FRAME_HEIGHT && depth.cols == Record3D::Record3DStream::MAXIMUM_FRAME_WIDTH;
-                if ( areTrueDepthDataBeingStreamed )
+                if ( currentDeviceType_ == Record3D::R3D_DEVICE_TYPE__FACEID )
                 {
                     cv::flip( rgb, rgb, 1 );
                     cv::flip( depth, depth, 1 );
@@ -102,37 +109,43 @@ class Record3DDemoApp
 
     void OnNewFrame(const Record3D::BufferRGB &$rgbFrame,
                     const Record3D::BufferDepth &$depthFrame,
-                    uint32_t $frameWidth,
-                    uint32_t $frameHeight,
+                    uint32_t $rgbWidth,
+                    uint32_t $rgbHeight,
+                    uint32_t $depthWidth,
+                    uint32_t $depthHeight,
+                    Record3D::DeviceType $deviceType,
                     Record3D::IntrinsicMatrixCoeffs $K)
     {
+        currentDeviceType_ = (Record3D::DeviceType) $deviceType;
+
 #ifdef HAS_OPENCV
         std::lock_guard<std::recursive_mutex> lock(mainThreadLock_);
         // When we switch between the TrueDepth and the LiDAR camera, the size frame size changes.
         // Recreate the RGB and Depth images with fitting size.
-        if (    imgRGB_.rows != $frameHeight || imgRGB_.cols != $frameWidth
-             || imgDepth_.rows != $frameHeight || imgDepth_.cols != $frameWidth )
+        if (    imgRGB_.rows != $rgbHeight || imgRGB_.cols != $rgbWidth
+             || imgDepth_.rows != $depthHeight || imgDepth_.cols != $depthWidth )
         {
             imgRGB_.release();
             imgDepth_.release();
 
-            imgRGB_ = cv::Mat::zeros( $frameHeight, $frameWidth, CV_8UC3);
-            imgDepth_ = cv::Mat::zeros( $frameHeight, $frameWidth, CV_32F );
+            imgRGB_ = cv::Mat::zeros( $rgbHeight, $rgbWidth, CV_8UC3);
+            imgDepth_ = cv::Mat::zeros( $depthHeight, $depthWidth, CV_32F );
         }
 
         // 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(), $frameWidth * $frameHeight * numRGBChannels * sizeof(uint8_t));
-        memcpy( imgDepth_.data, $depthFrame.data(), $frameWidth * $frameHeight * sizeof(float));
+        memcpy( imgRGB_.data, $rgbFrame.data(), $rgbWidth * $rgbHeight * numRGBChannels * sizeof(uint8_t));
+        memcpy( imgDepth_.data, $depthFrame.data(), $depthWidth * $depthHeight * sizeof(float));
 #endif
     }
 
 private:
     std::recursive_mutex mainThreadLock_{};
+    Record3D::DeviceType currentDeviceType_{};
 
 #ifdef HAS_OPENCV
-    cv::Mat imgRGB_ = cv::Mat::zeros(Record3D::Record3DStream::MAXIMUM_FRAME_HEIGHT, Record3D::Record3DStream::MAXIMUM_FRAME_WIDTH, CV_8UC3);;
-    cv::Mat imgDepth_ = cv::Mat::zeros(Record3D::Record3DStream::MAXIMUM_FRAME_HEIGHT, Record3D::Record3DStream::MAXIMUM_FRAME_WIDTH, CV_32F );;
+    cv::Mat imgRGB_{};
+    cv::Mat imgDepth_{};
 #endif
 };