rs-motion.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 <mutex>
5 #include "example.hpp" // Include short list of convenience functions for rendering
6 #include <cstring>
7 
8 struct short3
9 {
11 };
12 
13 #include "d435.h"
14 
15 void draw_axes()
16 {
17  glLineWidth(2);
19  // Draw x, y, z axes
20  glColor3f(1, 0, 0); glVertex3f(0, 0, 0); glVertex3f(-1, 0, 0);
21  glColor3f(0, 1, 0); glVertex3f(0, 0, 0); glVertex3f(0, -1, 0);
22  glColor3f(0, 0, 1); glVertex3f(0, 0, 0); glVertex3f(0, 0, 1);
23  glEnd();
24 
25  glLineWidth(1);
26 }
27 
28 void draw_floor()
29 {
31  glColor4f(0.4f, 0.4f, 0.4f, 1.f);
32  // Render "floor" grid
33  for (int i = 0; i <= 8; i++)
34  {
35  glVertex3i(i - 4, 1, 0);
36  glVertex3i(i - 4, 1, 8);
37  glVertex3i(-4, 1, i);
38  glVertex3i(4, 1, i);
39  }
40  glEnd();
41 }
42 
43 void render_scene(glfw_state app_state)
44 {
45  glClearColor(0.0, 0.0, 0.0, 1.0);
46  glColor3f(1.0, 1.0, 1.0);
47 
50  gluPerspective(60.0, 4.0 / 3.0, 1, 40);
51 
54 
56  gluLookAt(1, 0, 5, 1, 0, 0, 0, -1, 0);
57 
58  glTranslatef(0, 0, +0.5f + app_state.offset_y*0.05f);
59  glRotated(app_state.pitch, -1, 0, 0);
60  glRotated(app_state.yaw, 0, 1, 0);
61  draw_floor();
62 }
63 
65 {
66  std::vector<float3> positions, normals;
67  std::vector<short3> indexes;
68 public:
69  // Initialize renderer with data needed to draw the camera
71  {
72  uncompress_d435_obj(positions, normals, indexes);
73  }
74 
75  // Takes the calculated angle as input and rotates the 3D camera model accordignly
76  void render_camera(float3 theta)
77  {
78 
81 
82  glPushMatrix();
83  // Set the rotation, converting theta to degrees
84  glRotatef(theta.x * 180 / PI_FL, 0, 0, -1);
85  glRotatef(theta.y * 180 / PI_FL, 0, -1, 0);
86  glRotatef((theta.z - PI_FL / 2) * 180 / PI_FL, -1, 0, 0);
87 
88  draw_axes();
89 
90  // Scale camera drawing
91  glScalef(0.035f, 0.035f, 0.035f);
92 
94  // Draw the camera
95  for (auto& i : indexes)
96  {
97  glVertex3fv(&positions[i.x].x);
98  glVertex3fv(&positions[i.y].x);
99  glVertex3fv(&positions[i.z].x);
100  glColor4f(0.05f, 0.05f, 0.05f, 0.3f);
101  }
102  glEnd();
103 
104  glPopMatrix();
105 
107  glFlush();
108  }
109 
110 };
111 
113 {
114  // theta is the angle of camera rotation in x, y and z components
116  std::mutex theta_mtx;
117  /* alpha indicates the part that gyro and accelerometer take in computation of theta; higher alpha gives more weight to gyro, but too high
118  values cause drift; lower alpha gives more weight to accelerometer, which is more sensitive to disturbances */
119  float alpha = 0.98f;
120  bool firstGyro = true;
121  bool firstAccel = true;
122  // Keeps the arrival time of previous gyro frame
123  double last_ts_gyro = 0;
124 public:
125  // Function to calculate the change in angle of motion based on data from gyro
126  void process_gyro(rs2_vector gyro_data, double ts)
127  {
128  if (firstGyro) // On the first iteration, use only data from accelerometer to set the camera's initial position
129  {
130  firstGyro = false;
131  last_ts_gyro = ts;
132  return;
133  }
134  // Holds the change in angle, as calculated from gyro
135  float3 gyro_angle;
136 
137  // Initialize gyro_angle with data from gyro
138  gyro_angle.x = gyro_data.x; // Pitch
139  gyro_angle.y = gyro_data.y; // Yaw
140  gyro_angle.z = gyro_data.z; // Roll
141 
142  // Compute the difference between arrival times of previous and current gyro frames
143  double dt_gyro = (ts - last_ts_gyro) / 1000.0;
144  last_ts_gyro = ts;
145 
146  // Change in angle equals gyro measures * time passed since last measurement
147  gyro_angle = gyro_angle * static_cast<float>(dt_gyro);
148 
149  // Apply the calculated change of angle to the current angle (theta)
150  std::lock_guard<std::mutex> lock(theta_mtx);
151  theta.add(-gyro_angle.z, -gyro_angle.y, gyro_angle.x);
152  }
153 
154  void process_accel(rs2_vector accel_data)
155  {
156  // Holds the angle as calculated from accelerometer data
157  float3 accel_angle;
158 
159  // Calculate rotation angle from accelerometer data
160  accel_angle.z = atan2(accel_data.y, accel_data.z);
161  accel_angle.x = atan2(accel_data.x, sqrt(accel_data.y * accel_data.y + accel_data.z * accel_data.z));
162 
163  // If it is the first iteration, set initial pose of camera according to accelerometer data (note the different handling for Y axis)
164  std::lock_guard<std::mutex> lock(theta_mtx);
165  if (firstAccel)
166  {
167  firstAccel = false;
168  theta = accel_angle;
169  // Since we can't infer the angle around Y axis using accelerometer data, we'll use PI as a convetion for the initial pose
170  theta.y = PI_FL;
171  }
172  else
173  {
174  /*
175  Apply Complementary Filter:
176  - high-pass filter = theta * alpha: allows short-duration signals to pass through while filtering out signals
177  that are steady over time, is used to cancel out drift.
178  - low-pass filter = accel * (1- alpha): lets through long term changes, filtering out short term fluctuations
179  */
180  theta.x = theta.x * alpha + accel_angle.x * (1 - alpha);
181  theta.z = theta.z * alpha + accel_angle.z * (1 - alpha);
182  }
183  }
184 
185  // Returns the current rotation angle
187  {
188  std::lock_guard<std::mutex> lock(theta_mtx);
189  return theta;
190  }
191 };
192 
193 
195 {
196  bool found_gyro = false;
197  bool found_accel = false;
199  for (auto dev : ctx.query_devices())
200  {
201  // The same device should support gyro and accel
202  found_gyro = false;
203  found_accel = false;
204  for (auto sensor : dev.query_sensors())
205  {
206  for (auto profile : sensor.get_stream_profiles())
207  {
208  if (profile.stream_type() == RS2_STREAM_GYRO)
209  found_gyro = true;
210 
211  if (profile.stream_type() == RS2_STREAM_ACCEL)
212  found_accel = true;
213  }
214  }
215  if (found_gyro && found_accel)
216  break;
217  }
218  return found_gyro && found_accel;
219 }
220 
221 int main(int argc, char * argv[]) try
222 {
223  // Before running the example, check that a device supporting IMU is connected
224  if (!check_imu_is_supported())
225  {
226  std::cerr << "Device supporting IMU not found";
227  return EXIT_FAILURE;
228  }
229 
230  // Initialize window for rendering
231  window app(1280, 720, "RealSense Motion Example");
232  // Construct an object to manage view state
233  glfw_state app_state(0.0, 0.0);
234  // Register callbacks to allow manipulation of the view state
235  register_glfw_callbacks(app, app_state);
236 
237  // Declare RealSense pipeline, encapsulating the actual device and sensors
239  // Create a configuration for configuring the pipeline with a non default profile
241 
242  // Add streams of gyro and accelerometer to configuration
245 
246  // Declare object for rendering camera motion
248  // Declare object that handles camera pose calculations
250 
251  // Start streaming with the given configuration;
252  // Note that since we only allow IMU streams, only single frames are produced
253  auto profile = pipe.start(cfg, [&](rs2::frame frame)
254  {
255  // Cast the frame that arrived to motion frame
256  auto motion = frame.as<rs2::motion_frame>();
257  // If casting succeeded and the arrived frame is from gyro stream
258  if (motion && motion.get_profile().stream_type() == RS2_STREAM_GYRO && motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F)
259  {
260  // Get the timestamp of the current frame
261  double ts = motion.get_timestamp();
262  // Get gyro measures
263  rs2_vector gyro_data = motion.get_motion_data();
264  // Call function that computes the angle of motion based on the retrieved measures
265  algo.process_gyro(gyro_data, ts);
266  }
267  // If casting succeeded and the arrived frame is from accelerometer stream
268  if (motion && motion.get_profile().stream_type() == RS2_STREAM_ACCEL && motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F)
269  {
270  // Get accelerometer measures
271  rs2_vector accel_data = motion.get_motion_data();
272  // Call function that computes the angle of motion based on the retrieved measures
273  algo.process_accel(accel_data);
274  }
275  });
276 
277  // Main loop
278  while (app)
279  {
280  // Configure scene, draw floor, handle manipultation by the user etc.
281  render_scene(app_state);
282  // Draw the camera according to the computed theta
283  camera.render_camera(algo.get_theta());
284  }
285  // Stop the pipeline
286  pipe.stop();
287 
288  return EXIT_SUCCESS;
289 }
290 catch (const rs2::error & e)
291 {
292  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
293  return EXIT_FAILURE;
294 }
295 catch (const std::exception& e)
296 {
297  std::cerr << e.what() << std::endl;
298  return EXIT_FAILURE;
299 }
static const textual_icon lock
Definition: model-views.h:218
float z
Definition: example.hpp:36
std::vector< float3 > positions
Definition: rs-motion.cpp:66
void draw_axes()
Definition: rs-motion.cpp:15
#define glRotatef
float z
Definition: rs_types.h:131
#define GL_ONE
#define glBegin
double yaw
Definition: example.hpp:875
#define glPopMatrix
#define GL_PROJECTION
device_list query_devices() const
Definition: rs_context.hpp:112
uint16_t z
Definition: rs-motion.cpp:10
double pitch
Definition: example.hpp:876
#define glVertex3fv
unsigned short uint16_t
Definition: stdint.h:79
#define glFlush
#define GL_BLEND
#define glLoadIdentity
int main(int argc, char *argv[])
Definition: rs-motion.cpp:221
void uncompress_d435_obj(std::vector< float3 > &vertex_data, std::vector< float3 > &normals, std::vector< short3 > &index_data)
Definition: d435.h:8
#define glColor4f
#define glVertex3f
e
Definition: rmse.py:177
#define glEnable
std::vector< short3 > indexes
Definition: rs-motion.cpp:67
void add(float t1, float t2, float t3)
Definition: example.hpp:61
#define PI_FL
Definition: example.hpp:25
const std::string & get_failed_args() const
Definition: rs_types.hpp:117
float y
Definition: rs_types.h:131
GLdouble f
GLfloat GLfloat GLfloat alpha
#define GL_COLOR_BUFFER_BIT
#define GL_LINES
#define glLineWidth
#define glPushMatrix
double get_timestamp() const
Definition: rs_frame.hpp:474
#define glClear
float offset_y
Definition: example.hpp:881
#define glTranslatef
#define glEnd
float y
Definition: example.hpp:36
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)
void render_camera(float3 theta)
Definition: rs-motion.cpp:76
#define GL_MODELVIEW
uint16_t x
Definition: rs-motion.cpp:10
3D vector in Euclidean coordinate space
Definition: rs_types.h:129
#define glColor3f
void process_accel(rs2_vector accel_data)
Definition: rs-motion.cpp:154
#define glVertex3i
#define glScalef
static const textual_icon camera
Definition: model-views.h:219
#define glClearColor
std::ostream & cerr()
void render_scene(glfw_state app_state)
Definition: rs-motion.cpp:43
void process_gyro(rs2_vector gyro_data, double ts)
Definition: rs-motion.cpp:126
std::mutex theta_mtx
Definition: rs-motion.cpp:116
int i
#define glRotated
pipeline_profile start()
#define glBlendFunc
void draw_floor()
Definition: rs-motion.cpp:28
void register_glfw_callbacks(window &app, glfw_state &app_state)
Definition: example.hpp:1037
#define glMatrixMode
const std::string & get_failed_function() const
Definition: rs_types.hpp:112
float x
Definition: example.hpp:36
uint16_t y
Definition: rs-motion.cpp:10
bool check_imu_is_supported()
Definition: rs-motion.cpp:194
#define GL_TRIANGLES
#define glDisable
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