From 3e3a5194e2911756a6ff83fd0238f6393ca1cfa1 Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 11 Sep 2024 14:51:53 +0200 Subject: [PATCH] Fixing some errors. --- spt/triangulator.cpp | 20 +- spt/triangulator.hpp | 2 +- swig/spt.i | 18 + tests/poses_p1.json | 2136 +++++++++++++++++++++++++++++++++++++++ tests/test_interface.py | 35 +- 5 files changed, 2190 insertions(+), 21 deletions(-) create mode 100644 tests/poses_p1.json diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp index 433dd26..43c3025 100644 --- a/spt/triangulator.cpp +++ b/spt/triangulator.cpp @@ -251,10 +251,10 @@ std::vector>> TriangulatorInternal::triangulat } if (!drop_indices.empty()) { - for (size_t i = drop_indices.size() - 1; i >= 0; --i) + for (size_t i = drop_indices.size(); i > 0; --i) { - all_scored_poses.erase(all_scored_poses.begin() + drop_indices[i]); - all_pairs.erase(all_pairs.begin() + drop_indices[i]); + all_scored_poses.erase(all_scored_poses.begin() + drop_indices[i - 1]); + all_pairs.erase(all_pairs.begin() + drop_indices[i - 1]); } } @@ -328,7 +328,7 @@ void TriangulatorInternal::reset() // ================================================================================================= -void TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &icam) +cv::Mat TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &icam) { int width = icam.cam.width; int height = icam.cam.height; @@ -340,9 +340,7 @@ void TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &ica // Undistort points cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK); - // Update the camera parameters - icam.K = newK; - icam.DC = cv::Mat::zeros(5, 1, CV_64F); + return newK; } // ================================================================================================= @@ -363,7 +361,13 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn } // Undistort the points - undistort_points(points, icam); + cv::Mat newK = undistort_points(points, icam); + if (p == poses.size() - 1) + { + // Update the camera matrix as well + icam.K = newK; + icam.DC = cv::Mat::zeros(5, 1, CV_64F); + } // Update the original poses with the undistorted points for (int j = 0; j < num_joints; ++j) diff --git a/spt/triangulator.hpp b/spt/triangulator.hpp index 3437518..4f8cd63 100644 --- a/spt/triangulator.hpp +++ b/spt/triangulator.hpp @@ -60,7 +60,7 @@ private: std::vector last_poses_3d; - void undistort_points(cv::Mat &points, CameraInternal &icam); + cv::Mat undistort_points(cv::Mat &points, CameraInternal &icam); void undistort_poses(std::vector &poses, CameraInternal &icam); std::tuple, std::vector> project_poses( diff --git a/swig/spt.i b/swig/spt.i index 85d51a7..d383757 100644 --- a/swig/spt.i +++ b/swig/spt.i @@ -49,3 +49,21 @@ namespace std { // Parse the header file to generate wrappers %include "../spt/camera.hpp" %include "../spt/interface.hpp" + +// Add additional Python code to the module +%pythoncode %{ +def convert_cameras(cameras): + """Convert cameras from Python to C++.""" + c_cameras = [] + for cam in cameras: + camera = Camera() + camera.name = cam["name"] + camera.K = cam["K"] + camera.DC = cam["DC"] + camera.R = cam["R"] + camera.T = cam["T"] + camera.width = cam["width"] + camera.height = cam["height"] + c_cameras.append(camera) + return c_cameras +%} diff --git a/tests/poses_p1.json b/tests/poses_p1.json new file mode 100644 index 0000000..5888459 --- /dev/null +++ b/tests/poses_p1.json @@ -0,0 +1,2136 @@ +{ + "2D": [ + [ + [ + [ + 930.123, + 424.827, + 1.0 + ], + [ + 946.42, + 412.875, + 1.0 + ], + [ + 919.258, + 408.529, + 1.0 + ], + [ + 967.063, + 420.481, + 1.0 + ], + [ + 901.874, + 409.616, + 0.994 + ], + [ + 997.485, + 504.14, + 0.874 + ], + [ + 854.069, + 481.324, + 0.895 + ], + [ + 1009.436, + 606.269, + 0.92 + ], + [ + 810.61, + 571.501, + 0.875 + ], + [ + 962.717, + 585.626, + 0.875 + ], + [ + 866.021, + 580.193, + 0.628 + ], + [ + 942.074, + 704.052, + 0.776 + ], + [ + 857.329, + 697.533, + 0.747 + ], + [ + 926.863, + 850.727, + 0.857 + ], + [ + 816.042, + 846.381, + 0.83 + ], + [ + 911.653, + 973.499, + 0.929 + ], + [ + 774.756, + 983.277, + 0.962 + ], + [ + 899.701, + 700.793, + 0.747 + ], + [ + 925.777, + 492.732, + 0.874 + ], + [ + 934.469, + 415.048, + 0.994 + ] + ], + [ + [ + 1137.332, + 381.096, + 0.95 + ], + [ + 1143.783, + 366.906, + 1.0 + ], + [ + 1138.622, + 364.326, + 0.79 + ], + [ + 1183.774, + 372.066, + 1.0 + ], + [ + 1201.834, + 366.906, + 0.627 + ], + [ + 1286.976, + 448.178, + 0.747 + ], + [ + 1212.154, + 446.888, + 0.67 + ], + [ + 1383.729, + 552.671, + 0.887 + ], + [ + 1245.695, + 562.991, + 0.45 + ], + [ + 1303.747, + 662.324, + 0.872 + ], + [ + 1201.834, + 658.454, + 0.53 + ], + [ + 1234.085, + 760.366, + 0.666 + ], + [ + 1178.614, + 755.206, + 0.688 + ], + [ + 1246.985, + 953.872, + 0.71 + ], + [ + 1176.033, + 920.331, + 0.732 + ], + [ + 1249.565, + 1067.395, + 0.763 + ], + [ + 1167.003, + 1045.464, + 0.893 + ], + [ + 1206.349, + 757.786, + 0.666 + ], + [ + 1249.565, + 447.533, + 0.67 + ], + [ + 1192.804, + 369.486, + 0.627 + ] + ], + [ + [ + 726.471, + 394.265, + 0.847 + ], + [ + 723.919, + 378.952, + 0.605 + ], + [ + 723.919, + 380.228, + 0.841 + ], + [ + 628.214, + 382.78, + 0.704 + ], + [ + 707.33, + 391.712, + 0.891 + ], + [ + 593.76, + 484.866, + 0.764 + ], + [ + 706.054, + 505.283, + 0.756 + ], + [ + 653.735, + 634.166, + 0.557 + ], + [ + 722.643, + 681.38, + 0.784 + ], + [ + 764.753, + 612.473, + 0.667 + ], + [ + 823.452, + 627.786, + 0.728 + ], + [ + 630.766, + 811.54, + 0.664 + ], + [ + 704.778, + 817.92, + 0.659 + ], + [ + 630.766, + 981.257, + 0.751 + ], + [ + 685.637, + 1005.502, + 0.701 + ], + [ + 586.103, + 1068.03, + 0.606 + ], + [ + 670.324, + 1061.65, + 0.406 + ], + [ + 667.772, + 814.73, + 0.659 + ], + [ + 649.907, + 495.074, + 0.756 + ], + [ + 667.772, + 387.246, + 0.704 + ] + ], + [ + [ + 99.363, + 397.744, + 0.214 + ], + [ + 108.914, + 397.744, + 0.205 + ], + [ + 110.107, + 397.744, + 0.089 + ], + [ + 117.867, + 397.744, + 0.302 + ], + [ + 173.975, + 397.744, + 0.224 + ], + [ + 120.255, + 397.744, + 0.371 + ], + [ + 188.3, + 397.744, + 0.419 + ], + [ + 101.154, + 497.425, + 0.418 + ], + [ + 229.486, + 462.208, + 0.378 + ], + [ + 89.813, + 397.744, + 0.245 + ], + [ + 203.222, + 455.642, + 0.194 + ], + [ + 148.905, + 551.742, + 0.577 + ], + [ + 185.913, + 545.176, + 0.425 + ], + [ + 168.006, + 660.973, + 0.572 + ], + [ + 79.666, + 573.827, + 0.67 + ], + [ + 193.672, + 766.623, + 0.69 + ], + [ + 96.379, + 680.074, + 0.666 + ], + [ + 167.409, + 548.459, + 0.425 + ], + [ + 154.277, + 397.744, + 0.371 + ], + [ + 145.921, + 397.744, + 0.224 + ] + ] + ], + [ + [ + [ + 1173.536, + 411.798, + 0.877 + ], + [ + 1191.17, + 406.508, + 0.953 + ], + [ + 1174.418, + 395.928, + 0.873 + ], + [ + 1233.49, + 410.035, + 0.76 + ], + [ + 1171.773, + 378.295, + 0.322 + ], + [ + 1234.372, + 517.599, + 0.748 + ], + [ + 1181.471, + 425.905, + 0.781 + ], + [ + 1167.365, + 593.423, + 0.799 + ], + [ + 1148.849, + 496.439, + 0.76 + ], + [ + 1074.789, + 559.92, + 0.783 + ], + [ + 1073.025, + 499.966, + 0.802 + ], + [ + 1167.365, + 648.969, + 0.668 + ], + [ + 1132.979, + 622.518, + 0.548 + ], + [ + 1152.376, + 741.545, + 0.806 + ], + [ + 1131.216, + 708.041, + 0.723 + ], + [ + 1179.708, + 818.25, + 0.857 + ], + [ + 1125.044, + 775.93, + 0.84 + ], + [ + 1150.172, + 635.744, + 0.548 + ], + [ + 1207.922, + 471.752, + 0.748 + ], + [ + 1202.631, + 394.165, + 0.322 + ] + ], + [ + [ + 797.462, + 628.061, + 0.682 + ], + [ + 791.009, + 619.995, + 0.439 + ], + [ + 791.816, + 628.867, + 0.695 + ], + [ + 703.097, + 646.611, + 0.659 + ], + [ + 774.879, + 655.483, + 0.641 + ], + [ + 655.512, + 694.196, + 0.729 + ], + [ + 812.786, + 763.558, + 0.672 + ], + [ + 662.77, + 763.558, + 0.512 + ], + [ + 861.178, + 862.762, + 0.702 + ], + [ + 765.2, + 703.875, + 0.515 + ], + [ + 844.241, + 750.654, + 0.734 + ], + [ + 743.424, + 895.83, + 0.608 + ], + [ + 826.497, + 899.056, + 0.52 + ], + [ + 753.909, + 946.642, + 0.673 + ], + [ + 885.374, + 954.707, + 0.761 + ], + [ + 775.685, + 1005.519, + 0.754 + ], + [ + 913.603, + 1034.554, + 0.864 + ], + [ + 784.96, + 897.443, + 0.52 + ], + [ + 734.149, + 728.877, + 0.672 + ], + [ + 738.988, + 651.047, + 0.641 + ] + ], + [ + [ + 735.995, + 354.427, + 0.909 + ], + [ + 749.807, + 338.024, + 0.907 + ], + [ + 722.182, + 337.161, + 0.943 + ], + [ + 679.88, + 301.765, + 0.388 + ], + [ + 680.743, + 306.945, + 0.902 + ], + [ + 722.182, + 336.297, + 0.609 + ], + [ + 624.629, + 401.045, + 0.705 + ], + [ + 704.916, + 439.03, + 0.403 + ], + [ + 546.069, + 480.468, + 0.825 + ], + [ + 731.678, + 498.598, + 0.483 + ], + [ + 625.492, + 541.763, + 0.766 + ], + [ + 746.354, + 567.662, + 0.623 + ], + [ + 691.966, + 581.474, + 0.699 + ], + [ + 746.354, + 662.625, + 0.576 + ], + [ + 689.376, + 682.481, + 0.605 + ], + [ + 750.671, + 740.322, + 0.431 + ], + [ + 692.83, + 755.861, + 0.471 + ], + [ + 719.16, + 574.568, + 0.623 + ], + [ + 673.405, + 368.671, + 0.609 + ], + [ + 680.312, + 304.355, + 0.388 + ] + ] + ], + [ + [ + [ + 802.476, + 376.632, + 0.957 + ], + [ + 799.155, + 363.348, + 0.844 + ], + [ + 789.192, + 368.883, + 0.939 + ], + [ + 739.375, + 374.418, + 0.338 + ], + [ + 747.125, + 383.274, + 0.784 + ], + [ + 781.443, + 426.449, + 0.863 + ], + [ + 713.913, + 492.871, + 0.806 + ], + [ + 821.296, + 537.153, + 0.816 + ], + [ + 750.446, + 620.18, + 0.874 + ], + [ + 892.146, + 551.544, + 0.764 + ], + [ + 856.721, + 594.719, + 0.815 + ], + [ + 816.868, + 673.318, + 0.642 + ], + [ + 778.122, + 693.245, + 0.693 + ], + [ + 826.831, + 778.487, + 0.775 + ], + [ + 795.834, + 820.554, + 0.748 + ], + [ + 795.834, + 871.478, + 0.834 + ], + [ + 809.119, + 929.044, + 0.843 + ], + [ + 797.495, + 683.282, + 0.642 + ], + [ + 747.678, + 459.66, + 0.806 + ], + [ + 743.25, + 378.846, + 0.338 + ] + ], + [ + [ + 1208.428, + 431.339, + 0.585 + ], + [ + 1205.875, + 424.957, + 0.579 + ], + [ + 1210.98, + 409.642, + 0.34 + ], + [ + 1213.533, + 442.826, + 0.775 + ], + [ + 1296.493, + 409.642, + 0.631 + ], + [ + 1240.336, + 556.418, + 0.723 + ], + [ + 1380.73, + 490.049, + 0.73 + ], + [ + 1258.204, + 741.483, + 0.752 + ], + [ + 1471.348, + 602.365, + 0.758 + ], + [ + 1209.704, + 812.957, + 0.618 + ], + [ + 1380.73, + 694.259, + 0.705 + ], + [ + 1231.401, + 871.667, + 0.595 + ], + [ + 1309.256, + 855.075, + 0.584 + ], + [ + 1208.428, + 1042.693, + 0.605 + ], + [ + 1311.809, + 964.838, + 0.594 + ], + [ + 1221.191, + 1047.798, + 0.295 + ], + [ + 1301.599, + 1052.903, + 0.717 + ], + [ + 1270.329, + 863.371, + 0.584 + ], + [ + 1310.533, + 523.233, + 0.723 + ], + [ + 1255.013, + 426.234, + 0.631 + ] + ], + [ + [ + 1238.075, + 354.908, + 0.531 + ], + [ + 1255.196, + 345.191, + 0.552 + ], + [ + 1236.686, + 339.175, + 0.62 + ], + [ + 1300.544, + 359.535, + 0.711 + ], + [ + 1230.208, + 354.445, + 0.278 + ], + [ + 1333.398, + 431.259, + 0.56 + ], + [ + 1235.298, + 396.554, + 0.327 + ], + [ + 1348.206, + 527.508, + 0.416 + ], + [ + 1191.801, + 459.949, + 0.523 + ], + [ + 1252.882, + 525.195, + 0.465 + ], + [ + 1190.413, + 494.191, + 0.543 + ], + [ + 1270.003, + 583.037, + 0.207 + ], + [ + 1203.832, + 583.037, + 0.474 + ], + [ + 1199.205, + 583.037, + 0.129 + ], + [ + 1192.727, + 583.037, + 0.29 + ], + [ + 1399.569, + 583.037, + 0.044 + ], + [ + 1189.487, + 583.037, + 0.111 + ], + [ + 1236.918, + 583.037, + 0.207 + ], + [ + 1284.348, + 413.907, + 0.327 + ], + [ + 1265.376, + 356.99, + 0.278 + ] + ] + ], + [ + [ + [ + 705.839, + 195.073, + 0.884 + ], + [ + 714.578, + 176.625, + 0.942 + ], + [ + 684.479, + 187.305, + 0.922 + ], + [ + 728.171, + 147.497, + 0.697 + ], + [ + 651.467, + 177.596, + 0.823 + ], + [ + 783.515, + 210.608, + 0.825 + ], + [ + 633.019, + 239.736, + 0.827 + ], + [ + 800.021, + 329.063, + 0.827 + ], + [ + 657.292, + 339.743, + 0.835 + ], + [ + 765.067, + 394.116, + 0.882 + ], + [ + 669.915, + 395.087, + 0.867 + ], + [ + 793.224, + 410.622, + 0.685 + ], + [ + 719.433, + 433.924, + 0.693 + ], + [ + 832.062, + 504.803, + 0.823 + ], + [ + 765.067, + 531.019, + 0.782 + ], + [ + 865.074, + 556.263, + 0.843 + ], + [ + 799.05, + 591.217, + 0.871 + ], + [ + 756.328, + 422.273, + 0.685 + ], + [ + 708.267, + 225.172, + 0.825 + ], + [ + 689.819, + 162.546, + 0.697 + ] + ], + [ + [ + 584.905, + 561.812, + 0.814 + ], + [ + 573.336, + 543.303, + 0.752 + ], + [ + 562.925, + 556.028, + 0.919 + ], + [ + 494.672, + 591.89, + 0.451 + ], + [ + 505.084, + 594.203, + 0.853 + ], + [ + 385.931, + 648.574, + 0.709 + ], + [ + 581.434, + 741.12, + 0.646 + ], + [ + 314.208, + 761.943, + 0.772 + ], + [ + 626.55, + 919.271, + 0.757 + ], + [ + 437.988, + 772.354, + 0.568 + ], + [ + 663.569, + 877.625, + 0.433 + ], + [ + 551.357, + 898.448, + 0.555 + ], + [ + 654.314, + 903.076, + 0.532 + ], + [ + 566.395, + 944.721, + 0.691 + ], + [ + 706.371, + 973.642, + 0.683 + ], + [ + 586.061, + 1003.719, + 0.767 + ], + [ + 766.526, + 1039.581, + 0.874 + ], + [ + 602.835, + 900.762, + 0.532 + ], + [ + 483.682, + 694.847, + 0.646 + ], + [ + 499.878, + 593.046, + 0.451 + ] + ], + [ + [ + 946.904, + 558.412, + 0.731 + ], + [ + 952.715, + 556.752, + 0.746 + ], + [ + 947.734, + 543.47, + 0.54 + ], + [ + 984.258, + 592.445, + 0.655 + ], + [ + 1068.925, + 533.509, + 0.634 + ], + [ + 988.408, + 695.374, + 0.554 + ], + [ + 1128.69, + 550.111, + 0.706 + ], + [ + 977.617, + 794.153, + 0.609 + ], + [ + 1055.644, + 592.445, + 0.42 + ], + [ + 941.924, + 677.112, + 0.762 + ], + [ + 995.049, + 600.745, + 0.508 + ], + [ + 1039.873, + 818.225, + 0.584 + ], + [ + 1115.409, + 765.93, + 0.529 + ], + [ + 1060.624, + 877.99, + 0.631 + ], + [ + 1112.919, + 790.832, + 0.572 + ], + [ + 1071.415, + 939.416, + 0.76 + ], + [ + 1115.409, + 813.244, + 0.656 + ], + [ + 1077.641, + 792.078, + 0.529 + ], + [ + 1058.549, + 622.742, + 0.554 + ], + [ + 1026.591, + 562.977, + 0.634 + ] + ] + ], + [ + [ + [ + 751.911, + 320.818, + 0.993 + ], + [ + 754.621, + 307.269, + 0.919 + ], + [ + 735.652, + 304.559, + 1.0 + ], + [ + 697.716, + 320.818, + 0.43 + ], + [ + 678.747, + 312.689, + 1.0 + ], + [ + 726.168, + 410.24, + 0.81 + ], + [ + 619.132, + 403.466, + 0.809 + ], + [ + 719.394, + 528.115, + 0.569 + ], + [ + 597.454, + 543.019, + 0.89 + ], + [ + 776.299, + 513.212, + 0.741 + ], + [ + 723.458, + 521.341, + 0.823 + ], + [ + 726.168, + 677.153, + 0.692 + ], + [ + 661.134, + 681.218, + 0.7 + ], + [ + 700.425, + 860.063, + 0.775 + ], + [ + 684.167, + 876.321, + 0.755 + ], + [ + 696.361, + 1005.035, + 0.826 + ], + [ + 661.134, + 1061.941, + 0.866 + ], + [ + 693.651, + 679.185, + 0.692 + ], + [ + 672.65, + 406.853, + 0.809 + ], + [ + 688.231, + 316.753, + 0.43 + ] + ], + [ + [ + 1112.157, + 287.034, + 0.782 + ], + [ + 1112.157, + 270.888, + 0.867 + ], + [ + 1118.028, + 272.356, + 0.562 + ], + [ + 1123.899, + 278.227, + 0.92 + ], + [ + 1232.513, + 284.098, + 0.834 + ], + [ + 1085.737, + 401.519, + 0.729 + ], + [ + 1279.481, + 407.39, + 0.789 + ], + [ + 1062.253, + 590.859, + 0.796 + ], + [ + 1275.078, + 570.31, + 0.68 + ], + [ + 1144.448, + 620.214, + 0.392 + ], + [ + 1194.351, + 630.488, + 0.452 + ], + [ + 1112.157, + 759.651, + 0.61 + ], + [ + 1222.239, + 755.247, + 0.599 + ], + [ + 1088.673, + 982.749, + 0.767 + ], + [ + 1194.351, + 966.604, + 0.743 + ], + [ + 1100.415, + 1067.879, + 0.604 + ], + [ + 1184.077, + 1072.282, + 0.775 + ], + [ + 1167.198, + 757.449, + 0.599 + ], + [ + 1182.609, + 404.454, + 0.729 + ], + [ + 1178.206, + 281.163, + 0.834 + ] + ], + [ + [ + 1012.723, + 259.29, + 0.893 + ], + [ + 1034.237, + 249.878, + 0.981 + ], + [ + 1012.723, + 243.155, + 0.838 + ], + [ + 1081.297, + 253.912, + 0.861 + ], + [ + 1047.682, + 244.5, + 0.467 + ], + [ + 1137.769, + 330.553, + 0.773 + ], + [ + 980.453, + 323.83, + 0.894 + ], + [ + 1168.695, + 438.119, + 0.597 + ], + [ + 945.494, + 403.16, + 0.921 + ], + [ + 1125.668, + 524.172, + 0.543 + ], + [ + 962.974, + 494.592, + 0.904 + ], + [ + 1109.533, + 604.847, + 0.725 + ], + [ + 1019.446, + 602.158, + 0.726 + ], + [ + 1112.222, + 772.92, + 0.718 + ], + [ + 992.555, + 770.231, + 0.785 + ], + [ + 1121.634, + 914.101, + 0.641 + ], + [ + 972.386, + 907.378, + 0.884 + ], + [ + 1064.49, + 603.503, + 0.725 + ], + [ + 1059.111, + 327.191, + 0.773 + ], + [ + 1064.49, + 249.206, + 0.467 + ] + ] + ] + ], + "3D": [ + [ + [ + -0.363, + -0.27, + 1.455, + 0.964 + ], + [ + -0.406, + -0.246, + 1.478, + 0.959 + ], + [ + -0.372, + -0.301, + 1.489, + 0.973 + ], + [ + -0.527, + -0.292, + 1.461, + 0.965 + ], + [ + -0.416, + -0.38, + 1.486, + 0.989 + ], + [ + -0.58, + -0.232, + 1.278, + 0.981 + ], + [ + -0.395, + -0.515, + 1.321, + 0.979 + ], + [ + -0.643, + -0.229, + 1.025, + 0.987 + ], + [ + -0.326, + -0.588, + 1.085, + 0.987 + ], + [ + -0.414, + -0.193, + 1.094, + 0.986 + ], + [ + -0.289, + -0.351, + 1.102, + 0.977 + ], + [ + -0.544, + -0.343, + 0.758, + 0.94 + ], + [ + -0.375, + -0.477, + 0.792, + 0.974 + ], + [ + -0.515, + -0.366, + 0.396, + 0.971 + ], + [ + -0.25, + -0.489, + 0.444, + 0.984 + ], + [ + -0.501, + -0.388, + 0.052, + 0.984 + ], + [ + -0.213, + -0.569, + 0.083, + 0.971 + ], + [ + -0.473, + -0.422, + 0.766, + 0.933 + ], + [ + -0.5, + -0.375, + 1.299, + 0.994 + ], + [ + -0.473, + -0.338, + 1.472, + 0.988 + ] + ], + [ + [ + 0.473, + -0.018, + 1.519, + 0.991 + ], + [ + 0.484, + -0.04, + 1.541, + 0.976 + ], + [ + 0.485, + 0.013, + 1.539, + 0.993 + ], + [ + 0.59, + -0.077, + 1.532, + 0.977 + ], + [ + 0.558, + 0.069, + 1.525, + 0.988 + ], + [ + 0.578, + -0.171, + 1.349, + 0.981 + ], + [ + 0.622, + 0.142, + 1.339, + 0.997 + ], + [ + 0.496, + -0.145, + 1.093, + 0.986 + ], + [ + 0.582, + 0.154, + 1.067, + 0.978 + ], + [ + 0.317, + -0.033, + 1.126, + 0.986 + ], + [ + 0.367, + 0.146, + 1.126, + 0.985 + ], + [ + 0.545, + -0.096, + 0.804, + 0.971 + ], + [ + 0.557, + 0.07, + 0.817, + 0.968 + ], + [ + 0.552, + -0.139, + 0.465, + 0.982 + ], + [ + 0.573, + 0.028, + 0.452, + 0.954 + ], + [ + 0.669, + -0.204, + 0.105, + 0.977 + ], + [ + 0.596, + -0.016, + 0.096, + 0.997 + ], + [ + 0.555, + -0.013, + 0.803, + 0.973 + ], + [ + 0.599, + -0.015, + 1.343, + 0.977 + ], + [ + 0.575, + -0.001, + 1.526, + 0.975 + ] + ], + [ + [ + -0.344, + 0.303, + 1.569, + 0.988 + ], + [ + -0.305, + 0.33, + 1.585, + 0.965 + ], + [ + -0.331, + 0.319, + 1.588, + 0.973 + ], + [ + -0.296, + 0.443, + 1.579, + 0.981 + ], + [ + -0.413, + 0.421, + 1.614, + 0.983 + ], + [ + -0.331, + 0.626, + 1.442, + 0.956 + ], + [ + -0.576, + 0.368, + 1.446, + 0.981 + ], + [ + -0.361, + 0.8, + 1.242, + 0.993 + ], + [ + -0.763, + 0.382, + 1.251, + 0.995 + ], + [ + -0.301, + 0.708, + 1.052, + 0.979 + ], + [ + -0.615, + 0.345, + 1.039, + 0.954 + ], + [ + -0.359, + 0.566, + 0.826, + 0.974 + ], + [ + -0.504, + 0.412, + 0.743, + 0.936 + ], + [ + -0.383, + 0.606, + 0.429, + 0.959 + ], + [ + -0.532, + 0.379, + 0.422, + 0.956 + ], + [ + -0.394, + 0.545, + 0.145, + 0.942 + ], + [ + -0.569, + 0.33, + 0.088, + 0.981 + ], + [ + -0.436, + 0.477, + 0.759, + 0.946 + ], + [ + -0.451, + 0.495, + 1.44, + 0.975 + ], + [ + -0.367, + 0.428, + 1.587, + 0.978 + ] + ], + [ + [ + 1.279, + -0.723, + 1.529, + 0.922 + ], + [ + 1.27, + -0.697, + 1.534, + 0.909 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 1.269, + -0.649, + 1.532, + 0.915 + ], + [ + 1.179, + -0.623, + 1.538, + 0.902 + ], + [ + 1.282, + -0.6, + 1.493, + 0.968 + ], + [ + 1.139, + -0.67, + 1.495, + 0.979 + ], + [ + 1.316, + -0.588, + 1.373, + 0.905 + ], + [ + 1.063, + -0.664, + 1.403, + 0.959 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 1.116, + -0.679, + 1.359, + 0.892 + ], + [ + 1.24, + -0.598, + 1.241, + 0.987 + ], + [ + 1.159, + -0.644, + 1.238, + 0.973 + ], + [ + 1.22, + -0.577, + 1.065, + 0.983 + ], + [ + 1.319, + -0.777, + 1.159, + 0.894 + ], + [ + 1.195, + -0.54, + 0.896, + 0.951 + ], + [ + 1.291, + -0.775, + 1.002, + 0.998 + ], + [ + 1.2, + -0.621, + 1.239, + 0.98 + ], + [ + 1.213, + -0.634, + 1.494, + 0.973 + ], + [ + 1.225, + -0.635, + 1.535, + 0.908 + ] + ] + ] +} \ No newline at end of file diff --git a/tests/test_interface.py b/tests/test_interface.py index 0dc0112..1a80ad9 100644 --- a/tests/test_interface.py +++ b/tests/test_interface.py @@ -26,7 +26,7 @@ def main(): print("") # Load input data - roomparams = [[0, 0, 1], [4, 4, 2]] + roomparams = [[0, 0, 1.0], [4.8, 6.0, 2.0]] joint_names = [ "nose", "eye_left", @@ -57,17 +57,7 @@ def main(): pdata = json.load(file) cams = cdata["cameras"] poses_2d = pdata["2D"] - cameras = [] - for cam in cams: - camera = spt.Camera() - camera.name = cam["name"] - camera.K = cam["K"] - camera.DC = cam["DC"] - camera.R = cam["R"] - camera.T = cam["T"] - camera.width = cam["width"] - camera.height = cam["height"] - cameras.append(camera) + cameras = spt.convert_cameras(cams) # Run triangulation triangulator = spt.Triangulator(min_score=0.95) @@ -78,6 +68,27 @@ def main(): print("3D time:", time.time() - stime) print(np.array(poses_3d)) + # Load input data + roomparams = [[0, -0.5, 1.2], [5.6, 6.4, 2.4]] + cpath = "/SimplePoseTriangulation/data/p1/sample.json" + ppath = "/SimplePoseTriangulation/tests/poses_p1.json" + with open(cpath, "r") as file: + cdata = json.load(file) + with open(ppath, "r") as file: + pdata = json.load(file) + cams = cdata["cameras"] + poses_2d = pdata["2D"] + cameras = spt.convert_cameras(cams) + + # Run triangulation + triangulator.reset() + stime = time.time() + poses_3d = triangulator.triangulate_poses( + poses_2d, cameras, roomparams, joint_names + ) + print("3D time:", time.time() - stime) + print(np.array(poses_3d)) + # ==================================================================================================