Bug fix, added camera pose streaming

marek-simonik · Aug 16, 2022 · d712f40 · d712f40
1 parent 7a3764b
commit d712f40
Show file tree

Hide file tree

Showing 10 changed files with 100 additions and 44 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -2,6 +2,10 @@ cmake_minimum_required(VERSION 3.13)
 project(record3d)
 set(CMAKE_CXX_STANDARD 14)
 
+if (APPLE)
+    set(CMAKE_OSX_ARCHITECTURES "x86_64;arm64" CACHE INTERNAL "" FORCE)
+endif()
+
 if(UNIX AND NOT APPLE)
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 endif()

diff --git a/README.md b/README.md
@@ -1,5 +1,7 @@
 # [*Record3D – Point Cloud Animation and Streaming*](https://record3d.app/): the accompanying library 
 
+**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.**
 
 **2020/09/17 Update**: Introduced LiDAR support. To be used with Record3D 1.4 and newer.

diff --git a/demo-main.py b/demo-main.py
@@ -50,7 +50,10 @@ 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())
+            camera_pose = self.session.get_camera_pose()  # Quaternion + world position (accessible via camera_pose.[qx|qy|qz|qw|tx|ty|tz])
+
             print(intrinsic_mat)
+
             # You can now e.g. create point cloud by projecting the depth map using the intrinsic matrix.
 
             # Postprocess it

diff --git a/include/record3d/Record3DStream.h b/include/record3d/Record3DStream.h
@@ -127,7 +127,8 @@ namespace Record3D
                            uint32_t   $depthWidth,
                            uint32_t   $depthHeight,
                            DeviceType $deviceType,
-                           Record3D::IntrinsicMatrixCoeffs $K)> onNewFrame{};
+                           Record3D::IntrinsicMatrixCoeffs $K,
+                           Record3D::CameraPose $cameraPose)> onNewFrame{};
 #endif
         /**
          * This function is called when stream ends (that can happen either due to transfer error or by calling the `Disconnect()` method).
@@ -188,6 +189,16 @@ namespace Record3D
             return rgbIntrinsicMatrixCoeffs_;
         }
 
+        /**
+         * NOTE: This is alternative API for Python.
+         *
+         * @returns the intrinsic matrix of the lastly received Depth frame.
+         */
+        CameraPose GetCurrentCameraPose()
+        {
+            return cameraPose_;
+        }
+
         /**
          * NOTE: This is alternative API for Python.
          *
@@ -216,6 +227,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 rgbIntrinsicMatrixCoeffs_{}; /** Holds the intrinsic matrix of the most recent Depth frame. */
+        CameraPose cameraPose_{}; /** Holds the world position of the most recent camera frame. */
     };
 }
 #endif //CPP_RECORD3DSTREAM_H
diff --git a/include/record3d/Record3DStructs.h b/include/record3d/Record3DStructs.h
@@ -11,6 +11,20 @@ namespace Record3D
         float ty;
     };
 
+    struct CameraPose
+    {
+        // Quaternion coefficients
+        float qx;
+        float qy;
+        float qz;
+        float qw;
+
+        // Position
+        float tx;
+        float ty;
+        float tz;
+    };
+
     struct DeviceInfo
     {
         uint32_t productId{ 0 };

diff --git a/python-bindings/pybind11/include/pybind11/numpy.h b/python-bindings/pybind11/include/pybind11/numpy.h
@@ -27,6 +27,7 @@
 #if defined(_MSC_VER)
 #  pragma warning(push)
 #  pragma warning(disable: 4127) // warning C4127: Conditional expression is constant
+typedef SSIZE_T ssize_t;
 #endif
 
 /* This will be true on all flat address space platforms and allows us to reduce the

diff --git a/python-bindings/src/PythonBindings.cpp b/python-bindings/src/PythonBindings.cpp
@@ -26,6 +26,16 @@ PYBIND11_MODULE( record3d, m )
             .def_readonly( "ty", &Record3D::IntrinsicMatrixCoeffs::ty )
             ;
 
+    py::class_<Record3D::CameraPose>( m, "CameraPose" )
+            .def_readonly( "qx", &Record3D::CameraPose::qx )
+            .def_readonly( "qy", &Record3D::CameraPose::qy )
+            .def_readonly( "qz", &Record3D::CameraPose::qz )
+            .def_readonly( "qw", &Record3D::CameraPose::qw )
+            .def_readonly( "tx", &Record3D::CameraPose::tx )
+            .def_readonly( "ty", &Record3D::CameraPose::ty )
+            .def_readonly( "tz", &Record3D::CameraPose::tz )
+            ;
+
     py::class_<Record3D::Record3DStream>( m, "Record3DStream" )
             .def(py::init<>())
             .def_static( "get_connected_devices", &Record3D::Record3DStream::GetConnectedDevices, "Get IDs of connected devices." )
@@ -34,6 +44,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_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.")
             .def_readwrite("on_stream_stopped", &Record3D::Record3DStream::onStreamStopped, "Method called when stream is interrupted.")

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

diff --git a/src/DemoMain.cpp b/src/DemoMain.cpp
@@ -21,59 +21,61 @@ class Record3DDemoApp
 public:
     void Run()
     {
-        Record3D::Record3DStream stream{};
+        Record3D::Record3DStream stream { };
         stream.onStreamStopped = [&]
         {
             OnStreamStopped();
         };
-        stream.onNewFrame = [&](const Record3D::BufferRGB &$rgbFrame,
-                                const Record3D::BufferDepth &$depthFrame,
-                                uint32_t $rgbWidth,
-                                uint32_t $rgbHeight,
-                                uint32_t $depthWidth,
-                                uint32_t $depthHeight,
-                                Record3D::DeviceType $deviceType,
-                                Record3D::IntrinsicMatrixCoeffs $K)
+        stream.onNewFrame = [&]( const Record3D::BufferRGB &$rgbFrame,
+                                 const Record3D::BufferDepth &$depthFrame,
+                                 uint32_t $rgbWidth,
+                                 uint32_t $rgbHeight,
+                                 uint32_t $depthWidth,
+                                 uint32_t $depthHeight,
+                                 Record3D::DeviceType $deviceType,
+                                 Record3D::IntrinsicMatrixCoeffs $K,
+                                 Record3D::CameraPose $cameraPose )
         {
-            OnNewFrame( $rgbFrame, $depthFrame, $rgbWidth, $rgbHeight, $depthWidth, $depthHeight, $deviceType, $K );
+            OnNewFrame( $rgbFrame, $depthFrame, $rgbWidth, $rgbHeight, $depthWidth, $depthHeight, $deviceType, $K, $cameraPose );
         };
 
         // Try connecting to a device.
         const auto &devs = Record3D::Record3DStream::GetConnectedDevices();
-        if ( devs.empty())
+        if ( devs.empty() )
         {
             fprintf( stderr,
                      "No iOS devices found. Ensure you have connected your iDevice via USB to this computer.\n" );
             return;
         }
         else
         {
-            printf( "Found %lu iOS device(s):\n", devs.size());
-            for ( const auto &dev : devs )
+            printf( "Found %lu iOS device(s):\n", devs.size() );
+            for ( const auto &dev: devs )
             {
-                printf( "\tDevice ID: %u\n\tUDID: %s\n\n", dev.productId, dev.udid.c_str());
+                printf( "\tDevice ID: %u\n\tUDID: %s\n\n", dev.productId, dev.udid.c_str() );
             }
         }
 
-        const auto &selectedDevice = devs[ 0 ];
+        const auto &selectedDevice = devs[0];
         printf( "Trying to connect to device with ID %u.\n", selectedDevice.productId );
 
-        bool isConnected = stream.ConnectToDevice( devs[ 0 ] );
+        bool isConnected = stream.ConnectToDevice( devs[0] );
         if ( isConnected )
         {
             printf( "Connected and starting to stream. Enable USB streaming in the Record3D iOS app (https://record3d.app/) in case you don't see RGBD stream.\n" );
             while ( true )
             {
                 // 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_);
+                    std::lock_guard<std::recursive_mutex> lock( mainThreadLock_ );
+
+                    if ( imgRGB_.cols == 0 || imgRGB_.rows == 0 || imgDepth_.cols == 0 || imgDepth_.rows == 0 )
+                    {
+                        continue;
+                    }
+
                     rgb = imgRGB_.clone();
                     depth = imgDepth_.clone();
                 }
@@ -107,52 +109,53 @@ class Record3DDemoApp
         fprintf( stderr, "Stream stopped!" );
     }
 
-    void OnNewFrame(const Record3D::BufferRGB &$rgbFrame,
-                    const Record3D::BufferDepth &$depthFrame,
-                    uint32_t $rgbWidth,
-                    uint32_t $rgbHeight,
-                    uint32_t $depthWidth,
-                    uint32_t $depthHeight,
-                    Record3D::DeviceType $deviceType,
-                    Record3D::IntrinsicMatrixCoeffs $K)
+    void OnNewFrame( const Record3D::BufferRGB &$rgbFrame,
+                     const Record3D::BufferDepth &$depthFrame,
+                     uint32_t $rgbWidth,
+                     uint32_t $rgbHeight,
+                     uint32_t $depthWidth,
+                     uint32_t $depthHeight,
+                     Record3D::DeviceType $deviceType,
+                     Record3D::IntrinsicMatrixCoeffs $K,
+                     Record3D::CameraPose $cameraPose )
     {
         currentDeviceType_ = (Record3D::DeviceType) $deviceType;
 
 #ifdef HAS_OPENCV
-        std::lock_guard<std::recursive_mutex> lock(mainThreadLock_);
+        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 != $rgbHeight || imgRGB_.cols != $rgbWidth
+        if ( imgRGB_.rows != $rgbHeight || imgRGB_.cols != $rgbWidth
              || imgDepth_.rows != $depthHeight || imgDepth_.cols != $depthWidth )
         {
             imgRGB_.release();
             imgDepth_.release();
 
-            imgRGB_ = cv::Mat::zeros( $rgbHeight, $rgbWidth, CV_8UC3);
+            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(), $rgbWidth * $rgbHeight * numRGBChannels * sizeof(uint8_t));
-        memcpy( imgDepth_.data, $depthFrame.data(), $depthWidth * $depthHeight * 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_{};
+    std::recursive_mutex mainThreadLock_ { };
+    Record3D::DeviceType currentDeviceType_ { };
 
 #ifdef HAS_OPENCV
-    cv::Mat imgRGB_{};
-    cv::Mat imgDepth_{};
+    cv::Mat imgRGB_ { };
+    cv::Mat imgDepth_ { };
 #endif
 };
 
 
 int main()
 {
-    Record3DDemoApp app{};
+    Record3DDemoApp app { };
     app.Run();
 }
 

diff --git a/src/Record3DStream.cpp b/src/Record3DStream.cpp
@@ -149,7 +149,12 @@ namespace Record3D
             memcpy((void*) &rgbIntrinsicMatrixCoeffs_, rawMessageBuffer.data() + offset, currSize );
             offset += currSize;
 
-            // 3.3 Read and decode the RGB JPEG frame
+            // 3.3 Read the camera pose data
+            currSize = sizeof( CameraPose );
+            memcpy( (void*) &cameraPose_, rawMessageBuffer.data() + offset, currSize );
+            offset += currSize;
+
+            // 3.3 Read and decode the RGB frame
             currSize = record3DHeader.rgbSize;
             int loadedWidth, loadedHeight, loadedChannels;
             uint8_t* rgbPixels = stbi_load_from_memory( rawMessageBuffer.data() + offset, currSize, &loadedWidth, &loadedHeight, &loadedChannels, STBI_rgb );
@@ -190,7 +195,8 @@ namespace Record3D
                             record3DHeader.depthWidth,
                             record3DHeader.depthHeight,
                             currentDeviceType_,
-                            rgbIntrinsicMatrixCoeffs_ );
+                            rgbIntrinsicMatrixCoeffs_,
+                            cameraPose_ );
 #endif
             }
         }