Framing (#10)

* Fix: #6
* Add: Documentation on the how frame translation is happening
v0.1.10

.github/workflows/pypi.yml

@@ -0,0 +1,24 @@
+name: Upload Python Package
+
+on:
+  release:
+    types: [created]
+
+jobs:
+  deploy:
+    runs-on: ubuntu-20.04
+
+    steps:
+    - uses: actions/checkout@v2
+    - uses: actions/setup-python@v2
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install setuptools wheel twine
+    - name: Build and publish
+      env:
+        TWINE_USERNAME: __token__
+        TWINE_PASSWORD: ${{ secrets.PYPI_PASSWORD }}
+      run: |
+        python setup.py sdist bdist_wheel
+        twine upload dist/*

docs/framing.md

@@ -0,0 +1,18 @@
+# Frame Translation
+$\text{transform}(\text{current\_camera\_frame}, \text{return\_all\_frames}=\text{False})$
+
+## Transformation Matrix definition:
+1. $T_{\text{c}}^{\text{ros}}$ — Transformation matrix from the camera frame to the ROS frame. (A constant)
+2. $T_{\text{E}}^{\text{c}_k}$ — Transformation matrix of the current camera frame.
+3. $T_{\text{E}}^{\text{c}_0}$ = Transformation matrix of the initial camera frame with respect to the Earth frame.
+
+
+## Transformation Calculations:
+1. $T_{\text{c}_k}^{\text{c}_0} = T^{\text{E}}_{\text{c}_0} \cdot T_{\text{E}}^{\text{c}_k}$ — Transformation matrix from the initial frame to the current camera frame.
+2. $T^{\text{c}_k}_{\text{ros}_0} = T^{\text{c}}_{\text{ros}} \cdot T^{\text{c}_k}_{\text{c}_0}$ — Combined transformation matrix from the ROS frame to the current camera frame.
+3. $T^{\text{ros}_k}_{\text{ros}_0} = T^{\text{c}_k}_{\text{ros}_0} \cdot T_{\text{c}}^{\text{ros}}$ — Final transformation matrix from the ROS frame to the transformed current frame.
+
+
+## APIs and implementation :
+
+[APIs](frame_handler.md#t265.FrameHandler.FrameHandler.transform)

docs/tracking.md

@@ -1,5 +1,30 @@
 # Tracking class APIs
 
+## T265 Frame
+### T265 Body frame 
+The default frame T265 returns.
+
+Z pointing the opposite of the camera.
+
+            Y
+            |   / Z
+            |  /
+            | /
+     X______|/ 
+
+### ROS frame 
+The default frame T265 returns if you use ros package.
+
+Y pointing out of the camera. VR convention.
+
+        Z
+        |
+        |
+        |______ X
+       /
+      /
+     Y
+
 
 ## APIs
 ::: t265.Tracking.Tracking

mkdocs.yml

@@ -8,7 +8,7 @@ theme:
   features:
     - navigation.tabs
     - navigation.sections
-    - toc.integrate
+#    - toc.integrate
     - navigation.top
     - navigation.footer
     - navigation.instant
@@ -98,8 +98,15 @@ plugins:
       handlers:
         python:
           options:
-            show_source: false
+            show_source: true
+            show_root_heading: true
+            show_root_full_path: false
+            show_root_members_full_path: false
+            show_if_no_docstring: false
+            inherited_members: true
             members_order: source
+            heading_level: 2
+            docstring_section_style: table
 
 #  - git-authors
 #  - table-reader
@@ -120,6 +127,7 @@ extra_javascript:
 
 nav:
     - index.md
+    - framing.md
     - APIs:
       - tracking.md
       - frame_handler.md

setup.py

@@ -15,7 +15,7 @@ def read(rel_path):
 
 setup(
     name='t265',
-    version='0.1.8',
+    version='0.1.10',
     description='t265 Tracking camera API wrapper',
     author='Yusuke Tanaka',
     license='LGPLv3',

t265/FrameHandler.py

@@ -13,7 +13,6 @@ def __init__(self, frame_name):
         self.frame_name = frame_name
         self.T_init = np.eye(4)
         self.T_trans = np.eye(4)
-        pass
 
     def set_frame_transformation(self, trans=None, rot=None, rot_format='quat', degrees=False, T=None):
         """

t265/Tracking.py

@@ -66,7 +66,9 @@ def add_custom_frames(self, name, trans=None, rot=None, rot_format='quat', degre
         """
         if self.camera_on:
             raise RuntimeError('Cannot add custom frames after the camera is started')
-        self.frames[name] = FrameHandler(name).set_frame_transformation(trans, rot, rot_format, degrees, T)
+        fh = FrameHandler(name)
+        fh.set_frame_transformation(trans, rot, rot_format, degrees, T)
+        self.frames[name] = fh
 
     def start_tracking(self):
         """
@@ -153,8 +155,8 @@ def get_translation(self, frame=None, trans=True, rotation='quat', degrees=False
         Args:
             frame (str): The frame of the translation. If frame is None, return the translation of the default frame.
             trans (bool): If True, returns the translation.
-            rotation (str): Rotation format. 'quat', or Euler 'xyz', 'xzy', 'yxz', 'yzx', 'zxy', 'zyx', 'ZYX', 'ZXY',
-                            'YXZ', 'YZX', 'XZY', 'XYZ'.
+            rotation (str): Rotation format. 'quat', or Euler extrinsic: 'xyz', 'xzy', 'yxz', 'yzx', 'zxy', 'zyx', and
+                intrinsic moving frame: 'ZYX', 'ZXY', 'YXZ', 'YZX', 'XZY', 'XYZ'.
             degrees (bool): If True, the Euler angles are returned in degrees. ignored if rotation is 'quat'.
 
         Returns:
@@ -163,7 +165,7 @@ def get_translation(self, frame=None, trans=True, rotation='quat', degrees=False
         """
         ret = None
         if frame is None:
-            ret = self._get_translation(trans, rotation)
+            ret = self._get_translation(trans, rotation, degrees)
         else:
             T = self.get_matrix(frame)
             if T is not None:
@@ -235,7 +237,7 @@ def get_velocity(self, frame=None):
 
             return all_vel
 
-    def get_acceleration(self, frame=None):
+    def get_acceleration(self, frame=None) -> np.ndarray:
         """
         Get the acceleration of the camera.
 
@@ -250,7 +252,7 @@ def get_acceleration(self, frame=None):
                 raise NotImplementedError('Acceleration conversion is not implemented yet')
             return np.append(acc, ang_acc)
 
-    def get_matrix(self, frame=None):
+    def get_matrix(self, frame=None) -> np.ndarray:
         """
         Get the transformation matrix of the camera in a specific frame.
 
@@ -285,7 +287,7 @@ def get_matrix(self, frame=None):
             #     T = ros0_T_rosk
             return T
 
-    def _quat2other(self, quat, order: str = 'xyzw', format='matrix', degrees=False):
+    def _quat2other(self, quat, order: str = 'xyzw', format='matrix', degrees=False) -> np.ndarray:
         """
         Convert quaternion to other format.
 
@@ -320,7 +322,7 @@ def _vector2np(self, vector):
         """
         return np.array([vector.x, vector.y, vector.z])
 
-    def _quat2np(self, quat, order: str = 'xyzw'):
+    def _quat2np(self, quat, order: str = 'xyzw') -> np.ndarray:
         """
         Convert rs.quaternion to np.array.
 
@@ -336,7 +338,7 @@ def _quat2np(self, quat, order: str = 'xyzw'):
         else:
             return np.array([quat.x, quat.y, quat.z, quat.w])
 
-    def _set_defualt_frames(self):
+    def _set_defualt_frames(self) -> None:
         """
         Set the default frames.
         """
@@ -345,15 +347,15 @@ def _set_defualt_frames(self):
             f.set_frame_transformation(**frame)
             self.frames[name] = f
 
-    def _init_frames(self):
+    def _init_frames(self) -> None:
         """
         Initialize the frames.
         """
         T = self.get_matrix()
         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):
+    def _convert_frame(self, name, T, T_mat=True, angular=None, linear=None) -> FrameHandler:
         """
         Convert the frame to the given frame.
 
@@ -370,7 +372,7 @@ def _convert_frame(self, name, T, T_mat=True, angular=None, linear=None):
         return frame.convert(current_camera_frame=T, T_mat=T_mat, angular=angular, linear=linear)
 
     # Status functions
-    def is_camera_on(self):
+    def is_camera_on(self) -> bool:
         """
         Check if the camera is on.
 
@@ -379,7 +381,7 @@ def is_camera_on(self):
         """
         return self.camera_on
 
-    def get_camera_sn(self):
+    def get_camera_sn(self) -> str:
         """
         Get the camera serial number.
 

t265/unittest/full_test.py

@@ -1,20 +1,32 @@
 from t265 import Tracking
 import time
+import numpy as np
 
 if __name__ == "__main__":
     """
     Example of using the Tracking class
     prints the pose, velocity and acceleration every second in ros frame
     """
     track = Tracking()
+
+    # define a custom frane
+    c_T_b = np.linalg.inv(np.array([
+        [1, 0, 0, 0],
+        [0, 0, 1, 0.1515],
+        [0, -1, 0, 0.038],
+        [0, 0, 0, 1]]))
+    # Add the custom frame
+    track.add_custom_frames('body', T=c_T_b)
     while True:
         track.update_pose(wait=True)
-        pos = track.get_translation('ros')
+        pos = track.get_translation()      # get all 6d pose
         print(f"Camera_sn: {track.get_camera_sn()}")
         print(f'pos: {pos[0:3]}')
-        print(f"T: {track.get_matrix('ros')}")
-        print(f"euler {track.get_translation('ros', trans=False, rotation='zyx', degrees=True)}")
-        print("Velocity: {}".format(track.get_velocity('ros')))
-        print("Acceleration: {}\n".format(track.get_acceleration()))
-        time.sleep(1)
+        print(f"T: {track.get_matrix('ros')}")  # get matrix in ros frame
+        # Get the translation in the body frame
+        print(f"euler {track.get_translation('body', trans=False, rotation='XYZ', degrees=True)}")
+        print(f"default {track.get_translation(trans=False, rotation='XYZ', degrees=True)}")
 
+        # print("Velocity: {}".format(track.get_velocity('ros')))
+        # print("Acceleration: {}\n".format(track.get_acceleration()))
+        time.sleep(1)