pyfaceau 1.0.9__tar.gz → 1.3.3__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- pyfaceau-1.3.3/LICENSE +47 -0
- pyfaceau-1.3.3/PKG-INFO +84 -0
- pyfaceau-1.3.3/README.md +39 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/alignment/calc_params.py +40 -49
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/alignment/face_aligner.py +222 -52
- pyfaceau-1.3.3/pyfaceau/alignment/paw.py +285 -0
- pyfaceau-1.3.3/pyfaceau/config.py +118 -0
- pyfaceau-1.3.3/pyfaceau/data/__init__.py +19 -0
- pyfaceau-1.3.3/pyfaceau/data/hdf5_dataset.py +823 -0
- pyfaceau-1.3.3/pyfaceau/data/quality_filter.py +277 -0
- pyfaceau-1.3.3/pyfaceau/data/training_data_generator.py +544 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/detectors/__init__.py +8 -4
- pyfaceau-1.3.3/pyfaceau/detectors/retinaface.py +352 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/download_weights.py +3 -3
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/features/histogram_median_tracker.py +14 -26
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/features/pdm.py +55 -9
- pyfaceau-1.3.3/pyfaceau/features/triangulation.py +109 -0
- pyfaceau-1.3.3/pyfaceau/nn/__init__.py +88 -0
- pyfaceau-1.3.3/pyfaceau/nn/au_prediction_inference.py +447 -0
- pyfaceau-1.3.3/pyfaceau/nn/au_prediction_net.py +816 -0
- pyfaceau-1.3.3/pyfaceau/nn/fast_pipeline.py +364 -0
- pyfaceau-1.3.3/pyfaceau/nn/landmark_pose_inference.py +536 -0
- pyfaceau-1.3.3/pyfaceau/nn/landmark_pose_net.py +972 -0
- pyfaceau-1.3.3/pyfaceau/nn/train_au_prediction.py +634 -0
- pyfaceau-1.3.3/pyfaceau/nn/train_landmark_pose.py +819 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/pipeline.py +437 -98
- pyfaceau-1.3.3/pyfaceau/prediction/online_au_correction.py +256 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/processor.py +3 -5
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/utils/cython_extensions/cython_rotation_update.pyx +36 -46
- pyfaceau-1.3.3/pyfaceau.egg-info/PKG-INFO +84 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau.egg-info/SOURCES.txt +16 -8
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau.egg-info/requires.txt +1 -1
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyproject.toml +3 -3
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/setup.py +3 -40
- pyfaceau-1.0.9/COMMERCIAL-LICENSE.md +0 -109
- pyfaceau-1.0.9/LICENSE +0 -40
- pyfaceau-1.0.9/PKG-INFO +0 -478
- pyfaceau-1.0.9/README.md +0 -433
- pyfaceau-1.0.9/pyfaceau/clnf/__init__.py +0 -20
- pyfaceau-1.0.9/pyfaceau/clnf/cen_patch_experts.py +0 -439
- pyfaceau-1.0.9/pyfaceau/clnf/clnf_detector.py +0 -134
- pyfaceau-1.0.9/pyfaceau/clnf/nu_rlms.py +0 -248
- pyfaceau-1.0.9/pyfaceau/clnf/pdm.py +0 -206
- pyfaceau-1.0.9/pyfaceau/features/triangulation.py +0 -64
- pyfaceau-1.0.9/pyfaceau/utils/cython_extensions/cython_histogram_median.c +0 -34924
- pyfaceau-1.0.9/pyfaceau/utils/cython_extensions/cython_rotation_update.c +0 -32038
- pyfaceau-1.0.9/pyfaceau.egg-info/PKG-INFO +0 -478
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/MANIFEST.in +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/__init__.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/alignment/__init__.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/alignment/numba_calcparams_accelerator.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/detectors/extract_mtcnn_weights.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/detectors/openface_mtcnn.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/detectors/pfld.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/detectors/pymtcnn_detector.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/features/__init__.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/parallel_pipeline.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/prediction/__init__.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/prediction/au_predictor.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/prediction/batched_au_predictor.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/prediction/model_parser.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/prediction/running_median.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/prediction/running_median_fallback.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/refinement/__init__.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/refinement/pdm.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/refinement/svr_patch_expert.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/refinement/targeted_refiner.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/utils/__init__.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/utils/cython_extensions/cython_histogram_median.pyx +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau/utils/cython_extensions/setup.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau.egg-info/dependency_links.txt +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau.egg-info/entry_points.txt +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau.egg-info/not-zip-safe +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau.egg-info/top_level.txt +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/pyfaceau_gui.py +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/requirements.txt +0 -0
- {pyfaceau-1.0.9 → pyfaceau-1.3.3}/setup.cfg +0 -0
pyfaceau-1.3.3/LICENSE
ADDED
|
@@ -0,0 +1,47 @@
|
|
|
1
|
+
Creative Commons Attribution-NonCommercial 4.0 International (CC BY-NC 4.0)
|
|
2
|
+
|
|
3
|
+
Copyright (c) 2025 John Wilson IV, MD
|
|
4
|
+
|
|
5
|
+
This work is licensed under the Creative Commons Attribution-NonCommercial 4.0
|
|
6
|
+
International License. To view a copy of this license, visit
|
|
7
|
+
http://creativecommons.org/licenses/by-nc/4.0/ or send a letter to
|
|
8
|
+
Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
|
|
9
|
+
|
|
10
|
+
================================================================================
|
|
11
|
+
|
|
12
|
+
You are free to:
|
|
13
|
+
|
|
14
|
+
* Share — copy and redistribute the material in any medium or format
|
|
15
|
+
* Adapt — remix, transform, and build upon the material
|
|
16
|
+
|
|
17
|
+
The licensor cannot revoke these freedoms as long as you follow the license terms.
|
|
18
|
+
|
|
19
|
+
================================================================================
|
|
20
|
+
|
|
21
|
+
Under the following terms:
|
|
22
|
+
|
|
23
|
+
* Attribution — You must give appropriate credit, provide a link to the
|
|
24
|
+
license, and indicate if changes were made. You may do so in any reasonable
|
|
25
|
+
manner, but not in any way that suggests the licensor endorses you or your use.
|
|
26
|
+
|
|
27
|
+
* NonCommercial — You may not use the material for commercial purposes.
|
|
28
|
+
|
|
29
|
+
* No additional restrictions — You may not apply legal terms or technological
|
|
30
|
+
measures that legally restrict others from doing anything the license permits.
|
|
31
|
+
|
|
32
|
+
================================================================================
|
|
33
|
+
|
|
34
|
+
Notices:
|
|
35
|
+
|
|
36
|
+
You do not have to comply with the license for elements of the material in the
|
|
37
|
+
public domain or where your use is permitted by an applicable exception or
|
|
38
|
+
limitation.
|
|
39
|
+
|
|
40
|
+
No warranties are given. The license may not give you all of the permissions
|
|
41
|
+
necessary for your intended use. For example, other rights such as publicity,
|
|
42
|
+
privacy, or moral rights may limit how you use the material.
|
|
43
|
+
|
|
44
|
+
================================================================================
|
|
45
|
+
|
|
46
|
+
Full legal code available at:
|
|
47
|
+
https://creativecommons.org/licenses/by-nc/4.0/legalcode
|
pyfaceau-1.3.3/PKG-INFO
ADDED
|
@@ -0,0 +1,84 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: pyfaceau
|
|
3
|
+
Version: 1.3.3
|
|
4
|
+
Summary: Pure Python OpenFace 2.2 AU extraction with CLNF landmark refinement
|
|
5
|
+
Home-page: https://github.com/johnwilsoniv/face-analysis
|
|
6
|
+
Author: John Wilson
|
|
7
|
+
Author-email:
|
|
8
|
+
License: CC BY-NC 4.0
|
|
9
|
+
Project-URL: Homepage, https://github.com/johnwilsoniv/pyfaceau
|
|
10
|
+
Project-URL: Documentation, https://github.com/johnwilsoniv/pyfaceau
|
|
11
|
+
Project-URL: Repository, https://github.com/johnwilsoniv/pyfaceau
|
|
12
|
+
Project-URL: Bug Tracker, https://github.com/johnwilsoniv/pyfaceau/issues
|
|
13
|
+
Keywords: facial-action-units,openface,computer-vision,facial-analysis,emotion-recognition
|
|
14
|
+
Classifier: Development Status :: 5 - Production/Stable
|
|
15
|
+
Classifier: Intended Audience :: Science/Research
|
|
16
|
+
Classifier: Topic :: Scientific/Engineering :: Artificial Intelligence
|
|
17
|
+
Classifier: Topic :: Scientific/Engineering :: Image Recognition
|
|
18
|
+
Classifier: License :: Other/Proprietary License
|
|
19
|
+
Classifier: Programming Language :: Python :: 3
|
|
20
|
+
Classifier: Programming Language :: Python :: 3.10
|
|
21
|
+
Classifier: Programming Language :: Python :: 3.11
|
|
22
|
+
Classifier: Programming Language :: Python :: 3.12
|
|
23
|
+
Classifier: Operating System :: OS Independent
|
|
24
|
+
Requires-Python: >=3.10
|
|
25
|
+
Description-Content-Type: text/markdown
|
|
26
|
+
License-File: LICENSE
|
|
27
|
+
Requires-Dist: numpy>=1.20.0
|
|
28
|
+
Requires-Dist: opencv-python>=4.5.0
|
|
29
|
+
Requires-Dist: pandas>=1.3.0
|
|
30
|
+
Requires-Dist: onnxruntime>=1.10.0
|
|
31
|
+
Requires-Dist: scipy>=1.7.0
|
|
32
|
+
Requires-Dist: scikit-learn>=1.0.0
|
|
33
|
+
Requires-Dist: tqdm>=4.62.0
|
|
34
|
+
Requires-Dist: pyfhog>=0.1.0
|
|
35
|
+
Requires-Dist: pyclnf>=0.2.0
|
|
36
|
+
Provides-Extra: dev
|
|
37
|
+
Requires-Dist: pytest>=7.0.0; extra == "dev"
|
|
38
|
+
Requires-Dist: black>=22.0.0; extra == "dev"
|
|
39
|
+
Requires-Dist: flake8>=4.0.0; extra == "dev"
|
|
40
|
+
Provides-Extra: accel
|
|
41
|
+
Requires-Dist: onnxruntime-coreml>=1.10.0; extra == "accel"
|
|
42
|
+
Dynamic: home-page
|
|
43
|
+
Dynamic: license-file
|
|
44
|
+
Dynamic: requires-python
|
|
45
|
+
|
|
46
|
+
# pyfaceau
|
|
47
|
+
|
|
48
|
+
Python implementation of OpenFace 2.2's Facial Action Unit extraction pipeline.
|
|
49
|
+
|
|
50
|
+
## Installation
|
|
51
|
+
|
|
52
|
+
```bash
|
|
53
|
+
pip install pyfaceau
|
|
54
|
+
```
|
|
55
|
+
|
|
56
|
+
## Usage
|
|
57
|
+
|
|
58
|
+
```python
|
|
59
|
+
from pyfaceau import FaceAnalyzer
|
|
60
|
+
|
|
61
|
+
analyzer = FaceAnalyzer()
|
|
62
|
+
result = analyzer.analyze(image)
|
|
63
|
+
|
|
64
|
+
print(result.au_intensities) # 17 action unit intensities
|
|
65
|
+
print(result.landmarks) # 68 facial landmarks
|
|
66
|
+
print(result.pose) # head pose
|
|
67
|
+
```
|
|
68
|
+
|
|
69
|
+
## What it does
|
|
70
|
+
|
|
71
|
+
- Extracts 17 facial action units (AU01, AU02, AU04, AU05, AU06, AU07, AU09, AU10, AU12, AU14, AU15, AU17, AU20, AU23, AU25, AU26, AU45)
|
|
72
|
+
- Detects 68 facial landmarks via [pyclnf](https://github.com/johnwilsoniv/pyclnf)
|
|
73
|
+
- Estimates 3D head pose
|
|
74
|
+
- No C++ compilation required
|
|
75
|
+
|
|
76
|
+
## Citation
|
|
77
|
+
|
|
78
|
+
If you use this in research, please cite:
|
|
79
|
+
|
|
80
|
+
> Wilson IV, J., Rosenberg, J., Gray, M. L., & Razavi, C. R. (2025). A split-face computer vision/machine learning assessment of facial paralysis using facial action units. *Facial Plastic Surgery & Aesthetic Medicine*. https://doi.org/10.1177/26893614251394382
|
|
81
|
+
|
|
82
|
+
## License
|
|
83
|
+
|
|
84
|
+
CC BY-NC 4.0 — free for non-commercial use with attribution.
|
pyfaceau-1.3.3/README.md
ADDED
|
@@ -0,0 +1,39 @@
|
|
|
1
|
+
# pyfaceau
|
|
2
|
+
|
|
3
|
+
Python implementation of OpenFace 2.2's Facial Action Unit extraction pipeline.
|
|
4
|
+
|
|
5
|
+
## Installation
|
|
6
|
+
|
|
7
|
+
```bash
|
|
8
|
+
pip install pyfaceau
|
|
9
|
+
```
|
|
10
|
+
|
|
11
|
+
## Usage
|
|
12
|
+
|
|
13
|
+
```python
|
|
14
|
+
from pyfaceau import FaceAnalyzer
|
|
15
|
+
|
|
16
|
+
analyzer = FaceAnalyzer()
|
|
17
|
+
result = analyzer.analyze(image)
|
|
18
|
+
|
|
19
|
+
print(result.au_intensities) # 17 action unit intensities
|
|
20
|
+
print(result.landmarks) # 68 facial landmarks
|
|
21
|
+
print(result.pose) # head pose
|
|
22
|
+
```
|
|
23
|
+
|
|
24
|
+
## What it does
|
|
25
|
+
|
|
26
|
+
- Extracts 17 facial action units (AU01, AU02, AU04, AU05, AU06, AU07, AU09, AU10, AU12, AU14, AU15, AU17, AU20, AU23, AU25, AU26, AU45)
|
|
27
|
+
- Detects 68 facial landmarks via [pyclnf](https://github.com/johnwilsoniv/pyclnf)
|
|
28
|
+
- Estimates 3D head pose
|
|
29
|
+
- No C++ compilation required
|
|
30
|
+
|
|
31
|
+
## Citation
|
|
32
|
+
|
|
33
|
+
If you use this in research, please cite:
|
|
34
|
+
|
|
35
|
+
> Wilson IV, J., Rosenberg, J., Gray, M. L., & Razavi, C. R. (2025). A split-face computer vision/machine learning assessment of facial paralysis using facial action units. *Facial Plastic Surgery & Aesthetic Medicine*. https://doi.org/10.1177/26893614251394382
|
|
36
|
+
|
|
37
|
+
## License
|
|
38
|
+
|
|
39
|
+
CC BY-NC 4.0 — free for non-commercial use with attribution.
|
|
@@ -17,6 +17,7 @@ Date: 2025-10-29
|
|
|
17
17
|
import numpy as np
|
|
18
18
|
from scipy import linalg
|
|
19
19
|
import cv2
|
|
20
|
+
import os
|
|
20
21
|
|
|
21
22
|
# Try to import Cython-optimized rotation update for 99.9% accuracy
|
|
22
23
|
try:
|
|
@@ -26,10 +27,12 @@ try:
|
|
|
26
27
|
sys.path.insert(0, str(Path(__file__).parent.parent))
|
|
27
28
|
from cython_rotation_update import update_rotation_cython
|
|
28
29
|
CYTHON_AVAILABLE = True
|
|
29
|
-
|
|
30
|
+
if os.environ.get('PYFACEAU_VERBOSE', '0') == '1':
|
|
31
|
+
print("Cython rotation update module loaded - targeting 99.9% accuracy")
|
|
30
32
|
except ImportError:
|
|
31
33
|
CYTHON_AVAILABLE = False
|
|
32
|
-
|
|
34
|
+
if os.environ.get('PYFACEAU_VERBOSE', '0') == '1':
|
|
35
|
+
print("Warning: Cython rotation update not available - using Python (99.45% accuracy)")
|
|
33
36
|
|
|
34
37
|
# Try to import Numba JIT-accelerated CalcParams functions for 2-5x speedup
|
|
35
38
|
try:
|
|
@@ -41,7 +44,6 @@ try:
|
|
|
41
44
|
NUMBA_AVAILABLE = True
|
|
42
45
|
except ImportError:
|
|
43
46
|
NUMBA_AVAILABLE = False
|
|
44
|
-
print("Warning: Numba JIT accelerator not available - using standard Python (slower)")
|
|
45
47
|
|
|
46
48
|
|
|
47
49
|
class CalcParams:
|
|
@@ -97,10 +99,10 @@ class CalcParams:
|
|
|
97
99
|
@staticmethod
|
|
98
100
|
def rotation_matrix_to_euler(R):
|
|
99
101
|
"""
|
|
100
|
-
Convert 3x3 rotation matrix to Euler angles
|
|
102
|
+
Convert 3x3 rotation matrix to Euler angles
|
|
101
103
|
|
|
102
|
-
|
|
103
|
-
Uses
|
|
104
|
+
EXACTLY matches RotationMatrix2Euler() from RotationHelpers.h lines 73-90
|
|
105
|
+
Uses simple quaternion extraction (assumes trace+1 > 0)
|
|
104
106
|
|
|
105
107
|
Args:
|
|
106
108
|
R: 3x3 rotation matrix
|
|
@@ -108,47 +110,34 @@ class CalcParams:
|
|
|
108
110
|
Returns:
|
|
109
111
|
(rx, ry, rz) Euler angles in radians
|
|
110
112
|
"""
|
|
111
|
-
#
|
|
112
|
-
#
|
|
113
|
-
|
|
114
|
-
|
|
115
|
-
|
|
116
|
-
|
|
117
|
-
|
|
118
|
-
|
|
119
|
-
|
|
120
|
-
|
|
121
|
-
|
|
122
|
-
|
|
123
|
-
|
|
124
|
-
s = np.sqrt(1.0 + R[0,0] - R[1,1] - R[2,2]) * 2.0 # s = 4*q1
|
|
125
|
-
q0 = (R[2,1] - R[1,2]) / s
|
|
126
|
-
q1 = 0.25 * s
|
|
127
|
-
q2 = (R[0,1] + R[1,0]) / s
|
|
128
|
-
q3 = (R[0,2] + R[2,0]) / s
|
|
129
|
-
elif R[1,1] > R[2,2]:
|
|
130
|
-
# q2 is largest component
|
|
131
|
-
s = np.sqrt(1.0 + R[1,1] - R[0,0] - R[2,2]) * 2.0 # s = 4*q2
|
|
132
|
-
q0 = (R[0,2] - R[2,0]) / s
|
|
133
|
-
q1 = (R[0,1] + R[1,0]) / s
|
|
134
|
-
q2 = 0.25 * s
|
|
135
|
-
q3 = (R[1,2] + R[2,1]) / s
|
|
136
|
-
else:
|
|
137
|
-
# q3 is largest component
|
|
138
|
-
s = np.sqrt(1.0 + R[2,2] - R[0,0] - R[1,1]) * 2.0 # s = 4*q3
|
|
139
|
-
q0 = (R[1,0] - R[0,1]) / s
|
|
140
|
-
q1 = (R[0,2] + R[2,0]) / s
|
|
141
|
-
q2 = (R[1,2] + R[2,1]) / s
|
|
142
|
-
q3 = 0.25 * s
|
|
143
|
-
|
|
144
|
-
# Quaternion to Euler angles
|
|
113
|
+
# EXACT C++ implementation from RotationHelpers.h
|
|
114
|
+
# float q0 = sqrt(1 + rotation_matrix(0, 0) + rotation_matrix(1, 1) + rotation_matrix(2, 2)) / 2.0f;
|
|
115
|
+
q0 = np.sqrt(1.0 + R[0,0] + R[1,1] + R[2,2]) / 2.0
|
|
116
|
+
|
|
117
|
+
# float q1 = (rotation_matrix(2, 1) - rotation_matrix(1, 2)) / (4.0f*q0);
|
|
118
|
+
q1 = (R[2,1] - R[1,2]) / (4.0 * q0)
|
|
119
|
+
# float q2 = (rotation_matrix(0, 2) - rotation_matrix(2, 0)) / (4.0f*q0);
|
|
120
|
+
q2 = (R[0,2] - R[2,0]) / (4.0 * q0)
|
|
121
|
+
# float q3 = (rotation_matrix(1, 0) - rotation_matrix(0, 1)) / (4.0f*q0);
|
|
122
|
+
q3 = (R[1,0] - R[0,1]) / (4.0 * q0)
|
|
123
|
+
|
|
124
|
+
# Quaternion to Euler angles (exactly as in C++)
|
|
125
|
+
# float t1 = 2.0f * (q0*q2 + q1*q3);
|
|
145
126
|
t1 = 2.0 * (q0*q2 + q1*q3)
|
|
146
|
-
t1 =
|
|
127
|
+
# if (t1 > 1) t1 = 1.0f; if (t1 < -1) t1 = -1.0f;
|
|
128
|
+
if t1 > 1.0:
|
|
129
|
+
t1 = 1.0
|
|
130
|
+
if t1 < -1.0:
|
|
131
|
+
t1 = -1.0
|
|
147
132
|
|
|
133
|
+
# float yaw = asin(t1);
|
|
148
134
|
yaw = np.arcsin(t1)
|
|
135
|
+
# float pitch = atan2(2.0f * (q0*q1 - q2*q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
|
|
149
136
|
pitch = np.arctan2(2.0 * (q0*q1 - q2*q3), q0*q0 - q1*q1 - q2*q2 + q3*q3)
|
|
137
|
+
# float roll = atan2(2.0f * (q0*q3 - q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3);
|
|
150
138
|
roll = np.arctan2(2.0 * (q0*q3 - q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3)
|
|
151
139
|
|
|
140
|
+
# return cv::Vec3f(pitch, yaw, roll);
|
|
152
141
|
return np.array([pitch, yaw, roll], dtype=np.float32)
|
|
153
142
|
|
|
154
143
|
@staticmethod
|
|
@@ -381,15 +370,15 @@ class CalcParams:
|
|
|
381
370
|
euler_new = update_rotation_cython(euler_current, delta_rotation)
|
|
382
371
|
updated_global[1:4] = euler_new
|
|
383
372
|
else:
|
|
384
|
-
# Fallback to Python implementation
|
|
373
|
+
# Fallback to Python implementation matching C++ EXACTLY
|
|
385
374
|
# Get current rotation matrix
|
|
386
375
|
euler_current = params_global[1:4]
|
|
387
376
|
R1 = self.euler_to_rotation_matrix(euler_current)
|
|
388
377
|
|
|
389
378
|
# Construct incremental rotation matrix R'
|
|
390
|
-
#
|
|
391
|
-
#
|
|
392
|
-
#
|
|
379
|
+
# R2(1,2) = -1.0*(R2(2,1) = delta_p.at<float>(1,0)); // wx
|
|
380
|
+
# R2(2,0) = -1.0*(R2(0,2) = delta_p.at<float>(2,0)); // wy
|
|
381
|
+
# R2(0,1) = -1.0*(R2(1,0) = delta_p.at<float>(3,0)); // wz
|
|
393
382
|
R2 = np.eye(3, dtype=np.float32)
|
|
394
383
|
R2[1, 2] = -delta_p[1] # -wx
|
|
395
384
|
R2[2, 1] = delta_p[1] # wx
|
|
@@ -404,10 +393,12 @@ class CalcParams:
|
|
|
404
393
|
# Combine rotations
|
|
405
394
|
R3 = R1 @ R2
|
|
406
395
|
|
|
407
|
-
#
|
|
408
|
-
#
|
|
409
|
-
#
|
|
410
|
-
|
|
396
|
+
# C++ uses: RotationMatrix2AxisAngle -> AxisAngle2Euler
|
|
397
|
+
# cv::Vec3f axis_angle = Utilities::RotationMatrix2AxisAngle(R3);
|
|
398
|
+
# cv::Vec3f euler = Utilities::AxisAngle2Euler(axis_angle);
|
|
399
|
+
# This is: Rodrigues(R3) -> Rodrigues(axis_angle) -> RotationMatrix2Euler
|
|
400
|
+
axis_angle = self.rotation_matrix_to_axis_angle(R3)
|
|
401
|
+
euler_new = self.axis_angle_to_euler(axis_angle)
|
|
411
402
|
|
|
412
403
|
# Handle numerical instability
|
|
413
404
|
if np.any(np.isnan(euler_new)):
|