Overview #
The Motion feature provides real-time estimation of the camera’s orientation (Roll, Pitch, Yaw) by fusing data from the onboard IMU sensors (gyroscope and accelerometer).
Expected Output #
Prerequisite #
Tutorial #
C++
static constexpr bool GYRO_IS_DEG_PER_SEC = true;
static constexpr double DEFAULT_ALPHA = 0.98;
static constexpr int CALIBRATION_SAMPLES = 200;
static constexpr int CALIB_TIMEOUT_MS = 5000;
static inline double rad2deg(double r) { return r * 180.0 / M_PI; }
static inline double deg2rad(double d) { return d * M_PI / 180.0; }
static inline double wrapPi(double a)
{
while (a > M_PI) a -= 2.0 * M_PI;
while (a < -M_PI) a += 2.0 * M_PI;
return a;
}- Defines IMU unit assumption (gyro in deg/s), complementary filter alpha, calibration sample count/timeout, plus helpers for degree↔radian conversion and yaw wrapping to
[-π, π].
C++
struct ImuSample
{
bool ok = false;
double ax = 0, ay = 0, az = 0; // accel
double gx = 0, gy = 0, gz = 0; // gyro }
static ImuSample read_imu_from_device(Device& device)
static ImuSample read_imu_from_depth(VideoStream& depth)- ImuSample is a unified container for accel/gyro. Two read paths are implemented:
1. Device Property (preferred)
2. Depth Stream Property (fallback)
C++
double roll_acc = atan2(s.ay, s.az);
double pitch_acc = atan2(-s.ax, sqrt(s.ay * s.ay + s.az * s.az));
double gx = s.gx - bgx_;
double gy = s.gy - bgy_;
double gz = s.gz - bgz_;
if (GYRO_IS_DEG_PER_SEC)
{
gx = deg2rad(gx);
gy = deg2rad(gy);
gz = deg2rad(gz);
}
double roll_gyro = roll_ + gx * dt;
double pitch_gyro = pitch_ + gy * dt;
yaw_ = wrapPi(yaw_ + gz * dt);
roll_ = alpha_ * roll_gyro + (1.0 - alpha_) * roll_acc;
pitch_ = alpha_ * pitch_gyro + (1.0 - alpha_) * pitch_acc;- Accel estimates roll/pitch from gravity; gyro integration provides short-term motion for roll/pitch/yaw.
- Yaw cannot be derived from accel, so it relies on gyro integration and will drift.
- Roll/pitch are fused via complementary filter; higher alpha trusts gyro more (smoother but more drift).