rs-pose-predict.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2019 Intel Corporation. All Rights Reserved.
3 #include <librealsense2/rs.hpp>
4 #include <iostream>
5 #include <iomanip>
6 #include <chrono>
7 #include <thread>
8 #include <mutex>
9 
10 #include <math.h>
11 #include <float.h>
12 
14 {
15  float x = v.x/2, y = v.y/2, z = v.z/2, th2, th = sqrtf(th2 = x*x + y*y + z*z);
16  float c = cosf(th), s = th2 < sqrtf(120*FLT_EPSILON) ? 1-th2/6 : sinf(th)/th;
17  rs2_quaternion Q = { s*x, s*y, s*z, c };
18  return Q;
19 }
20 
22 {
23  rs2_quaternion Q = {
24  a.x * b.w + a.w * b.x - a.z * b.y + a.y * b.z,
25  a.y * b.w + a.z * b.x + a.w * b.y - a.x * b.z,
26  a.z * b.w - a.y * b.x + a.x * b.y + a.w * b.z,
27  a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
28  };
29  return Q;
30 }
31 
33 {
34  rs2_pose P = pose;
35  P.translation.x = dt_s * (dt_s/2 * pose.acceleration.x + pose.velocity.x) + pose.translation.x;
36  P.translation.y = dt_s * (dt_s/2 * pose.acceleration.y + pose.velocity.y) + pose.translation.y;
37  P.translation.z = dt_s * (dt_s/2 * pose.acceleration.z + pose.velocity.z) + pose.translation.z;
38  rs2_vector W = {
39  dt_s * (dt_s/2 * pose.angular_acceleration.x + pose.angular_velocity.x),
40  dt_s * (dt_s/2 * pose.angular_acceleration.y + pose.angular_velocity.y),
41  dt_s * (dt_s/2 * pose.angular_acceleration.z + pose.angular_velocity.z),
42  };
43  P.rotation = quaternion_multiply(quaternion_exp(W), pose.rotation);
44  return P;
45 }
46 
47 int main(int argc, char * argv[]) try
48 {
49  // Declare RealSense pipeline, encapsulating the actual device and sensors
51  // Create a configuration for configuring the pipeline with a non default profile
53  // Add pose stream
55 
56  // Define frame callback
57  // The callback is executed on a sensor thread and can be called simultaneously from multiple sensors
58  // Therefore any modification to common memory should be done under lock
59  std::mutex mutex;
60  auto callback = [&](const rs2::frame& frame)
61  {
62  std::lock_guard<std::mutex> lock(mutex);
63  if (rs2::pose_frame fp = frame.as<rs2::pose_frame>()) {
64  rs2_pose pose_data = fp.get_pose_data();
65  auto now = std::chrono::system_clock::now().time_since_epoch();
66  double now_ms = static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(now).count());
67  double pose_time_ms = fp.get_timestamp();
68  float dt_s = static_cast<float>(std::max(0., (now_ms - pose_time_ms)/1000.));
69  rs2_pose predicted_pose = predict_pose(pose_data, dt_s);
70  std::cout << "Predicted " << std::fixed << std::setprecision(3) << dt_s*1000 << "ms " <<
71  "Confidence: " << pose_data.tracker_confidence << " T: " <<
72  predicted_pose.translation.x << " " <<
73  predicted_pose.translation.y << " " <<
74  predicted_pose.translation.z << " (meters) \r";
75  }
76  };
77 
78  // Start streaming through the callback with default recommended configuration
80  std::cout << "started thread\n";
81  while(true) {
82  std::this_thread::sleep_for(std::chrono::milliseconds(100));
83  }
84 
85  return EXIT_SUCCESS;
86 }
87 catch (const rs2::error & e)
88 {
89  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
90  return EXIT_FAILURE;
91 }
92 catch (const std::exception& e)
93 {
94  std::cerr << e.what() << std::endl;
95  return EXIT_FAILURE;
96 }
static const textual_icon lock
Definition: model-views.h:218
GLboolean GLboolean GLboolean b
GLint y
rs2_quaternion quaternion_multiply(rs2_quaternion a, rs2_quaternion b)
rs2_vector angular_acceleration
Definition: rs_types.h:147
float z
Definition: rs_types.h:131
unsigned int tracker_confidence
Definition: rs_types.h:148
GLdouble s
rs2_vector translation
Definition: rs_types.h:142
GLdouble GLdouble z
rs2_vector angular_velocity
Definition: rs_types.h:146
e
Definition: rmse.py:177
::realsense_legacy_msgs::pose_< std::allocator< void > > pose
Definition: pose.h:88
GLboolean GLboolean GLboolean GLboolean a
Quaternion used to represent rotation.
Definition: rs_types.h:135
const std::string & get_failed_args() const
Definition: rs_types.hpp:117
float y
Definition: rs_types.h:131
const int W
rs2_vector velocity
Definition: rs_types.h:143
std::ostream & cout()
const GLubyte * c
Definition: glext.h:12690
GLdouble x
def callback(frame)
Definition: t265_stereo.py:91
rs2_quaternion quaternion_exp(rs2_vector v)
float x
Definition: rs_types.h:131
void enable_stream(rs2_stream stream_type, int stream_index, int width, int height, rs2_format format=RS2_FORMAT_ANY, int framerate=0)
3D vector in Euclidean coordinate space
Definition: rs_types.h:129
GLint GLsizei count
std::ostream & cerr()
rs2_vector acceleration
Definition: rs_types.h:144
rs2_pose predict_pose(rs2_pose &pose, float dt_s)
pipeline_profile start()
int main(int argc, char *argv[])
GLdouble v
const std::string & get_failed_function() const
Definition: rs_types.hpp:112
T as() const
Definition: rs_frame.hpp:580


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:40