From fc0fc441e66e87a9ac8c393435fedf78a4599339 Mon Sep 17 00:00:00 2001 From: Harri Makelin Date: Fri, 10 May 2024 13:20:59 +0100 Subject: [PATCH] Fix z scaling --- gisnav/gisnav/_transformations.py | 8 +++----- gisnav/gisnav/core/pose_node.py | 3 ++- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/gisnav/gisnav/_transformations.py b/gisnav/gisnav/_transformations.py index 7ce003b3..53c20fd1 100644 --- a/gisnav/gisnav/_transformations.py +++ b/gisnav/gisnav/_transformations.py @@ -443,12 +443,10 @@ def poses_to_twist( pose2.pose.pose.orientation.z, pose2.pose.pose.orientation.w, ] - r1 = tf_transformations.quaternion_matrix(q1) - r2 = tf_transformations.quaternion_matrix(q2) - r_diff = r2 * r1.T - # TODO do not convert to matrices above, just use quaternions - q_diff = tf_transformations.quaternion_from_matrix(r_diff) + q_diff = tf_transformations.quaternion_multiply( + q2, tf_transformations.quaternion_inverse(q1) + ) # Converting quaternion to rotation vector (axis-angle) angle = 2 * np.arccos(q_diff[3]) # Compute the rotation angle diff --git a/gisnav/gisnav/core/pose_node.py b/gisnav/gisnav/core/pose_node.py index dd4c4257..47b79225 100644 --- a/gisnav/gisnav/core/pose_node.py +++ b/gisnav/gisnav/core/pose_node.py @@ -336,9 +336,10 @@ def camera_optical_twist_in_camera_optical_frame( x, y, z = ( scaling * 320.0, scaling * 180.0, - scaling * 205.0, + scaling * -205.0, ) # todo do not hard code previous_pose = tf_.create_identity_pose_stamped(x, y, z) + previous_pose.header = msg.reference.header current_pose = self._get_pose(msg) if current_pose is not None: return tf_.poses_to_twist(current_pose, previous_pose)