Fix z scaling

Author: hmakelin

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

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)