Merge pull request #16 from phausamann/devel

Release 0.1.2

.bumpversion.cfg

@@ -1,5 +1,5 @@
 [bumpversion]
-current_version = 0.1.1
+current_version = 0.1.2
 commit = True
 
 [bumpversion:file:setup.py]

HISTORY.rst

@@ -2,6 +2,15 @@
 History
 =======
 
+0.1.2 (October 7th, 2020)
+-------------------------
+
+Improvements
+~~~~~~~~~~~~
+
+* Use SQUAD instead of linear interpolation for quaternions.
+
+
 0.1.1 (September 17th, 2020)
 ----------------------------
 

rigid_body_motion/__init__.py

@@ -1,7 +1,7 @@
 """Top-level package for rigid-body-motion."""
 __author__ = """Peter Hausamann"""
 __email__ = "peter.hausamann@tum.de"
-__version__ = "0.1.1"
+__version__ = "0.1.2"
 from pathlib import Path
 
 from pkg_resources import resource_filename

rigid_body_motion/reference_frames.py

@@ -6,6 +6,7 @@
     as_quat_array,
     derivative,
     from_rotation_matrix,
+    squad,
 )
 from scipy.interpolate import interp1d
 from scipy.signal import butter, filtfilt
@@ -244,7 +245,6 @@ def _broadcast(cls, arr, timestamps):
     @classmethod
     def _interpolate(cls, source_arr, target_arr, source_ts, target_ts):
         """ Interpolate source array at target array timestamps. """
-        # TODO SLERP for quaternions
         # TODO specify time_axis as parameter
         # TODO priority=None/<rf_name>
         # TODO method + optional scipy dependency?
@@ -267,7 +267,28 @@ def _interpolate(cls, source_arr, target_arr, source_ts, target_ts):
             target_arr = target_arr[target_ts <= source_ts[-1]]
             target_ts = target_ts[target_ts <= source_ts[-1]]
 
-        source_arr_interp = interp1d(source_ts, source_arr, axis=0)(target_ts)
+        if source_arr.shape[1] == 7:
+            # ugly edge case of t and r stacked
+            source_arr_interp = np.hstack(
+                (
+                    interp1d(source_ts, source_arr[:, :3], axis=0)(target_ts),
+                    as_float_array(
+                        squad(
+                            as_quat_array(source_arr[:, 3:]),
+                            source_ts,
+                            target_ts,
+                        )
+                    ),
+                )
+            )
+        elif source_arr.shape[1] == 4:
+            source_arr_interp = as_float_array(
+                squad(as_quat_array(source_arr), source_ts, target_ts)
+            )
+        else:
+            source_arr_interp = interp1d(source_ts, source_arr, axis=0)(
+                target_ts
+            )
 
         return source_arr_interp, target_arr, target_ts.astype(ts_dtype)
 
@@ -308,11 +329,15 @@ def _match_timestamps_multi(cls, arr_list, ts_list):
             if ts is None:
                 return_arr_list.append(cls._broadcast(arr, return_ts))
             else:
-                return_arr_list.append(
-                    interp1d(ts.astype(float), arr, axis=0)(
+                if arr.shape[1] == 4:
+                    arr_t = as_float_array(
+                        squad(arr, ts.astype(float), return_ts.astype(float))
+                    )
+                else:
+                    arr_t = interp1d(ts.astype(float), arr, axis=0)(
                         return_ts.astype(float)
                     )
-                )
+                return_arr_list.append(arr_t)
 
         return return_arr_list, return_ts
 

setup.py

@@ -34,6 +34,6 @@
     name="rigid-body-motion",
     packages=find_packages(exclude=["tests"]),
     url="https://github.com/phausamann/rigid-body-motion",
-    version="0.1.1",
+    version="0.1.2",
     zip_safe=False,
 )