1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm
#ifndef IMU_STABILIZATION_HPP
#define IMU_STABILIZATION_HPP
#include "../imuData.hpp"
#include "../status.hpp"
#include "../quaternion.hpp"
#include "../matrix.hpp"
#include "../circularBuffer.hpp"
#include <mutex>
#include <vector>
namespace VideoStitch {
namespace Stab {
class IIRFilter;
enum class GyroBiasStatus { DISABLED = -1, OK = 0, IN_PROGRESS = 1, THRESHOLD_TOO_HIGH = 2, CAMERA_MOVING = 3 };
/*
* The method is inspired by: https://github.com/richardstechnotes/RTIMULib-Arduino
*
* At each timestep, we fuse the current orientation computed using accelerometer only (accQ)
* with the orientation computed by integrating the gyro over time from the previous position
*/
class VS_EXPORT FusionIMU {
public:
FusionIMU();
/**
* @brief call this the first time
* @param acc : accelerometer data, expressed in the IMU coordinate system
* @param timestamp : in microseconds
* @return Quaternion in the IMU coordinate system
*/
Quaternion<double> init(const Vector3<double>& acc, mtime_t timestamp);
/**
* @brief call this all the subsequent times, after init() has been called the first time
* @param gyr : gyroscope data, expressed in the IMU coordinate system
* @param acc : accelerometer data, expressed in the IMU coordinate system
* @param timestamp : in microseconds
* @return Quaternion in the IMU coordinate system
*/
Quaternion<double> update(const Vector3<double>& gyr, const Vector3<double>& acc, mtime_t timestamp);
/**
* @brief Set the fusion factor used to combined measures (acc) and predicted (gyr) orientations
* @param factor: 0: use gyroscope only 1: use accelerometer only
* Any value in between [0; 1] is valid. A value outside this range will trigger a log error
* and won't change the value of the attribute
*/
void setFusionFactor(double factor);
/**
* @brief Returns true if the fusion uses the gyroscope measurements
* This is true only if the fusion factor is below 1.
* A fusion factor of exactly 1 uses accelerometer data only
*/
bool usesGyroscope() const;
/**
* @brief Set the timestamp without changing the other internal variables
* @param timestamp : in microseconds
*/
void setTimestamp(mtime_t timestamp);
private:
bool initialized;
void computeOrientationFromAcc(const Vector3<double>& accelerometer);
Quaternion<double> RPY2Quat(const Vector3<double>& rpy) const;
Vector3<double> Quat2RPY(const Quaternion<double>& q) const;
Vector3<double> fusionRPY; ///< fusion acc + gyr
Quaternion<double> fusionQ;
Vector3<double> accRPY;
Quaternion<double> accQ;
mtime_t lastTimestamp;
double fusionFactor;
};
class VS_EXPORT IMUStabilization {
public:
IMUStabilization();
~IMUStabilization();
void addMeasures(const std::vector<VideoStitch::IMU::Measure>& mes);
Quaternion<double> computeOrientation(mtime_t timestamp);
/**
* @brief computes the horizon leveling using last IMU data
*/
Quaternion<double> computeHorizonLeveling();
void setLowPassIIR(double ratioNyquist);
void setFusionFactor(double fusionFactor);
void enableGyroBiasCorrection();
void disableGyroBiasCorrection();
bool isGyroBiasCorrectionValid() const;
GyroBiasStatus getGyroBiasStatus() const;
private:
IMUStabilization(const IMUStabilization& other) = delete;
IMUStabilization& operator=(const IMUStabilization&) = delete;
/**
* @brief Converts IMU quaternion into VS quaternion
* @param qIMU: input quaternion, expressed in IMU coordinate system
* @return Quaternion expressed in VS coordinate system
*/
Quaternion<double> changeCoordinateSystemIMU2Lib(const Quaternion<double>& qIMU) const;
/**
* @brief computes the orientation in the IMU coordinate system
* @param accelerometer: last available accelerometer data, expressed in IMU coordinate system
* @return Quaternion expressed in IMU coordinate system
*/
Quaternion<double> computeOrientationFromAccelerometer(const Vector3<double>& accelerometer) const;
void resetGyroBias();
std::mutex measuresLock;
CircularBuffer<VideoStitch::IMU::Measure> measures; // raw imu measurements
CircularBuffer<std::pair<mtime_t, Quaternion<double> > > orientations; // timestamped quaternions
std::size_t maxSizeBuffers;
/// Filters for accelerometer and gyroscope data
std::unique_ptr<IIRFilter> iirAx;
std::unique_ptr<IIRFilter> iirAy;
std::unique_ptr<IIRFilter> iirAz;
std::unique_ptr<IIRFilter> iirGx;
std::unique_ptr<IIRFilter> iirGy;
std::unique_ptr<IIRFilter> iirGz;
FusionIMU fusion; ///< computes the fusion between accelerometer and gyroscope data
bool m_enableGyroBiasCorrection;
bool m_gyroBiasIsValid;
int m_gyroBiasNb; ///< number of gathered measurements for gyro bias computation
Vector3<double> m_gyroBias;
Vector3<double> m_previousAcc;
std::vector<Vector3<double> > m_gyroBiasMeasurements; ///< gyroscope values gathered for bias cancellation
double m_threshold_noise_after_bias_correction;
mutable std::mutex m_statusGyroBiasLock;
GyroBiasStatus m_statusGyroBias;
};
} // namespace Stab
} // namespace VideoStitch
#endif