Charuco groundplane

skellyforge/freemocap_utils/config.py

@@ -1,4 +1,5 @@
 from skellyforge.freemocap_utils.constants import(
+    PARAM_GROUNDPLANE_VECTOR,
     TASK_INTERPOLATION,
     TASK_FILTERING,
     TASK_FINDING_GOOD_FRAME,
@@ -29,7 +30,8 @@
  TASK_SKELETON_ROTATION:
  {PARAM_ROTATE_DATA: ROTATE_METHOD_X,
   PARAM_AUTO_FIND_GOOD_FRAME: True,
-  PARAM_GOOD_FRAME: None
+  PARAM_GOOD_FRAME: None,
+  PARAM_GROUNDPLANE_VECTOR: None
  }
 }
 

skellyforge/freemocap_utils/constants.py

@@ -12,7 +12,9 @@
 PARAM_ROTATE_DATA = "Data Rotation Method"
 PARAM_AUTO_FIND_GOOD_FRAME = "Auto-find Good Frame"
 PARAM_GOOD_FRAME = "Good Frame"
+PARAM_GROUNDPLANE_VECTOR = "Groundplane Vector"
 
 ROTATE_METHOD_FOOT_SPINE = 'Foot/Spine Rotation'
 ROTATE_METHOD_X = 'Rotate around X'
+ROTATE_METHOD_GROUNDPLANE_VECTOR = 'Rotate According to Groundplane Vector'
 ROTATE_METHOD_NONE = 'None'

skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py

@@ -1,5 +1,7 @@
+import numpy as np
 from pyqtgraph.parametertree import Parameter,registerParameterType
 from skellyforge.freemocap_utils.constants import(
+    PARAM_GROUNDPLANE_VECTOR,
     TASK_INTERPOLATION,
     TASK_FILTERING,
     TASK_FINDING_GOOD_FRAME,
@@ -13,6 +15,7 @@
     PARAM_GOOD_FRAME,
     ROTATE_METHOD_FOOT_SPINE,
     ROTATE_METHOD_X,
+    ROTATE_METHOD_GROUNDPLANE_VECTOR,
     ROTATE_METHOD_NONE
 )
 
@@ -35,10 +38,11 @@
 
 rotation_settings = [
     {"name": TASK_SKELETON_ROTATION.title(), "type": "group", "children": [
-        {"name": PARAM_ROTATE_DATA, "type": "list", "values": [ROTATE_METHOD_NONE, ROTATE_METHOD_X, ROTATE_METHOD_FOOT_SPINE], "value": "None"},
+        {"name": PARAM_ROTATE_DATA, "type": "list", "values": [ROTATE_METHOD_NONE, ROTATE_METHOD_X, ROTATE_METHOD_FOOT_SPINE, ROTATE_METHOD_GROUNDPLANE_VECTOR], "value": "None"},
         {"name": "Instructions", "type": "str", "value": "Uncheck 'Auto-find Good Frame' to type in the good frame manually.", "readonly": True},
         {"name": PARAM_AUTO_FIND_GOOD_FRAME, "type": "bool", "value": True},
         {"name": PARAM_GOOD_FRAME, "type": "str", "value": "", "step": 1},
+        {"name": PARAM_GROUNDPLANE_VECTOR, "type": "str", "value": ""},
     ]}
 ]
 
@@ -70,12 +74,24 @@ def auto_find_good_frame_changed(self, value):
                 self.good_frame_param.setValue("0")
             self.good_frame_param.setOpts(enabled=True, readonly=False)
 
+class GroundplaneVectorRotationParam:
+    def __init__(self, parent):
+        self.parent = parent
+        self.groundplane_vector_param = self.parent.child(TASK_SKELETON_ROTATION.title()).child(PARAM_GROUNDPLANE_VECTOR)
+
+    def enable(self):
+        self.groundplane_vector_param.setOpts(visible=True)
+
+    def disable(self):
+        self.groundplane_vector_param.setOpts(visible=False)
+
 
 class CustomRotationParam(Parameter):
     def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
         self.rotate_data_param = self.child(TASK_SKELETON_ROTATION.title()).child(PARAM_ROTATE_DATA)
         self.foot_spine_rotation = FootSpineRotationParam(self)
+        self.groundplane_vector_rotation = GroundplaneVectorRotationParam(self)
         self.rotate_data_param.sigValueChanged.connect(self.rotate_data_changed)
         # Initial setup
         self.rotate_data_changed(self.rotate_data_param)
@@ -84,11 +100,17 @@ def rotate_data_changed(self, value):
         rotation_mode = value.value()
         if rotation_mode == ROTATE_METHOD_FOOT_SPINE:
             self.foot_spine_rotation.enable()
+            self.groundplane_vector_rotation.disable()
+        elif rotation_mode == ROTATE_METHOD_GROUNDPLANE_VECTOR:
+            self.groundplane_vector_rotation.enable()
+            self.foot_spine_rotation.disable()
         else:
             self.foot_spine_rotation.disable()
+            self.groundplane_vector_rotation.disable()
 
 
-
+def get_array_from_str_parameter(param: str) -> np.ndarray:
+    return np.asarray([float(x) for x in param.split(",")])
 
 
 interpolation_params = Parameter.create(name='interp_params', type='group', children=interpolation_settings)

skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py

@@ -114,10 +114,6 @@ def rotate_skeleton_to_vector(reference_vector:np.ndarray, vector_to_rotate_to:n
     return rotated_skeleton_data_array
 
 
-
-
-
-
 def align_skeleton_with_origin(skeleton_data:np.ndarray, skeleton_indices:list, good_frame:int) -> np.ndarray:
 
     """

skellyforge/freemocap_utils/postprocessing_widgets/task_worker_thread.py

@@ -5,7 +5,7 @@
 from freemocap_utils.postprocessing_widgets.postprocessing_functions.interpolate_data import interpolate_skeleton_data
 from freemocap_utils.postprocessing_widgets.postprocessing_functions.filter_data import filter_skeleton_data
 from freemocap_utils.postprocessing_widgets.postprocessing_functions.good_frame_finder import find_good_frame
-from freemocap_utils.postprocessing_widgets.postprocessing_functions.rotate_skeleton import align_skeleton_with_origin, rotate_by_90_degrees_around_x_axis
+from freemocap_utils.postprocessing_widgets.postprocessing_functions.rotate_skeleton import align_skeleton_with_origin, create_vector, rotate_by_90_degrees_around_x_axis, rotate_skeleton_to_vector
 
 from freemocap_utils.postprocessing_widgets.visualization_widgets.mediapipe_skeleton_builder import mediapipe_indices
 
@@ -15,6 +15,7 @@
 from skellyforge.freemocap_utils.constants import TASK_INTERPOLATION, TASK_FINDING_GOOD_FRAME, TASK_FILTERING, \
     TASK_SKELETON_ROTATION, PARAM_ROTATE_DATA, PARAM_AUTO_FIND_GOOD_FRAME, PARAM_GOOD_FRAME, PARAM_SAMPLING_RATE, \
     PARAM_CUTOFF_FREQUENCY, PARAM_ORDER, PARAM_METHOD
+from skellyforge.freemocap_utils.postprocessing_widgets.parameter_widgets import get_array_from_str_parameter
 from skellyforge.freemocap_utils.postprocessing_widgets.postprocessing_functions.filter_data import filter_skeleton_data
 from skellyforge.freemocap_utils.postprocessing_widgets.postprocessing_functions.good_frame_finder import \
     find_good_frame
@@ -27,6 +28,7 @@
 
 
 from freemocap_utils.constants import (
+    PARAM_GROUNDPLANE_VECTOR,
     TASK_INTERPOLATION,
     TASK_FILTERING,
     TASK_FINDING_GOOD_FRAME,
@@ -39,7 +41,8 @@
     PARAM_AUTO_FIND_GOOD_FRAME,
     PARAM_GOOD_FRAME,
     ROTATE_METHOD_FOOT_SPINE,
-    ROTATE_METHOD_X
+    ROTATE_METHOD_X,
+    ROTATE_METHOD_GROUNDPLANE_VECTOR,
 )
 
 
@@ -134,6 +137,15 @@ def rotate_skeleton_task(self):
         elif rotate_values_dict[PARAM_ROTATE_DATA] == ROTATE_METHOD_X:
             origin_aligned_skeleton = rotate_by_90_degrees_around_x_axis(self.tasks[TASK_FILTERING]['result'])
             return True, origin_aligned_skeleton
+        elif rotate_values_dict[PARAM_ROTATE_DATA] == ROTATE_METHOD_GROUNDPLANE_VECTOR:
+            origin_normal_unit_vector = create_vector(np.array([0, 0, 0]), np.array([0, 0, 1]))
+            reference_vector = get_array_from_str_parameter(self.settings[TASK_SKELETON_ROTATION][PARAM_GROUNDPLANE_VECTOR])
+            origin_aligned_skeleton = rotate_skeleton_to_vector(
+                reference_vector=reference_vector,
+                vector_to_rotate_to=origin_normal_unit_vector,
+                original_skeleton_np_array=self.tasks[TASK_FILTERING]['result']
+            )
+            return True, origin_aligned_skeleton
         else:
             origin_aligned_skeleton = None
             return False, origin_aligned_skeleton