From d712f4026d26a6cf3c7f697e4199ae5bf47395f1 Mon Sep 17 00:00:00 2001 From: Marek Simonik Date: Tue, 16 Aug 2022 18:21:44 +0200 Subject: [PATCH] Bug fix, added camera pose streaming --- CMakeLists.txt | 4 + README.md | 2 + demo-main.py | 3 + include/record3d/Record3DStream.h | 14 +++- include/record3d/Record3DStructs.h | 14 ++++ .../pybind11/include/pybind11/numpy.h | 1 + python-bindings/src/PythonBindings.cpp | 11 +++ setup.py | 2 +- src/DemoMain.cpp | 83 ++++++++++--------- src/Record3DStream.cpp | 10 ++- 10 files changed, 100 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 06981c9..f2fc3a9 100644 --- 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 index e5448c1..17c4a61 100644 --- 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 index 0d0ccc9..e8aac7c 100644 --- 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 index 2fa5e04..d704920 100644 --- 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 depthImageBuffer_{}; /** Holds the most recent Depth buffer. */ std::vector 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 index 5f5064d..6e5858a 100644 --- 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 index 8b21d3d..5e4ae39 100644 --- 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 index 756fe95..ac5de98 100644 --- 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_( 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_( 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 index 15d042e..fb4bb9d 100644 --- 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='admin@record3d.app', diff --git a/src/DemoMain.cpp b/src/DemoMain.cpp index 38a53f9..d7b5ab2 100644 --- a/src/DemoMain.cpp +++ b/src/DemoMain.cpp @@ -21,26 +21,27 @@ 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" ); @@ -48,17 +49,17 @@ class Record3DDemoApp } 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" ); @@ -66,14 +67,15 @@ 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 lock(mainThreadLock_); + std::lock_guard 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 lock(mainThreadLock_); + std::lock_guard 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 index 5ad3abf..dc127e9 100644 --- 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 } }