import numpy as np pose = np.array([[[6.5433e+02, 5.8361e+02, 9.9946e-01], [6.8423e+02, 6.5411e+02, 9.9998e-01], [6.9117e+02, 7.2399e+02, 9.9999e-01], [6.9718e+02, 7.9327e+02, 9.9987e-01], [7.0293e+02, 8.5917e+02, 9.3761e-01]], [[6.3452e+02, 2.8308e+02, 9.9482e-01], [6.5906e+02, 3.4467e+02, 9.9938e-01], [6.5794e+02, 4.0197e+02, 1.0000e+00], [6.6237e+02, 4.6714e+02, 9.9999e-01], [6.6390e+02, 5.2631e+02, 9.9611e-01]], [[6.3164e+02, 4.3033e+00, 9.6437e-01], [6.4993e+02, 5.6203e+01, 9.9290e-01], [6.5003e+02, 1.1126e+02, 9.9973e-01], [6.5038e+02, 1.6832e+02, 9.9971e-01], [6.4932e+02, 2.2213e+02, 9.7749e-01]], [[6.9001e+02, 9.0983e+02, 9.5246e-01], [7.1195e+02, 9.7251e+02, 9.9073e-01], [7.1940e+02, 1.0361e+03, 9.9944e-01], [7.2875e+02, 1.1032e+03, 9.9775e-01], [7.3482e+02, 1.1613e+03, 2.4290e-01]], [[6.9527e+02, 8.9296e+02, 9.9578e-01], [7.2340e+02, 9.6668e+02, 9.9980e-01], [7.2624e+02, 1.0355e+03, 9.9973e-01], [7.3433e+02, 1.1097e+03, 9.9814e-01], [7.4265e+02, 1.1769e+03, 5.6901e-01]]]) segment = np.array( [[ 0.22812, 1159.4], [ 0.22812, 1366.9], [ 1224, 1366.9], [ 1224, 1257.8], [ 1217.4, 1257.8], [ 1215.2, 1255.6], [ 1200.2, 1255.6], [ 1198.1, 1253.5], [ 1183.1, 1253.5], [ 1181, 1251.4], [ 1163.9, 1251.4], [ 1161.7, 1249.2], [ 1144.6, 1249.2], [ 1142.5, 1247.1], [ 1121.1, 1247.1], [ 1119, 1244.9], [ 1116.8, 1247.1], [ 1110.4, 1247.1], [ 1108.3, 1244.9], [ 1069.8, 1244.9], [ 1067.6, 1242.8], [ 1044.1, 1242.8], [ 1042, 1240.7], [ 1027, 1240.7], [ 1024.8, 1238.5], [ 1009.9, 1238.5], [ 1007.7, 1236.4], [ 994.89, 1236.4], [ 992.75, 1234.2], [ 975.64, 1234.2], [ 973.5, 1232.1], [ 952.11, 1232.1], [ 949.97, 1230], [ 924.3, 1230], [ 922.16, 1227.8], [ 902.91, 1227.8], [ 900.77, 1225.7], [ 883.66, 1225.7], [ 881.52, 1223.5], [ 860.13, 1223.5], [ 857.99, 1221.4], [ 843.02, 1221.4], [ 840.88, 1219.3], [ 825.91, 1219.3], [ 823.77, 1217.1], [ 798.1, 1217.1], [ 795.96, 1215], [ 751.04, 1215], [ 748.9, 1212.8], [ 714.68, 1212.8], [ 712.54, 1210.7], [ 706.12, 1210.7], [ 703.98, 1208.6], [ 697.56, 1208.6], [ 695.42, 1206.4], [ 678.31, 1206.4], [ 676.17, 1204.3], [ 663.34, 1204.3], [ 661.2, 1202.2], [ 639.81, 1202.2], [ 637.67, 1200], [ 603.44, 1200], [ 601.3, 1197.9], [ 590.61, 1197.9], [ 588.47, 1195.7], [ 577.78, 1195.7], [ 575.64, 1193.6], [ 562.8, 1193.6], [ 560.66, 1191.5], [ 547.83, 1191.5], [ 545.69, 1189.3], [ 532.85, 1189.3], [ 530.72, 1187.2], [ 479.38, 1187.2], [ 477.24, 1185], [ 447.29, 1185], [ 445.15, 1182.9], [ 406.65, 1182.9], [ 404.51, 1180.8], [ 357.45, 1180.8], [ 355.31, 1178.6], [ 323.23, 1178.6], [ 321.09, 1176.5], [ 306.11, 1176.5], [ 303.98, 1174.3], [ 289, 1174.3], [ 286.86, 1172.2], [ 269.75, 1172.2], [ 267.61, 1170.1], [ 244.08, 1170.1], [ 241.94, 1167.9], [ 233.39, 1167.9], [ 231.25, 1165.8], [ 231.25, 1159.4]] )