diff --git a/examples/pose-predict/readme.md b/examples/pose-predict/readme.md new file mode 100644 index 00000000000..7a0d538543d --- /dev/null +++ b/examples/pose-predict/readme.md @@ -0,0 +1,51 @@ +# rs-pose-predict Sample + +> In order to run this example, a device supporting pose stream (T265) is required. + +## Overview +This sample builds on the concepts presented in [`rs-pose` example](../pose/) and shows how pose data can be used asynchronously to implement simple pose prediction. + +## Expected Output +The application should open a window in which it prints the **predicted** x, y, z values of the device position relative to its initial position. + +## Code Overview + +We start by configuring the pipeline to pose stream, similar to previous example: +```cpp +// Declare RealSense pipeline, encapsulating the actual device and sensors +rs2::pipeline pipe; +// Create a configuration for configuring the pipeline with a non default profile +rs2::config cfg; +// Add pose stream +cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF); +``` + +Next, we define new frame callback ([learn more about callback API](../callback)) that would execute for every new pose event from the sensor. +```cpp +// Define frame callback +// The callback is executed on a sensor thread and can be called simultaneously from multiple sensors +// Therefore any modification to common memory should be done under lock +std::mutex mutex; +auto callback = [&](const rs2::frame& frame) +{ + std::lock_guard lock(mutex); + if (rs2::pose_frame fp = frame.as()) { + rs2_pose pose_data = fp.get_pose_data(); + rs2_pose predicted_pose = predict_pose(pose_data, dt_us); + std::cout << pose_data.tracker_confidence << " : " << std::setprecision(3) << std::fixed << + predicted_pose.translation.x << " " << + predicted_pose.translation.y << " " << + predicted_pose.translation.z << " (meters)\r"; + } +}; +``` + +Once pipeline is started with a callback, main thread can go to sleep until termination signal is received: +```cpp +// Start streaming through the callback with default recommended configuration +rs2::pipeline_profile profiles = pipe.start(cfg, callback); +std::cout << "started thread\n"; +while(true) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); +} +``` diff --git a/examples/readme.md b/examples/readme.md index a4d51fd765c..d80ba2ae5b1 100644 --- a/examples/readme.md +++ b/examples/readme.md @@ -23,6 +23,7 @@ For a detailed explanations and API documentation see our [Documentation](../doc 11. [Measure](./measure) - Lets the user measure the dimentions of 3D objects in a stream. #### Tracking Examples: 12. [Pose](./pose) - Demonstarates how to obtain data from pose frames. +12. [Pose Prediction](./pose-predict) - Demonstarates how to use tracking camera asynchroniously to implement simple pose prediction. ### C Examples: 1. [Depth](./C/depth) - Demonstrates how to stream depth data and prints a simple text-based representation of the depth image. diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index 1bc65bf9948..5bc564159d0 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -24,7 +24,7 @@ extern "C" { #define RS2_API_MAJOR_VERSION 2 #define RS2_API_MINOR_VERSION 19 -#define RS2_API_PATCH_VERSION 0 +#define RS2_API_PATCH_VERSION 1 #define RS2_API_BUILD_VERSION 0 #ifndef STRINGIFY diff --git a/package.xml b/package.xml index f3a940b3efd..5f6d4e6ab82 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ librealsense2 - 2.19.0 + 2.19.1 Library for capturing data from the Intel(R) RealSense(TM) SR300 and D400 cameras. This effort was initiated to better support researchers, creative coders, and app developers in domains such as robotics, virtual reality, and the internet of things. Several often-requested features of RealSense(TM); devices are implemented in this project, including multi-camera capture.