CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-03 08:29:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

optical_flow: Use correct signs for gyro calculations

Browse Source
This commit is contained in:
sfalexrog
2019-03-01 15:22:13 +03:00
parent f1c614d91a
commit 23204bb561
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
clever/src/optical_flow.cpp
View File

@@ -219,16 +219,16 @@ private:
{
tf2::Quaternion prev_rot, curr_rot;
tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot);
tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr).transform.rotation, curr_rot);
tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot);
geometry_msgs::Vector3Stamped flow;
flow.header.frame_id = frame_id;
flow.header.stamp = curr;
// https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Quaternion_↔_angular_velocities
auto diff = ((curr_rot - prev_rot) * prev_rot.inverse()) * 2.0f;
flow.vector.x = diff.x();
flow.vector.y = diff.y();
flow.vector.z = diff.z();
flow.vector.x = -diff.x();
flow.vector.y = -diff.y();
flow.vector.z = -diff.z();
return flow;
}