Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update rs-motion.cpp #1

Merged
merged 1 commit into from
Oct 2, 2020
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions examples/motion/rs-motion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -117,15 +117,17 @@ class rotation_estimator
/* 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.98;
bool first = true;
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 (first) // On the first iteration, use only data from accelerometer to set the camera's initial position
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;
}
Expand Down Expand Up @@ -160,9 +162,9 @@ class rotation_estimator

// 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 (first)
if (firstAccel)
{
first = false;
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;
Expand Down