From b1cbeb822c1f8dc43e7bbd1b451107ff6da906e6 Mon Sep 17 00:00:00 2001 From: philipqueen Date: Wed, 11 Oct 2023 17:32:53 -0600 Subject: [PATCH 1/3] add rotation functions --- .../rotate_skeleton.py | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py b/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py index 0981442..c855448 100644 --- a/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py +++ b/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py @@ -74,6 +74,12 @@ def calculate_rotation_matrix(vector1,vector2): return rotation_matrix +def create_rotation_matrix_from_rotation_vector(rotation_vector: np.ndarray) -> np.ndarray: + """Take in a rotation vector and return a rotation matrix""" + origin_normal_unit_vector = create_vector(np.array([0, 0, 0]), np.array([0, 0, 1])) + unit_rotation_vector = calculate_unit_vector(rotation_vector) + return calculate_rotation_matrix(unit_rotation_vector, origin_normal_unit_vector) + def rotate_point(point,rotation_matrix): rotated_point = np.dot(rotation_matrix,point) return rotated_point @@ -114,8 +120,24 @@ def rotate_skeleton_to_vector(reference_vector:np.ndarray, vector_to_rotate_to:n return rotated_skeleton_data_array +def rotate_skeleton_with_matrix(rotation_matrix: np.ndarray, original_skeleton_np_array: np.ndarray) -> np.ndarray: + """ + Rotate the entire skeleton with given rotation matrix. + Input: + Rotation matrix: Rotation matrix describing the desired rotation + Original skeleton data: The freemocap data you want to rotate + Output: + rotated_skeleton_data: A numpy data array of your rotated skeleton + """ + rotated_skeleton_data_array = np.zeros(original_skeleton_np_array.shape) + for frame in range(original_skeleton_np_array.shape[0]): + rotated_skeleton_data_array[frame, :, :] = rotate_skeleton_frame( + original_skeleton_np_array[frame, :, :], rotation_matrix + ) + + return rotated_skeleton_data_array def align_skeleton_with_origin(skeleton_data:np.ndarray, skeleton_indices:list, good_frame:int) -> np.ndarray: From 72fd34c117ea382deee549a5f842299466afeb06 Mon Sep 17 00:00:00 2001 From: philipqueen Date: Wed, 11 Oct 2023 17:45:23 -0600 Subject: [PATCH 2/3] fit it into skellyforge architecture --- skellyforge/freemocap_utils/constants.py | 1 + .../parameter_widgets.py | 3 ++- .../rotate_skeleton.py | 26 ------------------- .../task_worker_thread.py | 12 +++++++-- 4 files changed, 13 insertions(+), 29 deletions(-) diff --git a/skellyforge/freemocap_utils/constants.py b/skellyforge/freemocap_utils/constants.py index c20fc4e..4f25dfc 100644 --- a/skellyforge/freemocap_utils/constants.py +++ b/skellyforge/freemocap_utils/constants.py @@ -15,4 +15,5 @@ ROTATE_METHOD_FOOT_SPINE = 'Foot/Spine Rotation' ROTATE_METHOD_X = 'Rotate around X' +ROTATE_METHOD_VECTOR = 'Rotate According to Vector' ROTATE_METHOD_NONE = 'None' \ No newline at end of file diff --git a/skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py b/skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py index 865ac93..3ec5c78 100644 --- a/skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py +++ b/skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py @@ -13,6 +13,7 @@ PARAM_GOOD_FRAME, ROTATE_METHOD_FOOT_SPINE, ROTATE_METHOD_X, + ROTATE_METHOD_VECTOR, ROTATE_METHOD_NONE ) @@ -35,7 +36,7 @@ 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_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}, diff --git a/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py b/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py index c855448..30b219a 100644 --- a/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py +++ b/skellyforge/freemocap_utils/postprocessing_widgets/postprocessing_functions/rotate_skeleton.py @@ -74,12 +74,6 @@ def calculate_rotation_matrix(vector1,vector2): return rotation_matrix -def create_rotation_matrix_from_rotation_vector(rotation_vector: np.ndarray) -> np.ndarray: - """Take in a rotation vector and return a rotation matrix""" - origin_normal_unit_vector = create_vector(np.array([0, 0, 0]), np.array([0, 0, 1])) - unit_rotation_vector = calculate_unit_vector(rotation_vector) - return calculate_rotation_matrix(unit_rotation_vector, origin_normal_unit_vector) - def rotate_point(point,rotation_matrix): rotated_point = np.dot(rotation_matrix,point) return rotated_point @@ -120,26 +114,6 @@ def rotate_skeleton_to_vector(reference_vector:np.ndarray, vector_to_rotate_to:n return rotated_skeleton_data_array -def rotate_skeleton_with_matrix(rotation_matrix: np.ndarray, original_skeleton_np_array: np.ndarray) -> np.ndarray: - """ - Rotate the entire skeleton with given rotation matrix. - - Input: - Rotation matrix: Rotation matrix describing the desired rotation - Original skeleton data: The freemocap data you want to rotate - Output: - rotated_skeleton_data: A numpy data array of your rotated skeleton - - """ - rotated_skeleton_data_array = np.zeros(original_skeleton_np_array.shape) - for frame in range(original_skeleton_np_array.shape[0]): - rotated_skeleton_data_array[frame, :, :] = rotate_skeleton_frame( - original_skeleton_np_array[frame, :, :], rotation_matrix - ) - - return rotated_skeleton_data_array - - def align_skeleton_with_origin(skeleton_data:np.ndarray, skeleton_indices:list, good_frame:int) -> np.ndarray: """ diff --git a/skellyforge/freemocap_utils/postprocessing_widgets/task_worker_thread.py b/skellyforge/freemocap_utils/postprocessing_widgets/task_worker_thread.py index 77b5d0c..742c838 100644 --- a/skellyforge/freemocap_utils/postprocessing_widgets/task_worker_thread.py +++ b/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_rotation_matrix_from_rotation_vector, create_vector, rotate_by_90_degrees_around_x_axis, rotate_skeleton_to_vector, rotate_skeleton_with_matrix from freemocap_utils.postprocessing_widgets.visualization_widgets.mediapipe_skeleton_builder import mediapipe_indices @@ -39,7 +39,8 @@ PARAM_AUTO_FIND_GOOD_FRAME, PARAM_GOOD_FRAME, ROTATE_METHOD_FOOT_SPINE, - ROTATE_METHOD_X + ROTATE_METHOD_X, + ROTATE_METHOD_VECTOR, ) @@ -134,6 +135,13 @@ 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_VECTOR: + origin_normal_unit_vector = create_vector(np.array([0, 0, 0]), np.array([0, 0, 1])) + origin_aligned_skeleton = rotate_skeleton_to_vector( + reference_vector=[], # figure out how to pass parameters to this + vector_to_rotate_to=origin_normal_unit_vector, + original_skeleton_np_array=self.tasks[TASK_FILTERING]['result'] + ) else: origin_aligned_skeleton = None return False, origin_aligned_skeleton From 3fb6ec273f65bfe9091632595a7721b4c4023e03 Mon Sep 17 00:00:00 2001 From: philipqueen Date: Mon, 16 Oct 2023 17:45:21 -0600 Subject: [PATCH 3/3] groundplane vector rotation --- skellyforge/freemocap_utils/config.py | 4 ++- skellyforge/freemocap_utils/constants.py | 3 ++- .../parameter_widgets.py | 27 ++++++++++++++++--- .../task_worker_thread.py | 12 ++++++--- 4 files changed, 37 insertions(+), 9 deletions(-) diff --git a/skellyforge/freemocap_utils/config.py b/skellyforge/freemocap_utils/config.py index 5bf0060..0a03017 100644 --- a/skellyforge/freemocap_utils/config.py +++ b/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 } } diff --git a/skellyforge/freemocap_utils/constants.py b/skellyforge/freemocap_utils/constants.py index 4f25dfc..f877e4b 100644 --- a/skellyforge/freemocap_utils/constants.py +++ b/skellyforge/freemocap_utils/constants.py @@ -12,8 +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_VECTOR = 'Rotate According to Vector' +ROTATE_METHOD_GROUNDPLANE_VECTOR = 'Rotate According to Groundplane Vector' ROTATE_METHOD_NONE = 'None' \ No newline at end of file diff --git a/skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py b/skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py index 3ec5c78..9805c0d 100644 --- a/skellyforge/freemocap_utils/postprocessing_widgets/parameter_widgets.py +++ b/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,7 +15,7 @@ PARAM_GOOD_FRAME, ROTATE_METHOD_FOOT_SPINE, ROTATE_METHOD_X, - ROTATE_METHOD_VECTOR, + ROTATE_METHOD_GROUNDPLANE_VECTOR, ROTATE_METHOD_NONE ) @@ -36,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, ROTATE_METHOD_VECTOR], "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": ""}, ]} ] @@ -71,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) @@ -85,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) diff --git a/skellyforge/freemocap_utils/postprocessing_widgets/task_worker_thread.py b/skellyforge/freemocap_utils/postprocessing_widgets/task_worker_thread.py index 742c838..10dbe59 100644 --- a/skellyforge/freemocap_utils/postprocessing_widgets/task_worker_thread.py +++ b/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, create_rotation_matrix_from_rotation_vector, create_vector, rotate_by_90_degrees_around_x_axis, rotate_skeleton_to_vector, rotate_skeleton_with_matrix +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, @@ -40,7 +42,7 @@ PARAM_GOOD_FRAME, ROTATE_METHOD_FOOT_SPINE, ROTATE_METHOD_X, - ROTATE_METHOD_VECTOR, + ROTATE_METHOD_GROUNDPLANE_VECTOR, ) @@ -135,13 +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_VECTOR: + 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=[], # figure out how to pass parameters to this + 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