92 lines
3.4 KiB
C++
92 lines
3.4 KiB
C++
|
# pragma once
|
||
|
|
|
||
|
|
|
||
|
|
#include <librealsense2/rs.hpp>
|
||
|
|
#include <mutex>
|
||
|
|
|
||
|
|
|
||
|
|
#ifndef PI
|
||
|
|
#define PI 3.14159265358979323846
|
||
|
|
#define PI_FL 3.141592f
|
||
|
|
#endif
|
||
|
|
|
||
|
|
class IMUProcessor
|
||
|
|
{
|
||
|
|
// theta is the angle of camera rotation in x, y and z components
|
||
|
|
float3 theta;
|
||
|
|
std::mutex theta_mtx;
|
||
|
|
/* alpha indicates the part that gyro and accelerometer take in computation of theta; higher alpha gives more weight to gyro, but too high
|
||
|
|
values cause drift; lower alpha gives more weight to accelerometer, which is more sensitive to disturbances */
|
||
|
|
float alpha = 0.9f;
|
||
|
|
bool firstGyro = true;
|
||
|
|
bool firstAccel = true;
|
||
|
|
// Keeps the arrival time of previous gyro frame
|
||
|
|
double last_ts_gyro = 0;
|
||
|
|
public:
|
||
|
|
// Function to calculate the change in angle of motion based on data from gyro
|
||
|
|
void process_gyro(rs2_vector gyro_data, double ts)
|
||
|
|
{
|
||
|
|
if (firstGyro) // On the first iteration, use only data from accelerometer to set the camera's initial position
|
||
|
|
{
|
||
|
|
firstGyro = false;
|
||
|
|
last_ts_gyro = ts;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
// Holds the change in angle, as calculated from gyro
|
||
|
|
float3 gyro_angle;
|
||
|
|
|
||
|
|
// Initialize gyro_angle with data from gyro
|
||
|
|
gyro_angle.x = gyro_data.x; // Pitch
|
||
|
|
gyro_angle.y = gyro_data.y; // Yaw
|
||
|
|
gyro_angle.z = gyro_data.z; // Roll
|
||
|
|
|
||
|
|
// Compute the difference between arrival times of previous and current gyro frames
|
||
|
|
double dt_gyro = (ts - last_ts_gyro) / 1000.0;
|
||
|
|
last_ts_gyro = ts;
|
||
|
|
|
||
|
|
// Change in angle equals gyro measures * time passed since last measurement
|
||
|
|
gyro_angle = gyro_angle * static_cast<float>(dt_gyro);
|
||
|
|
|
||
|
|
// Apply the calculated change of angle to the current angle (theta)
|
||
|
|
std::lock_guard<std::mutex> lock(theta_mtx);
|
||
|
|
theta.add(-gyro_angle.z, -gyro_angle.y, gyro_angle.x);
|
||
|
|
}
|
||
|
|
|
||
|
|
void process_accel(rs2_vector accel_data)
|
||
|
|
{
|
||
|
|
// Holds the angle as calculated from accelerometer data
|
||
|
|
float3 accel_angle;
|
||
|
|
|
||
|
|
// Calculate rotation angle from accelerometer data
|
||
|
|
accel_angle.z = atan2(accel_data.y, accel_data.z);
|
||
|
|
accel_angle.x = atan2(accel_data.x, sqrt(accel_data.y * accel_data.y + accel_data.z * accel_data.z));
|
||
|
|
|
||
|
|
// If it is the first iteration, set initial pose of camera according to accelerometer data (note the different handling for Y axis)
|
||
|
|
std::lock_guard<std::mutex> lock(theta_mtx);
|
||
|
|
if (firstAccel)
|
||
|
|
{
|
||
|
|
firstAccel = false;
|
||
|
|
theta = accel_angle;
|
||
|
|
// Since we can't infer the angle around Y axis using accelerometer data, we'll use PI as a convetion for the initial pose
|
||
|
|
theta.y = PI_FL;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
/*
|
||
|
|
Apply Complementary Filter:
|
||
|
|
- high-pass filter = theta * alpha: allows short-duration signals to pass through while filtering out signals
|
||
|
|
that are steady over time, is used to cancel out drift.
|
||
|
|
- low-pass filter = accel * (1- alpha): lets through long term changes, filtering out short term fluctuations
|
||
|
|
*/
|
||
|
|
theta.x = theta.x * alpha + accel_angle.x * (1 - alpha);
|
||
|
|
theta.z = theta.z * alpha + accel_angle.z * (1 - alpha);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns the current rotation angle
|
||
|
|
float3 get_theta()
|
||
|
|
{
|
||
|
|
std::lock_guard<std::mutex> lock(theta_mtx);
|
||
|
|
return theta;
|
||
|
|
}
|
||
|
|
};
|