Add: velocity to return in the local frame

docs/framing.md

@@ -16,3 +16,89 @@ $\text{transform}(\text{current\_camera\_frame}, \text{return\_all\_frames}=\tex
 ## APIs and implementation :
 
 [APIs](frame_handler.md#t265.FrameHandler.FrameHandler.transform)
+
+------
+## Acceleration frame conversion
+
+
+Given:
+
+- \( \mathbf{T}_{current} \): The current transformation matrix of the camera frame, a \(4 \times 4\) matrix.
+- \( \mathbf{a} \): Linear acceleration in the initial frame, represented as a \(3 \times 1\) vector.
+- \( \boldsymbol{\alpha} \): Angular acceleration in the initial frame, represented as a \(3 \times 1\) vector.
+- \( \mathbf{T}_{trans} \): The transformation matrix to the desired frame, a \(4 \times 4\) matrix.
+
+We want to find the transformed linear acceleration \( \mathbf{a}' \) and angular acceleration \( \boldsymbol{\alpha}' \) in the desired frame.
+
+1. **Extract the rotation matrices and translation vectors:**
+
+   - \( \mathbf{T}_{current} \):
+
+     \[
+     \mathbf{T}_{current} = \begin{bmatrix}
+     \mathbf{R}_{current} & \mathbf{t}_{current} \\
+     \mathbf{0}_{1 \times 3} & 1
+     \end{bmatrix}
+     \]
+
+     where \( \mathbf{R}_{current} \) is the \(3 \times 3\) rotation matrix and \( \mathbf{t}_{current} \) is the \(3 \times 1\) translation vector.
+
+   - \( \mathbf{T}_{trans} \):
+
+     \[
+     \mathbf{T}_{trans} = \begin{bmatrix}
+     \mathbf{R}_{trans} & \mathbf{t}_{trans} \\
+     \mathbf{0}_{1 \times 3} & 1
+     \end{bmatrix}
+     \]
+
+     where \( \mathbf{R}_{trans} \) is the \(3 \times 3\) rotation matrix and \( \mathbf{t}_{trans} \) is the \(3 \times 1\) translation vector.
+
+2. **Compute the inverse transformation matrix of the initial frame \( \mathbf{T}_{init}^{-1} \):**
+
+   \[
+   \mathbf{T}_{init}^{-1} = \begin{bmatrix}
+   \mathbf{R}_{init}^{T} & -\mathbf{R}_{init}^{T} \mathbf{t}_{init} \\
+   \mathbf{0}_{1 \times 3} & 1
+   \end{bmatrix}
+   \]
+
+   Here, \( \mathbf{R}_{init}^{T} \) is the transpose of the initial rotation matrix, and \( -\mathbf{R}_{init}^{T} \mathbf{t}_{init} \) is the transformed translation vector.
+
+3. **Transform the linear and angular accelerations:**
+
+   \[
+   \mathbf{a}_{ck} = \mathbf{R}_{current}^{T} \mathbf{a}
+   \]
+
+   \[
+   \boldsymbol{\alpha}_{ck} = \mathbf{R}_{current}^{T} \boldsymbol{\alpha}
+   \]
+
+4. **Compute the final rotation matrix \( \mathbf{R}_{final} \):**
+
+   \[
+   \mathbf{R}_{final} = \mathbf{R}_{trans} \mathbf{R}_{current}
+   \]
+
+5. **Transform the linear acceleration in the desired frame:**
+
+   \[
+   \mathbf{a}' = \mathbf{R}_{final} \mathbf{a}_{ck}
+   \]
+
+6. **Transform the angular acceleration in the desired frame:**
+
+   \[
+   \boldsymbol{\alpha}' = \mathbf{R}_{final} \boldsymbol{\alpha}_{ck}
+   \]
+
+Thus, the transformed linear and angular accelerations in the desired frame can be represented as:
+
+\[
+\mathbf{a}' = \mathbf{R}_{trans} \mathbf{R}_{current}^{T} \mathbf{a}
+\]
+
+\[
+\boldsymbol{\alpha}' = \mathbf{R}_{trans} \mathbf{R}_{current}^{T} \boldsymbol{\alpha}
+\]

requirements.txt

@@ -1,2 +1,4 @@
+pyrealsense2<2.54.1
 pyrealsense2<2.54
 numpy
+scipy

t265/FrameHandler.py

@@ -109,23 +109,27 @@ def _convert_rotation(self, rot, rot_format='quat', from_degrees=False, format='
 
         return ret
 
-    def convert(self, current_camera_frame, T_mat=True, angular=None, linear=None):
+    def convert(self, current_camera_frame, T_mat=True, angular=None, linear=None, local_frame=False):
         """
         Apply the frame transformation to the given frame, resulting in a transformed frame and, optionally, angular and linear velocities.
 
         Args:
             current_camera_frame (np.array): 4x4 transformation matrix of the current camera frame.
             T_mat (bool, optional): If True, the resulting transformed frame matrix is included in the return tuple. Defaults to True.
-            angular (bool, optional): If True, the transformed angular velocity is included in the return tuple. Defaults to True.
-            linear (bool, optional): If True, the transformed linear velocity is included in the return tuple. Defaults to True.
+            angular (np, optional): If not None, the transformed angular velocity is included in the return tuple. Defaults to True.
+            linear (np, optional): If not None, the transformed linear velocity is included in the return tuple. Defaults to True.
+            local_frame (bool, optional): If True, the velocity is w.r.t. the current frame
 
         Returns:
             tuple: A tuple containing the transformed frame, angular velocity, and linear velocity as requested.
                    The order of the returned items is as follows: transformed frame, angular velocity, linear velocity.
         """
         T = self.transform(current_camera_frame, return_all_frames=False)
         if angular is not None and linear is not None:
-            vel = self.transform_velocity(current_camera_frame, angular, linear)
+            if local_frame:
+                vel = self.transform_velocity_to_current_pose(current_camera_frame, angular, linear)
+            else:
+                vel = self.transform_velocity(current_camera_frame, angular, linear)
 
         if T_mat and angular is not None and linear is not None:
             return T, vel
@@ -210,3 +214,41 @@ def transform_velocity(self, current_camera_frame, angular, linear):
 
         # Return the transformed velocity vector, combining the transformed angular and linear velocities
         return np.append(ros0_v_ros, ros0_w_ros)
+
+    def transform_velocity_to_current_pose(self, current_camera_frame, angular, linear):
+        # Compute the transformation matrix from initial to current pose frame
+        T = self.transform(current_camera_frame, return_all_frames=False)
+
+        # Extract the rotation matrix and translation vector
+        current_R = T[:3, :3]
+        current_t = T[:3, 3]
+
+        # Transform the angular velocity
+        transformed_angular = current_R @ angular
+        # Transform the linear velocity
+        transformed_linear = current_R @ linear + np.cross(transformed_angular, current_t)
+
+        return np.append(transformed_angular, transformed_linear)
+
+    def transform_acceleration(self, current_camera_frame, angular_acc, linear_acc):
+        """
+        Transforms acceleration components from the current camera frame to the target frame.
+
+        Args:
+            current_camera_frame (np.array): 4x4 transformation matrix of the current camera frame.
+            angular_acc (np.array): Angular acceleration vector [awx, awy, awz] in the initial frame.
+            linear_acc (np.array): Linear acceleration vector [ax, ay, az] in the initial frame.
+
+        Returns:
+            np.array: The transformed acceleration vector [ax', ay', az', awx', awy', awz'] in the target frame.
+        """
+        T, ros0_T_rosk, ros0_T_ck, c0_T_ck, c0_T_E, E_T_ck, ros_T_c, c_T_ros = self.transform(current_camera_frame,
+                                                                                              return_all_frames=True)
+        c_p_ros = self.T_trans[0:3, 3].flatten()
+        ros0_R_rosk = ros0_T_rosk[0:3, 0:3]
+        ck_R_E = E_T_ck[0:3, 0:3].transpose()
+        ck_alpha_ck = ck_R_E @ angular_acc
+        ck_a_ck = ck_R_E @ linear_acc
+        ros0_alpha_ros = (ros0_R_rosk @ ck_alpha_ck).reshape(3, )
+        ros0_a_ros = (ros0_R_rosk @ ck_a_ck).reshape(3, )
+        return np.append(ros0_a_ros, ros0_alpha_ros)

t265/Tracking.py

@@ -8,12 +8,9 @@
 DEFAULT_RETRY_TIME = 500
 DEFAULT_TIMEOUT = 100
 
-DEFAULT_FRAMES = \
-    {'ros':
-         dict(trans=None, rot=[0, 90, 90], rot_format='xyz', degrees=True, T=None),
-    }
-
-
+DEFAULT_FRAMES = {
+    'ros': dict(trans=None, rot=[0, 90, 90], rot_format='xyz', degrees=True, T=None),
+}
 
 class Tracking:
     def __init__(self, camera_sn=None, connection_retry=DEFAULT_CONNECTION_RETRY, retry_time=DEFAULT_RETRY_TIME,
@@ -219,10 +216,14 @@ def _to_nparray(self, t, q, trans, rotation, degrees):
             ret_list.append(self._quat2other(q, format=rotation, degrees=degrees))
         return self._to_single_nparray(ret_list)
 
-    def get_velocity(self, frame=None):
+    def get_velocity(self, frame=None, local_frame=False):
         """
         Get the velocity of the camera.
 
+        Args:
+            frame (str): frame to report the velocity
+            local_frame (bool): return the velocity with respect to the current frame instead of the origin frame at t=0.
+
         Returns:
             np.array: [vx, vy, vz, wx, wy, wz]
         """
@@ -231,7 +232,7 @@ def get_velocity(self, frame=None):
             ang_vel = self._vector2np(self.pose.get_pose_data().angular_velocity)
             if frame is not None:
                 T = self.get_matrix()
-                all_vel = self._convert_frame(frame, T, T_mat=False, angular=ang_vel, linear=vel)
+                all_vel = self._convert_frame(frame, T, T_mat=False, angular=ang_vel, linear=vel, local_frame=local_frame)
             else:
                 all_vel = np.append(vel, ang_vel)
 
@@ -249,7 +250,9 @@ def get_acceleration(self, frame=None) -> np.ndarray:
             ang_acc = self._vector2np(self.pose.get_pose_data().angular_acceleration)
 
             if frame is not None:
-                raise NotImplementedError('Acceleration conversion is not implemented yet')
+                T = self.get_matrix()
+                acc_transformed = self._convert_frame_acc(frame, T, linear_acc=acc, angular_acc=ang_acc)
+                return acc_transformed
             return np.append(acc, ang_acc)
 
     def get_matrix(self, frame=None) -> np.ndarray:
@@ -272,19 +275,6 @@ def get_matrix(self, frame=None) -> np.ndarray:
 
             if frame is not None:
                 T = self._convert_frame(frame, T, T_mat=True, angular=None, linear=None)
-
-            # if frame == 'ros':
-            #     rot = R.from_euler('xyz', [0, 90, 90], degrees=True).as_matrix()
-            #     c_T_ros = np.eye(4)
-            #     c_T_ros[0:3, 0:3] = rot
-            #
-            #     E_T_ck = T
-            #     c0_T_E = self.init_T.transpose()
-            #     c0_T_ck = c0_T_E @ E_T_ck
-            #     ros_T_c = c_T_ros.transpose()
-            #     ros0_T_ck = ros_T_c @ c0_T_ck
-            #     ros0_T_rosk = ros0_T_ck @ c_T_ros
-            #     T = ros0_T_rosk
             return T
 
     def _quat2other(self, quat, order: str = 'xyzw', format='matrix', degrees=False) -> np.ndarray:
@@ -355,7 +345,7 @@ def _init_frames(self) -> None:
         for name, frame in self.frames.items():
             frame.set_init_frame(T=T)
 
-    def _convert_frame(self, name, T, T_mat=True, angular=None, linear=None) -> FrameHandler:
+    def _convert_frame(self, name, T, T_mat=True, angular=None, linear=None, local_frame=False) -> FrameHandler:
         """
         Convert the frame to the given frame.
 
@@ -369,9 +359,14 @@ def _convert_frame(self, name, T, T_mat=True, angular=None, linear=None) -> Fram
         frame = self.frames.get(name)
         if frame is None:
             raise ValueError(f'Frame {frame} does not exist')
-        return frame.convert(current_camera_frame=T, T_mat=T_mat, angular=angular, linear=linear)
+        return frame.convert(current_camera_frame=T, T_mat=T_mat, angular=angular, linear=linear, local_frame=local_frame)
+
+    def _convert_frame_acc(self, name, T, angular_acc, linear_acc) -> FrameHandler:
+        frame = self.frames.get(name)
+        if frame is None:
+            raise ValueError(f'Frame {frame} does not exist')
+        return frame.transform_acceleration(current_camera_frame=T, angular_acc=angular_acc, linear_acc=linear_acc)
 
-    # Status functions
     def is_camera_on(self) -> bool:
         """
         Check if the camera is on.