PnPHeadPose.py

import numpy as np
import cv2
import mediapipe as mp


class PnPHeadPose:
    def __init__(self):
        self.canonical_metric_landmarks = np.array(
            [0.000000, -3.406404, 5.979507, 0.499977, 0.652534, 0.000000, -1.126865, 7.475604, 0.500026, 0.547487,
             0.000000,
             -2.089024, 6.058267, 0.499974, 0.602372, -0.463928, 0.955357, 6.633583, 0.482113, 0.471979, 0.000000,
             -0.463170,
             7.586580, 0.500151, 0.527156, 0.000000, 0.365669, 7.242870, 0.499910, 0.498253, 0.000000, 2.473255,
             5.788627,
             0.499523, 0.401062, -4.253081, 2.577646, 3.279702, 0.289712, 0.380764, 0.000000, 4.019042, 5.284764,
             0.499955,
             0.312398, 0.000000, 4.885979, 5.385258, 0.499987, 0.269919, 0.000000, 8.261778, 4.481535, 0.500023,
             0.107050,
             0.000000, -3.706811, 5.864924, 0.500023, 0.666234, 0.000000, -3.918301, 5.569430, 0.500016, 0.679224,
             0.000000,
             -3.994436, 5.219482, 0.500023, 0.692348, 0.000000, -4.542400, 5.404754, 0.499977, 0.695278, 0.000000,
             -4.745577,
             5.529457, 0.499977, 0.705934, 0.000000, -5.019567, 5.601448, 0.499977, 0.719385, 0.000000, -5.365123,
             5.535441,
             0.499977, 0.737019, 0.000000, -6.149624, 5.071372, 0.499968, 0.781371, 0.000000, -1.501095, 7.112196,
             0.499816,
             0.562981, -0.416106, -1.466449, 6.447657, 0.473773, 0.573910, -7.087960, 5.434801, 0.099620, 0.104907,
             0.254141,
             -2.628639, 2.035898, 3.848121, 0.365930, 0.409576, -3.198363, 1.985815, 3.796952, 0.338758, 0.413025,
             -3.775151,
             2.039402, 3.646194, 0.311120, 0.409460, -4.465819, 2.422950, 3.155168, 0.274658, 0.389131, -2.164289,
             2.189867,
             3.851822, 0.393362, 0.403706, -3.208229, 3.223926, 4.115822, 0.345234, 0.344011, -2.673803, 3.205337,
             4.092203,
             0.370094, 0.346076, -3.745193, 3.165286, 3.972409, 0.319322, 0.347265, -4.161018, 3.059069, 3.719554,
             0.297903,
             0.353591, -5.062006, 1.934418, 2.776093, 0.247792, 0.410810, -2.266659, -7.425768, 4.389812, 0.396889,
             0.842755,
             -4.445859, 2.663991, 3.173422, 0.280098, 0.375600, -7.214530, 2.263009, 0.073150, 0.106310, 0.399956,
             -5.799793,
             2.349546, 2.204059, 0.209925, 0.391353, -2.844939, -0.720868, 4.433130, 0.355808, 0.534406, -0.711452,
             -3.329355,
             5.877044, 0.471751, 0.650404, -0.606033, -3.924562, 5.444923, 0.474155, 0.680192, -1.431615, -3.500953,
             5.496189,
             0.439785, 0.657229, -1.914910, -3.803146, 5.028930, 0.414617, 0.666541, -1.131043, -3.973937, 5.189648,
             0.450374,
             0.680861, -1.563548, -4.082763, 4.842263, 0.428771, 0.682691, -2.650112, -5.003649, 4.188483, 0.374971,
             0.727805,
             -0.427049, -1.094134, 7.360529, 0.486717, 0.547629, -0.496396, -0.475659, 7.440358, 0.485301, 0.527395,
             -5.253307,
             3.881582, 3.363159, 0.257765, 0.314490, -1.718698, 0.974609, 4.558359, 0.401223, 0.455172, -1.608635,
             -0.942516,
             5.814193, 0.429819, 0.548615, -1.651267, -0.610868, 5.581319, 0.421352, 0.533741, -4.765501, -0.701554,
             3.534632,
             0.276896, 0.532057, -0.478306, 0.295766, 7.101013, 0.483370, 0.499587, -3.734964, 4.508230, 4.550454,
             0.337212,
             0.282883, -4.588603, 4.302037, 4.048484, 0.296392, 0.293243, -6.279331, 6.615427, 1.425850, 0.169295,
             0.193814,
             -1.220941, 4.142165, 5.106035, 0.447580, 0.302610, -2.193489, 3.100317, 4.000575, 0.392390, 0.353888,
             -3.102642,
             -4.352984, 4.095905, 0.354490, 0.696784, -6.719682, -4.788645, -1.745401, 0.067305, 0.730105, -1.193824,
             -1.306795,
             5.737747, 0.442739, 0.572826, -0.729766, -1.593712, 5.833208, 0.457098, 0.584792, -2.456206, -4.342621,
             4.283884,
             0.381974, 0.694711, -2.204823, -4.304508, 4.162499, 0.392389, 0.694203, -4.985894, 4.802461, 3.751977,
             0.277076,
             0.271932, -1.592294, -1.257709, 5.456949, 0.422552, 0.563233, -2.644548, 4.524654, 4.921559, 0.385919,
             0.281364,
             -2.760292, 5.100971, 5.015990, 0.383103, 0.255840, -3.523964, 8.005976, 3.729163, 0.331431, 0.119714,
             -5.599763,
             5.715470, 2.724259, 0.229924, 0.232003, -3.063932, 6.566144, 4.529981, 0.364501, 0.189114, -5.720968,
             4.254584,
             2.830852, 0.229622, 0.299541, -6.374393, 4.785590, 1.591691, 0.173287, 0.278748, -0.672728, -3.688016,
             5.737804,
             0.472879, 0.666198, -1.262560, -3.787691, 5.417779, 0.446828, 0.668527, -1.732553, -3.952767, 5.000579,
             0.422762,
             0.673890, -1.043625, -1.464973, 5.662455, 0.445308, 0.580066, -2.321234, -4.329069, 4.258156, 0.388103,
             0.693961,
             -2.056846, -4.477671, 4.520883, 0.403039, 0.706540, -2.153084, -4.276322, 4.038093, 0.403629, 0.693953,
             -0.946874,
             -1.035249, 6.512274, 0.460042, 0.557139, -1.469132, -4.036351, 4.604908, 0.431158, 0.692366, -1.024340,
             -3.989851,
             4.926693, 0.452182, 0.692366, -0.533422, -3.993222, 5.138202, 0.475387, 0.692366, -0.769720, -6.095394,
             4.985883,
             0.465828, 0.779190, -0.699606, -5.291850, 5.448304, 0.472329, 0.736226, -0.669687, -4.949770, 5.509612,
             0.473087,
             0.717857, -0.630947, -4.695101, 5.449371, 0.473122, 0.704626, -0.583218, -4.517982, 5.339869, 0.473033,
             0.695278,
             -1.537170, -4.423206, 4.745470, 0.427942, 0.695278, -1.615600, -4.475942, 4.813632, 0.426479, 0.703540,
             -1.729053,
             -4.618680, 4.854463, 0.423162, 0.711846, -1.838624, -4.828746, 4.823737, 0.418309, 0.720063, -2.368250,
             -3.106237,
             4.868096, 0.390095, 0.639573, -7.542244, -1.049282, -2.431321, 0.013954, 0.560034, 0.000000, -1.724003,
             6.601390,
             0.499914, 0.580147, -1.826614, -4.399531, 4.399021, 0.413200, 0.695400, -1.929558, -4.411831, 4.497052,
             0.409626,
             0.701823, -0.597442, -2.013686, 5.866456, 0.468080, 0.601535, -1.405627, -1.714196, 5.241087, 0.422729,
             0.585985,
             -0.662449, -1.819321, 5.863759, 0.463080, 0.593784, -2.342340, 0.572222, 4.294303, 0.372120, 0.473414,
             -3.327324,
             0.104863, 4.113860, 0.334562, 0.496073, -1.726175, -0.919165, 5.273355, 0.411671, 0.546965, -5.133204,
             7.485602,
             2.660442, 0.242176, 0.147676, -4.538641, 6.319907, 3.683424, 0.290777, 0.201446, -3.986562, 5.109487,
             4.466315,
             0.327338, 0.256527, -2.169681, -5.440433, 4.455874, 0.399510, 0.748921, -1.395634, 5.011963, 5.316032,
             0.441728,
             0.261676, -1.619500, 6.599217, 4.921106, 0.429765, 0.187834, -1.891399, 8.236377, 4.274997, 0.412198,
             0.108901,
             -4.195832, 2.235205, 3.375099, 0.288955, 0.398952, -5.733342, 1.411738, 2.431726, 0.218937, 0.435411,
             -1.859887,
             2.355757, 3.843181, 0.412782, 0.398970, -4.988612, 3.074654, 3.083858, 0.257135, 0.355440, -1.303263,
             1.416453,
             4.831091, 0.427685, 0.437961, -1.305757, -0.672779, 6.415959, 0.448340, 0.536936, -6.465170, 0.937119,
             1.689873,
             0.178560, 0.457554, -5.258659, 0.945811, 2.974312, 0.247308, 0.457194, -4.432338, 0.722096, 3.522615,
             0.286267,
             0.467675, -3.300681, 0.861641, 3.872784, 0.332828, 0.460712, -2.430178, 1.131492, 4.039035, 0.368756,
             0.447207,
             -1.820731, 1.467954, 4.224124, 0.398964, 0.432655, -0.563221, 2.307693, 5.566789, 0.476410, 0.405806,
             -6.338145,
             -0.529279, 1.881175, 0.189241, 0.523924, -5.587698, 3.208071, 2.687839, 0.228962, 0.348951, -0.242624,
             -1.462857,
             7.071491, 0.490726, 0.562401, -1.611251, 0.339326, 4.895421, 0.404670, 0.485133, -7.743095, 2.364999,
             -2.005167,
             0.019469, 0.401564, -1.391142, 1.851048, 4.448999, 0.426243, 0.420431, -1.785794, -0.978284, 4.850470,
             0.396993,
             0.548797, -4.670959, 2.664461, 3.084075, 0.266470, 0.376977, -1.333970, -0.283761, 6.097047, 0.439121,
             0.518958,
             -7.270895, -2.890917, -2.252455, 0.032314, 0.644357, -1.856432, 2.585245, 3.757904, 0.419054, 0.387155,
             -0.923388,
             0.073076, 6.671944, 0.462783, 0.505747, -5.000589, -6.135128, 1.892523, 0.238979, 0.779745, -5.085276,
             -7.178590,
             0.714711, 0.198221, 0.831938, -7.159291, -0.811820, -0.072044, 0.107550, 0.540755, -5.843051, -5.248023,
             0.924091,
             0.183610, 0.740257, -6.847258, 3.662916, 0.724695, 0.134410, 0.333683, -2.412942, -8.258853, 4.119213,
             0.385764,
             0.883154, -0.179909, -1.689864, 6.573301, 0.490967, 0.579378, -2.103655, -0.163946, 4.566119, 0.382385,
             0.508573,
             -6.407571, 2.236021, 1.560843, 0.174399, 0.397671, -3.670075, 2.360153, 3.635230, 0.318785, 0.396235,
             -3.177186,
             2.294265, 3.775704, 0.343364, 0.400597, -2.196121, -4.598322, 4.479786, 0.396100, 0.710217, -6.234883,
             -1.944430,
             1.663542, 0.187885, 0.588538, -1.292924, -9.295920, 4.094063, 0.430987, 0.944065, -3.210651, -8.533278,
             2.802001,
             0.318993, 0.898285, -4.068926, -7.993109, 1.925119, 0.266248, 0.869701, 0.000000, 6.545390, 5.027311,
             0.500023,
             0.190576, 0.000000, -9.403378, 4.264492, 0.499977, 0.954453, -2.724032, 2.315802, 3.777151, 0.366170,
             0.398822,
             -2.288460, 2.398891, 3.697603, 0.393207, 0.395537, -1.998311, 2.496547, 3.689148, 0.410373, 0.391080,
             -6.130040,
             3.399261, 2.038516, 0.194993, 0.342102, -2.288460, 2.886504, 3.775031, 0.388665, 0.362284, -2.724032,
             2.961810,
             3.871767, 0.365962, 0.355971, -3.177186, 2.964136, 3.876973, 0.343364, 0.355357, -3.670075, 2.927714,
             3.724325,
             0.318785, 0.358340, -4.018389, 2.857357, 3.482983, 0.301415, 0.363156, -7.555811, 4.106811, -0.991917,
             0.058133,
             0.319076, -4.018389, 2.483695, 3.440898, 0.301415, 0.387449, 0.000000, -2.521945, 5.932265, 0.499988,
             0.618434,
             -1.776217, -2.683946, 5.213116, 0.415838, 0.624196, -1.222237, -1.182444, 5.952465, 0.445682, 0.566077,
             -0.731493,
             -2.536683, 5.815343, 0.465844, 0.620641, 0.000000, 3.271027, 5.236015, 0.499923, 0.351524, -4.135272,
             -6.996638,
             2.671970, 0.288719, 0.819946, -3.311811, -7.660815, 3.382963, 0.335279, 0.852820, -1.313701, -8.639995,
             4.702456,
             0.440512, 0.902419, -5.940524, -6.223629, -0.631468, 0.128294, 0.791941, -1.998311, 2.743838, 3.744030,
             0.408772,
             0.373894, -0.901447, 1.236992, 5.754256, 0.455607, 0.451801, 0.000000, -8.765243, 4.891441, 0.499877,
             0.908990,
             -2.308977, -8.974196, 3.609070, 0.375437, 0.924192, -6.954154, -2.439843, -0.131163, 0.114210, 0.615022,
             -1.098819,
             -4.458788, 5.120727, 0.448662, 0.695278, -1.181124, -4.579996, 5.189564, 0.448020, 0.704632, -1.255818,
             -4.787901,
             5.237051, 0.447112, 0.715808, -1.325085, -5.106507, 5.205010, 0.444832, 0.730794, -1.546388, -5.819392,
             4.757893,
             0.430012, 0.766809, -1.953754, -4.183892, 4.431713, 0.406787, 0.685673, -2.117802, -4.137093, 4.555096,
             0.400738,
             0.681069, -2.285339, -4.051196, 4.582438, 0.392400, 0.677703, -2.850160, -3.665720, 4.484994, 0.367856,
             0.663919,
             -5.278538, -2.238942, 2.861224, 0.247923, 0.601333, -0.946709, 1.907628, 5.196779, 0.452770, 0.420850,
             -1.314173,
             3.104912, 4.231404, 0.436392, 0.359887, -1.780000, 2.860000, 3.881555, 0.416164, 0.368714, -1.845110,
             -4.098880,
             4.247264, 0.413386, 0.692366, -5.436187, -4.030482, 2.109852, 0.228018, 0.683572, -0.766444, 3.182131,
             4.861453,
             0.468268, 0.352671, -1.938616, -6.614410, 4.521085, 0.411362, 0.804327, 0.000000, 1.059413, 6.774605,
             0.499989,
             0.469825, -0.516573, 1.583572, 6.148363, 0.479154, 0.442654, 0.000000, 1.728369, 6.316750, 0.499974,
             0.439637,
             -1.246815, 0.230297, 5.681036, 0.432112, 0.493589, 0.000000, -7.942194, 5.181173, 0.499886, 0.866917,
             0.000000,
             -6.991499, 5.153478, 0.499913, 0.821729, -0.997827, -6.930921, 4.979576, 0.456549, 0.819201, -3.288807,
             -5.382514,
             3.795752, 0.344549, 0.745439, -2.311631, -1.566237, 4.590085, 0.378909, 0.574010, -2.680250, -6.111567,
             4.096152,
             0.374293, 0.780185, -3.832928, -1.537326, 4.137731, 0.319688, 0.570738, -2.961860, -2.274215, 4.440943,
             0.357155,
             0.604270, -4.386901, -2.683286, 3.643886, 0.295284, 0.621581, -1.217295, -7.834465, 4.969286, 0.447750,
             0.862477,
             -1.542374, -0.136843, 5.201008, 0.410986, 0.508723, -3.878377, -6.041764, 3.311079, 0.313951, 0.775308,
             -3.084037,
             -6.809842, 3.814195, 0.354128, 0.812553, -3.747321, -4.503545, 3.726453, 0.324548, 0.703993, -6.094129,
             -3.205991,
             1.473482, 0.189096, 0.646300, -4.588995, -4.728726, 2.983221, 0.279777, 0.714658, -6.583231, -3.941269,
             0.070268,
             0.133823, 0.682701, -3.492580, -3.195820, 4.130198, 0.336768, 0.644733, -1.255543, 0.802341, 5.307551,
             0.429884,
             0.466522, -1.126122, -0.933602, 6.538785, 0.455528, 0.548623, -1.443109, -1.142774, 5.905127, 0.437114,
             0.558896,
             -0.923043, -0.529042, 7.003423, 0.467288, 0.529925, -1.755386, 3.529117, 4.327696, 0.414712, 0.335220,
             -2.632589,
             3.713828, 4.364629, 0.377046, 0.322778, -3.388062, 3.721976, 4.309028, 0.344108, 0.320151, -4.075766,
             3.675413,
             4.076063, 0.312876, 0.322332, -4.622910, 3.474691, 3.646321, 0.283526, 0.333190, -5.171755, 2.535753,
             2.670867,
             0.241246, 0.382786, -7.297331, 0.763172, -0.048769, 0.102986, 0.468763, -4.706828, 1.651000, 3.109532,
             0.267612,
             0.424560, -4.071712, 1.476821, 3.476944, 0.297879, 0.433176, -3.269817, 1.470659, 3.731945, 0.333434,
             0.433878,
             -2.527572, 1.617311, 3.865444, 0.366427, 0.426116, -1.970894, 1.858505, 3.961782, 0.396012, 0.416696,
             -1.579543,
             2.097941, 4.084996, 0.420121, 0.410228, -7.664182, 0.673132, -2.435867, 0.007561, 0.480777, -1.397041,
             -1.340139,
             5.630378, 0.432949, 0.569518, -0.884838, 0.658740, 6.233232, 0.458639, 0.479089, -0.767097, -0.968035,
             7.077932,
             0.473466, 0.545744, -0.460213, -1.334106, 6.787447, 0.476088, 0.563830, -0.748618, -1.067994, 6.798303,
             0.468472,
             0.555057, -1.236408, -1.585568, 5.480490, 0.433991, 0.582362, -0.387306, -1.409990, 6.957705, 0.483518,
             0.562984,
             -0.319925, -1.607931, 6.508676, 0.482483, 0.577849, -1.639633, 2.556298, 3.863736, 0.426450, 0.389799,
             -1.255645,
             2.467144, 4.203800, 0.438999, 0.396495, -1.031362, 2.382663, 4.615849, 0.450067, 0.400434, -4.253081,
             2.772296,
             3.315305, 0.289712, 0.368253, -4.530000, 2.910000, 3.339685, 0.276670, 0.363373, 0.463928, 0.955357,
             6.633583,
             0.517862, 0.471948, 4.253081, 2.577646, 3.279702, 0.710288, 0.380764, 0.416106, -1.466449, 6.447657,
             0.526227,
             0.573910, 7.087960, 5.434801, 0.099620, 0.895093, 0.254141, 2.628639, 2.035898, 3.848121, 0.634070,
             0.409576,
             3.198363, 1.985815, 3.796952, 0.661242, 0.413025, 3.775151, 2.039402, 3.646194, 0.688880, 0.409460,
             4.465819,
             2.422950, 3.155168, 0.725342, 0.389131, 2.164289, 2.189867, 3.851822, 0.606630, 0.403705, 3.208229,
             3.223926,
             4.115822, 0.654766, 0.344011, 2.673803, 3.205337, 4.092203, 0.629906, 0.346076, 3.745193, 3.165286,
             3.972409,
             0.680678, 0.347265, 4.161018, 3.059069, 3.719554, 0.702097, 0.353591, 5.062006, 1.934418, 2.776093,
             0.752212,
             0.410805, 2.266659, -7.425768, 4.389812, 0.602918, 0.842863, 4.445859, 2.663991, 3.173422, 0.719902,
             0.375600,
             7.214530, 2.263009, 0.073150, 0.893693, 0.399960, 5.799793, 2.349546, 2.204059, 0.790082, 0.391354,
             2.844939,
             -0.720868, 4.433130, 0.643998, 0.534488, 0.711452, -3.329355, 5.877044, 0.528249, 0.650404, 0.606033,
             -3.924562,
             5.444923, 0.525850, 0.680191, 1.431615, -3.500953, 5.496189, 0.560215, 0.657229, 1.914910, -3.803146,
             5.028930,
             0.585384, 0.666541, 1.131043, -3.973937, 5.189648, 0.549626, 0.680861, 1.563548, -4.082763, 4.842263,
             0.571228,
             0.682692, 2.650112, -5.003649, 4.188483, 0.624852, 0.728099, 0.427049, -1.094134, 7.360529, 0.513050,
             0.547282,
             0.496396, -0.475659, 7.440358, 0.515097, 0.527252, 5.253307, 3.881582, 3.363159, 0.742247, 0.314507,
             1.718698,
             0.974609, 4.558359, 0.598631, 0.454979, 1.608635, -0.942516, 5.814193, 0.570338, 0.548575, 1.651267,
             -0.610868,
             5.581319, 0.578632, 0.533623, 4.765501, -0.701554, 3.534632, 0.723087, 0.532054, 0.478306, 0.295766,
             7.101013,
             0.516446, 0.499639, 3.734964, 4.508230, 4.550454, 0.662801, 0.282918, 4.588603, 4.302037, 4.048484,
             0.703624,
             0.293271, 6.279331, 6.615427, 1.425850, 0.830705, 0.193814, 1.220941, 4.142165, 5.106035, 0.552386,
             0.302568,
             2.193489, 3.100317, 4.000575, 0.607610, 0.353888, 3.102642, -4.352984, 4.095905, 0.645429, 0.696707,
             6.719682,
             -4.788645, -1.745401, 0.932695, 0.730105, 1.193824, -1.306795, 5.737747, 0.557261, 0.572826, 0.729766,
             -1.593712,
             5.833208, 0.542902, 0.584792, 2.456206, -4.342621, 4.283884, 0.618026, 0.694711, 2.204823, -4.304508,
             4.162499,
             0.607591, 0.694203, 4.985894, 4.802461, 3.751977, 0.722943, 0.271963, 1.592294, -1.257709, 5.456949,
             0.577414,
             0.563167, 2.644548, 4.524654, 4.921559, 0.614083, 0.281387, 2.760292, 5.100971, 5.015990, 0.616907,
             0.255886,
             3.523964, 8.005976, 3.729163, 0.668509, 0.119914, 5.599763, 5.715470, 2.724259, 0.770092, 0.232021,
             3.063932,
             6.566144, 4.529981, 0.635536, 0.189249, 5.720968, 4.254584, 2.830852, 0.770391, 0.299556, 6.374393,
             4.785590,
             1.591691, 0.826722, 0.278755, 0.672728, -3.688016, 5.737804, 0.527121, 0.666198, 1.262560, -3.787691,
             5.417779,
             0.553172, 0.668527, 1.732553, -3.952767, 5.000579, 0.577238, 0.673890, 1.043625, -1.464973, 5.662455,
             0.554692,
             0.580066, 2.321234, -4.329069, 4.258156, 0.611897, 0.693961, 2.056846, -4.477671, 4.520883, 0.596961,
             0.706540,
             2.153084, -4.276322, 4.038093, 0.596371, 0.693953, 0.946874, -1.035249, 6.512274, 0.539958, 0.557139,
             1.469132,
             -4.036351, 4.604908, 0.568842, 0.692366, 1.024340, -3.989851, 4.926693, 0.547818, 0.692366, 0.533422,
             -3.993222,
             5.138202, 0.524613, 0.692366, 0.769720, -6.095394, 4.985883, 0.534090, 0.779141, 0.699606, -5.291850,
             5.448304,
             0.527671, 0.736226, 0.669687, -4.949770, 5.509612, 0.526913, 0.717857, 0.630947, -4.695101, 5.449371,
             0.526878,
             0.704626, 0.583218, -4.517982, 5.339869, 0.526967, 0.695278, 1.537170, -4.423206, 4.745470, 0.572058,
             0.695278,
             1.615600, -4.475942, 4.813632, 0.573521, 0.703540, 1.729053, -4.618680, 4.854463, 0.576838, 0.711846,
             1.838624,
             -4.828746, 4.823737, 0.581691, 0.720063, 2.368250, -3.106237, 4.868096, 0.609945, 0.639910, 7.542244,
             -1.049282,
             -2.431321, 0.986046, 0.560034, 1.826614, -4.399531, 4.399021, 0.586800, 0.695400, 1.929558, -4.411831,
             4.497052,
             0.590372, 0.701823, 0.597442, -2.013686, 5.866456, 0.531915, 0.601537, 1.405627, -1.714196, 5.241087,
             0.577268,
             0.585935, 0.662449, -1.819321, 5.863759, 0.536915, 0.593786, 2.342340, 0.572222, 4.294303, 0.627543,
             0.473352,
             3.327324, 0.104863, 4.113860, 0.665586, 0.495951, 1.726175, -0.919165, 5.273355, 0.588354, 0.546862,
             5.133204,
             7.485602, 2.660442, 0.757824, 0.147676, 4.538641, 6.319907, 3.683424, 0.709250, 0.201508, 3.986562,
             5.109487,
             4.466315, 0.672684, 0.256581, 2.169681, -5.440433, 4.455874, 0.600409, 0.749005, 1.395634, 5.011963,
             5.316032,
             0.558266, 0.261672, 1.619500, 6.599217, 4.921106, 0.570304, 0.187871, 1.891399, 8.236377, 4.274997,
             0.588166,
             0.109044, 4.195832, 2.235205, 3.375099, 0.711045, 0.398952, 5.733342, 1.411738, 2.431726, 0.781070,
             0.435405,
             1.859887, 2.355757, 3.843181, 0.587247, 0.398932, 4.988612, 3.074654, 3.083858, 0.742870, 0.355446,
             1.303263,
             1.416453, 4.831091, 0.572156, 0.437652, 1.305757, -0.672779, 6.415959, 0.551868, 0.536570, 6.465170,
             0.937119,
             1.689873, 0.821442, 0.457556, 5.258659, 0.945811, 2.974312, 0.752702, 0.457182, 4.432338, 0.722096,
             3.522615,
             0.713757, 0.467627, 3.300681, 0.861641, 3.872784, 0.667113, 0.460673, 2.430178, 1.131492, 4.039035,
             0.631101,
             0.447154, 1.820731, 1.467954, 4.224124, 0.600862, 0.432473, 0.563221, 2.307693, 5.566789, 0.523481,
             0.405627,
             6.338145, -0.529279, 1.881175, 0.810748, 0.523926, 5.587698, 3.208071, 2.687839, 0.771046, 0.348959,
             0.242624,
             -1.462857, 7.071491, 0.509127, 0.562718, 1.611251, 0.339326, 4.895421, 0.595293, 0.485024, 7.743095,
             2.364999,
             -2.005167, 0.980531, 0.401564, 1.391142, 1.851048, 4.448999, 0.573500, 0.420000, 1.785794, -0.978284,
             4.850470,
             0.602995, 0.548688, 4.670959, 2.664461, 3.084075, 0.733530, 0.376977, 1.333970, -0.283761, 6.097047,
             0.560611,
             0.519017, 7.270895, -2.890917, -2.252455, 0.967686, 0.644357, 1.856432, 2.585245, 3.757904, 0.580985,
             0.387160,
             0.923388, 0.073076, 6.671944, 0.537728, 0.505385, 5.000589, -6.135128, 1.892523, 0.760966, 0.779753,
             5.085276,
             -7.178590, 0.714711, 0.801779, 0.831938, 7.159291, -0.811820, -0.072044, 0.892441, 0.540761, 5.843051,
             -5.248023,
             0.924091, 0.816351, 0.740260, 6.847258, 3.662916, 0.724695, 0.865595, 0.333687, 2.412942, -8.258853,
             4.119213,
             0.614074, 0.883246, 0.179909, -1.689864, 6.573301, 0.508953, 0.579438, 2.103655, -0.163946, 4.566119,
             0.617942,
             0.508316, 6.407571, 2.236021, 1.560843, 0.825608, 0.397675, 3.670075, 2.360153, 3.635230, 0.681215,
             0.396235,
             3.177186, 2.294265, 3.775704, 0.656636, 0.400597, 2.196121, -4.598322, 4.479786, 0.603900, 0.710217,
             6.234883,
             -1.944430, 1.663542, 0.812086, 0.588539, 1.292924, -9.295920, 4.094063, 0.568013, 0.944565, 3.210651,
             -8.533278,
             2.802001, 0.681008, 0.898285, 4.068926, -7.993109, 1.925119, 0.733752, 0.869701, 2.724032, 2.315802,
             3.777151,
             0.633830, 0.398822, 2.288460, 2.398891, 3.697603, 0.606793, 0.395537, 1.998311, 2.496547, 3.689148,
             0.589660,
             0.391062, 6.130040, 3.399261, 2.038516, 0.805016, 0.342108, 2.288460, 2.886504, 3.775031, 0.611335,
             0.362284,
             2.724032, 2.961810, 3.871767, 0.634038, 0.355971, 3.177186, 2.964136, 3.876973, 0.656636, 0.355357,
             3.670075,
             2.927714, 3.724325, 0.681215, 0.358340, 4.018389, 2.857357, 3.482983, 0.698585, 0.363156, 7.555811,
             4.106811,
             -0.991917, 0.941867, 0.319076, 4.018389, 2.483695, 3.440898, 0.698585, 0.387449, 1.776217, -2.683946,
             5.213116,
             0.584177, 0.624107, 1.222237, -1.182444, 5.952465, 0.554318, 0.566077, 0.731493, -2.536683, 5.815343,
             0.534154,
             0.620640, 4.135272, -6.996638, 2.671970, 0.711218, 0.819975, 3.311811, -7.660815, 3.382963, 0.664630,
             0.852871,
             1.313701, -8.639995, 4.702456, 0.559100, 0.902632, 5.940524, -6.223629, -0.631468, 0.871706, 0.791941,
             1.998311,
             2.743838, 3.744030, 0.591234, 0.373894, 0.901447, 1.236992, 5.754256, 0.544341, 0.451584, 2.308977,
             -8.974196,
             3.609070, 0.624563, 0.924192, 6.954154, -2.439843, -0.131163, 0.885770, 0.615029, 1.098819, -4.458788,
             5.120727,
             0.551338, 0.695278, 1.181124, -4.579996, 5.189564, 0.551980, 0.704632, 1.255818, -4.787901, 5.237051,
             0.552888,
             0.715808, 1.325085, -5.106507, 5.205010, 0.555168, 0.730794, 1.546388, -5.819392, 4.757893, 0.569944,
             0.767035,
             1.953754, -4.183892, 4.431713, 0.593203, 0.685676, 2.117802, -4.137093, 4.555096, 0.599262, 0.681069,
             2.285339,
             -4.051196, 4.582438, 0.607600, 0.677703, 2.850160, -3.665720, 4.484994, 0.631938, 0.663500, 5.278538,
             -2.238942,
             2.861224, 0.752033, 0.601315, 0.946709, 1.907628, 5.196779, 0.547226, 0.420395, 1.314173, 3.104912,
             4.231404,
             0.563544, 0.359828, 1.780000, 2.860000, 3.881555, 0.583841, 0.368714, 1.845110, -4.098880, 4.247264,
             0.586614,
             0.692366, 5.436187, -4.030482, 2.109852, 0.771915, 0.683578, 0.766444, 3.182131, 4.861453, 0.531597,
             0.352483,
             1.938616, -6.614410, 4.521085, 0.588371, 0.804441, 0.516573, 1.583572, 6.148363, 0.520797, 0.442565,
             1.246815,
             0.230297, 5.681036, 0.567985, 0.493479, 0.997827, -6.930921, 4.979576, 0.543283, 0.819255, 3.288807,
             -5.382514,
             3.795752, 0.655317, 0.745515, 2.311631, -1.566237, 4.590085, 0.621009, 0.574018, 2.680250, -6.111567,
             4.096152,
             0.625560, 0.780312, 3.832928, -1.537326, 4.137731, 0.680198, 0.570719, 2.961860, -2.274215, 4.440943,
             0.642764,
             0.604338, 4.386901, -2.683286, 3.643886, 0.704663, 0.621530, 1.217295, -7.834465, 4.969286, 0.552012,
             0.862592,
             1.542374, -0.136843, 5.201008, 0.589072, 0.508637, 3.878377, -6.041764, 3.311079, 0.685945, 0.775357,
             3.084037,
             -6.809842, 3.814195, 0.645735, 0.812640, 3.747321, -4.503545, 3.726453, 0.675343, 0.703978, 6.094129,
             -3.205991,
             1.473482, 0.810858, 0.646305, 4.588995, -4.728726, 2.983221, 0.720122, 0.714667, 6.583231, -3.941269,
             0.070268,
             0.866152, 0.682705, 3.492580, -3.195820, 4.130198, 0.663187, 0.644597, 1.255543, 0.802341, 5.307551,
             0.570082,
             0.466326, 1.126122, -0.933602, 6.538785, 0.544562, 0.548376, 1.443109, -1.142774, 5.905127, 0.562759,
             0.558785,
             0.923043, -0.529042, 7.003423, 0.531987, 0.530140, 1.755386, 3.529117, 4.327696, 0.585271, 0.335177,
             2.632589,
             3.713828, 4.364629, 0.622953, 0.322779, 3.388062, 3.721976, 4.309028, 0.655896, 0.320163, 4.075766,
             3.675413,
             4.076063, 0.687132, 0.322346, 4.622910, 3.474691, 3.646321, 0.716482, 0.333201, 5.171755, 2.535753,
             2.670867,
             0.758757, 0.382787, 7.297331, 0.763172, -0.048769, 0.897013, 0.468769, 4.706828, 1.651000, 3.109532,
             0.732392,
             0.424547, 4.071712, 1.476821, 3.476944, 0.702114, 0.433163, 3.269817, 1.470659, 3.731945, 0.666525,
             0.433866,
             2.527572, 1.617311, 3.865444, 0.633505, 0.426088, 1.970894, 1.858505, 3.961782, 0.603876, 0.416587,
             1.579543,
             2.097941, 4.084996, 0.579658, 0.409945, 7.664182, 0.673132, -2.435867, 0.992440, 0.480777, 1.397041,
             -1.340139,
             5.630378, 0.567192, 0.569420, 0.884838, 0.658740, 6.233232, 0.541366, 0.478899, 0.767097, -0.968035,
             7.077932,
             0.526564, 0.546118, 0.460213, -1.334106, 6.787447, 0.523913, 0.563830, 0.748618, -1.067994, 6.798303,
             0.531529,
             0.555057, 1.236408, -1.585568, 5.480490, 0.566036, 0.582329, 0.387306, -1.409990, 6.957705, 0.516311,
             0.563054,
             0.319925, -1.607931, 6.508676, 0.517472, 0.577877, 1.639633, 2.556298, 3.863736, 0.573595, 0.389807,
             1.255645,
             2.467144, 4.203800, 0.560698, 0.395332, 1.031362, 2.382663, 4.615849, 0.549756, 0.399751, 4.253081,
             2.772296,
             3.315305, 0.710288, 0.368253, 4.530000, 2.910000, 3.339685, 0.723330, 0.363373])
        self.procrustes_landmark_basis = [(4, 0.070909939706326), (6, 0.032100144773722), (10, 0.008446550928056),
                                          (33, 0.058724168688059), (54, 0.007667080033571), (67, 0.009078059345484),
                                          (117, 0.009791937656701), (119, 0.014565368182957), (121, 0.018591361120343),
                                          (127, 0.005197994410992), (129, 0.120625205338001), (132, 0.005560018587857),
                                          (133, 0.05328618362546), (136, 0.066890455782413), (143, 0.014816547743976),
                                          (147, 0.014262833632529), (198, 0.025462191551924), (205, 0.047252278774977),
                                          (263, 0.058724168688059), (284, 0.007667080033571), (297, 0.009078059345484),
                                          (346, 0.009791937656701), (348, 0.014565368182957), (350, 0.018591361120343),
                                          (356, 0.005197994410992), (358, 0.120625205338001), (361, 0.005560018587857),
                                          (362, 0.05328618362546), (365, 0.066890455782413), (372, 0.014816547743976),
                                          (376, 0.014262833632529), (420, 0.025462191551924), (425, 0.047252278774977)]
        self.canonical_metric_landmarks = np.reshape(self.canonical_metric_landmarks,
                                                     (self.canonical_metric_landmarks.shape[0] // 5, 5))
        self.canonical_metric_landmarks = self.canonical_metric_landmarks[:, :3]

        # Rotate face around
        rotate_mat = np.asarray([[1, 0, 0], [0, -1, 0], [0, 0, -1]], dtype=np.float64)
        self.canonical_metric_landmarks = np.matmul(self.canonical_metric_landmarks.reshape(-1, 3), rotate_mat)

        self.points_idx = [4, 6, 10, 33, 54, 67, 117, 119, 121, 127, 129, 132, 133, 136, 143, 147, 198, 205, 263, 284, 297, 346, 348, 350, 356, 358, 361, 362, 365, 372, 376, 420, 425]
        #self.points_idx.append([33, 263, 1, 61, 291, 199])
        #self.points_idx = [33,263,1,61,291,199]
        self.points_idx = list(set(self.points_idx))
       # self.points_idx = list(set(range(468)))
        self.points_idx.sort()


        self.canoncial_metric_landmarks_pnp = self.canonical_metric_landmarks[self.points_idx, :]
        self.LEFT_EYE = {idx for connection in mp.solutions.face_mesh.FACEMESH_LEFT_EYE for idx in connection}
        self.RIGHT_EYE = {idx for connection in mp.solutions.face_mesh.FACEMESH_RIGHT_EYE for idx in connection}
        self.eye_idx = list(self.LEFT_EYE.union(self.RIGHT_EYE))

    def fit_func(self, landmarks, camera_parameters):
        fx, fy, cx, cy = camera_parameters
        landmarks = landmarks[self.points_idx, :]
        # Initial fit
        camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float64)
        success, rvec, tvec, inliers = cv2.solvePnPRansac(self.canoncial_metric_landmarks_pnp, landmarks,
                                                          camera_matrix, None, flags=cv2.SOLVEPNP_EPNP)

        # Second fit for higher accuracy
        success, rvec, tvec = cv2.solvePnP(self.canoncial_metric_landmarks_pnp, landmarks, camera_matrix, None,
                                           rvec=rvec, tvec=tvec, useExtrinsicGuess=True, flags=cv2.SOLVEPNP_ITERATIVE)

        return rvec, tvec

    def drawPose(self, img, r, t, cam, dist):
        modelAxes = np.array([
            np.array([0., -20., 0.]).reshape(1, 3),
            np.array([50., -20., 0.]).reshape(1, 3),
            np.array([0., -70., 0.]).reshape(1, 3),
            np.array([0., -20., -50.]).reshape(1, 3)
        ])

        projAxes, jac = cv2.projectPoints(modelAxes, r, t, cam, dist)

        cv2.line(img, (int(projAxes[0, 0, 0]), int(projAxes[0, 0, 1])),
                 (int(projAxes[1, 0, 0]), int(projAxes[1, 0, 1])),
                 (0, 255, 255), 2)
        cv2.line(img, (int(projAxes[0, 0, 0]), int(projAxes[0, 0, 1])),
                 (int(projAxes[2, 0, 0]), int(projAxes[2, 0, 1])),
                 (255, 0, 255), 2)
        cv2.line(img, (int(projAxes[0, 0, 0]), int(projAxes[0, 0, 1])),
                 (int(projAxes[3, 0, 0]), int(projAxes[3, 0, 1])),
                 (255, 255, 0), 2)

    def project_model(self, rvec, tvec, camera_parameters):
        fx, fy, cx, cy = camera_parameters
        camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float64)
        points, _ = cv2.projectPoints(self.canonical_metric_landmarks, rvec, tvec, camera_matrix, None)
        return points.squeeze(1)