diff --git a/data/e1/README.md b/data/e1/README.md new file mode 100644 index 0000000..52f5baa --- /dev/null +++ b/data/e1/README.md @@ -0,0 +1,12 @@ +Image source: + +```bash +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam01/images/00002.jpg' ./c1_00002.jpg +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam02/images/00002.jpg' ./c2_00002.jpg +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam03/images/00002.jpg' ./c3_00002.jpg +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam04/images/00002.jpg' ./c4_00002.jpg +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam05/images/00002.jpg' ./c5_00002.jpg +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam06/images/00002.jpg' ./c6_00002.jpg +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam07/images/00002.jpg' ./c7_00002.jpg +cp '/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam08/images/00002.jpg' ./c8_00002.jpg +``` diff --git a/data/e1/c1_00002.jpg b/data/e1/c1_00002.jpg new file mode 100644 index 0000000..f070319 Binary files /dev/null and b/data/e1/c1_00002.jpg differ diff --git a/data/e1/c2_00002.jpg b/data/e1/c2_00002.jpg new file mode 100644 index 0000000..1e8cbe3 Binary files /dev/null and b/data/e1/c2_00002.jpg differ diff --git a/data/e1/c3_00002.jpg b/data/e1/c3_00002.jpg new file mode 100644 index 0000000..809f0fe Binary files /dev/null and b/data/e1/c3_00002.jpg differ diff --git a/data/e1/c4_00002.jpg b/data/e1/c4_00002.jpg new file mode 100644 index 0000000..2cc4922 Binary files /dev/null and b/data/e1/c4_00002.jpg differ diff --git a/data/e1/c5_00002.jpg b/data/e1/c5_00002.jpg new file mode 100644 index 0000000..5c0279f Binary files /dev/null and b/data/e1/c5_00002.jpg differ diff --git a/data/e1/c6_00002.jpg b/data/e1/c6_00002.jpg new file mode 100644 index 0000000..c0cff52 Binary files /dev/null and b/data/e1/c6_00002.jpg differ diff --git a/data/e1/c7_00002.jpg b/data/e1/c7_00002.jpg new file mode 100644 index 0000000..aa92543 Binary files /dev/null and b/data/e1/c7_00002.jpg differ diff --git a/data/e1/c8_00002.jpg b/data/e1/c8_00002.jpg new file mode 100644 index 0000000..9b7d933 Binary files /dev/null and b/data/e1/c8_00002.jpg differ diff --git a/data/e1/sample.json b/data/e1/sample.json new file mode 100644 index 0000000..3a60167 --- /dev/null +++ b/data/e1/sample.json @@ -0,0 +1,483 @@ +{ + "id": "006_legoassemble-000002", + "imgpaths": [ + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam01/images/c1_00002.jpg", + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam02/images/c2_00002.jpg", + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam03/images/c3_00002.jpg", + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam04/images/c4_00002.jpg", + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam05/images/c5_00002.jpg", + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam06/images/c6_00002.jpg", + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam07/images/c7_00002.jpg", + "/datasets/egohumans/02_legoassemble/006_legoassemble/exo/cam08/images/c8_00002.jpg" + ], + "dataset_name": "egohumans", + "room_size": [ + 6.0, + 5.0, + 2.0 + ], + "room_center": [ + 1.5, + 1.0, + -0.5 + ], + "num_persons": 3, + "cameras": [ + { + "type": "fisheye", + "name": "cam01", + "width": 3840, + "height": 2160, + "K": [ + [ + 1905.440429, + 0.0, + 1920.0 + ], + [ + 0.0, + 1908.738384, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + 0.022721, + 0.096403, + -0.122399, + 0.06457 + ], + "R": [ + [ + -1.320967, + 1.091159, + -0.053999 + ], + [ + 0.324639, + 0.311149, + -1.654174 + ], + [ + -1.043145, + -1.284934, + -0.446418 + ] + ], + "T": [ + [ + 1.938363 + ], + [ + 2.912456 + ], + [ + -0.05983 + ] + ] + }, + { + "type": "fisheye", + "name": "cam02", + "width": 3840, + "height": 2160, + "K": [ + [ + 1932.244518, + 0.0, + 1920.0 + ], + [ + 0.0, + 1936.559524, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + 0.038159, + 0.063352, + -0.103183, + 0.057205 + ], + "R": [ + [ + -0.559664, + 1.606496, + -0.210818 + ], + [ + 0.354514, + -0.096222, + -1.674383 + ], + [ + -1.58101, + -0.590262, + -0.300823 + ] + ], + "T": [ + [ + 3.363792 + ], + [ + 2.221809 + ], + [ + -0.150734 + ] + ] + }, + { + "type": "fisheye", + "name": "cam03", + "width": 3840, + "height": 2160, + "K": [ + [ + 1894.357534, + 0.0, + 1920.0 + ], + [ + 0.0, + 1895.186619, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + 0.050168, + 0.041318, + -0.141156, + 0.109365 + ], + "R": [ + [ + 1.33355, + 1.075326, + -0.06178 + ], + [ + 0.254785, + -0.410461, + -1.64472 + ], + [ + -1.046531, + 1.270312, + -0.479142 + ] + ], + "T": [ + [ + 3.350025 + ], + [ + -0.796138 + ], + [ + -0.123788 + ] + ] + }, + { + "type": "fisheye", + "name": "cam04", + "width": 3840, + "height": 2160, + "K": [ + [ + 1954.001155, + 0.0, + 1920.0 + ], + [ + 0.0, + 1961.448327, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + 0.002428, + 0.182701, + -0.217826, + 0.085325 + ], + "R": [ + [ + -1.267021, + -1.154138, + -0.033506 + ], + [ + -0.150745, + 0.214669, + -1.694016 + ], + [ + 1.144741, + -1.249152, + -0.260162 + ] + ], + "T": [ + [ + -0.017013 + ], + [ + 3.310275 + ], + [ + -0.109274 + ] + ] + }, + { + "type": "fisheye", + "name": "cam05", + "width": 3840, + "height": 2160, + "K": [ + [ + 1935.020765, + 0.0, + 1920.0 + ], + [ + 0.0, + 1932.219587, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + 0.058948, + -0.009451, + 0.031702, + -0.02819 + ], + "R": [ + [ + 1.702269, + 0.12581, + -0.157957 + ], + [ + -0.14946, + -0.116741, + -1.703681 + ], + [ + -0.135795, + 1.705591, + -0.104959 + ] + ], + "T": [ + [ + 1.404604 + ], + [ + -1.586799 + ], + [ + -0.106577 + ] + ] + }, + { + "type": "fisheye", + "name": "cam06", + "width": 3840, + "height": 2160, + "K": [ + [ + 1922.984553, + 0.0, + 1920.0 + ], + [ + 0.0, + 1913.40327, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + 0.051617, + -0.019883, + 0.131883, + -0.148917 + ], + "R": [ + [ + 0.859208, + -1.483123, + -0.024598 + ], + [ + -0.452399, + -0.234941, + -1.636654 + ], + [ + 1.412655, + 0.826829, + -0.509173 + ] + ], + "T": [ + [ + -1.069183 + ], + [ + 0.162843 + ], + [ + -0.081835 + ] + ] + }, + { + "type": "fisheye", + "name": "cam07", + "width": 3840, + "height": 2160, + "K": [ + [ + 1921.007178, + 0.0, + 1920.0 + ], + [ + 0.0, + 1922.985943, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + 0.012135, + 0.192931, + -0.334768, + 0.182519 + ], + "R": [ + [ + -0.069061, + 1.712667, + -0.022347 + ], + [ + 0.123643, + -0.017322, + -1.709652 + ], + [ + -1.708344, + -0.07049, + -0.122835 + ] + ], + "T": [ + [ + 4.379272 + ], + [ + 1.482245 + ], + [ + -0.111096 + ] + ] + }, + { + "type": "fisheye", + "name": "cam08", + "width": 3840, + "height": 2160, + "K": [ + [ + 1944.872901, + 0.0, + 1920.0 + ], + [ + 0.0, + 1952.784761, + 1080.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "DC": [ + -0.011357, + 0.225262, + -0.308, + 0.148322 + ], + "R": [ + [ + -0.32595, + -1.682675, + -0.029302 + ], + [ + -0.137145, + 0.056305, + -1.707782 + ], + [ + 1.677333, + -0.322385, + -0.145329 + ] + ], + "T": [ + [ + -1.092642 + ], + [ + 1.923018 + ], + [ + -0.068911 + ] + ] + } + ] +} \ No newline at end of file diff --git a/scripts/test_triangulate.py b/scripts/test_triangulate.py index 2bd3b55..0490d60 100644 --- a/scripts/test_triangulate.py +++ b/scripts/test_triangulate.py @@ -295,7 +295,7 @@ def main(): if not os.path.isdir(dirpath): continue - if (dirname[0] not in ["p", "h"]) or len(dirname) != 2: + if (dirname[0] not in ["p", "h", "e"]) or len(dirname) != 2: continue # Load sample infos diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp index d6aa6cd..98be43f 100644 --- a/spt/triangulator.cpp +++ b/spt/triangulator.cpp @@ -605,8 +605,18 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn int height = icam.cam.height; // Undistort camera matrix - cv::Mat newK = cv::getOptimalNewCameraMatrix( - icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); + cv::Mat newK; + if (icam.cam.type == "fisheye") + { + cv::fisheye::estimateNewCameraMatrixForUndistortRectify( + icam.K, icam.DC, cv::Size(width, height), cv::Matx33d::eye(), + newK, 1.0, cv::Size(width, height), 1.0); + } + else + { + newK = cv::getOptimalNewCameraMatrix( + icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); + } for (size_t p = 0; p < poses.size(); ++p) { @@ -615,7 +625,14 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn points = points.reshape(2); // Undistort the points - cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK); + if (icam.cam.type == "fisheye") + { + cv::fisheye::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK); + } + else + { + cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK); + } // Update the original poses with the undistorted points points = points.reshape(1); @@ -642,7 +659,14 @@ void TriangulatorInternal::undistort_poses(std::vector &poses, CameraIn // Update the camera matrix icam.K = newK.clone(); - icam.DC = cv::Mat::zeros(5, 1, CV_32F); + if (icam.cam.type == "fisheye") + { + icam.DC = cv::Mat::zeros(4, 1, CV_32F); + } + else + { + icam.DC = cv::Mat::zeros(5, 1, CV_32F); + } } // ================================================================================================= @@ -696,16 +720,11 @@ std::tuple, std::vector> TriangulatorInternal::pro } // Project points to image plane + // Since images are already undistorted, use the standard projection for all camera types cv::Mat uv; - if (icam.cam.type == "fisheye") - { - } - else - { - cv::Mat DCc = icam.DC.rowRange(0, 5); - cv::projectPoints( - xyz, cv::Mat::zeros(3, 1, CV_32F), cv::Mat::zeros(3, 1, CV_32F), icam.K, DCc, uv); - } + cv::projectPoints( + xyz, cv::Mat::zeros(3, 1, CV_32F), cv::Mat::zeros(3, 1, CV_32F), + icam.K, cv::Mat::zeros(5, 1, CV_32F), uv); uv = uv.reshape(1, {xyz.rows, 2}); // Add scores again diff --git a/swig/spt.i b/swig/spt.i index 91ea2d0..ce0f7ef 100644 --- a/swig/spt.i +++ b/swig/spt.i @@ -64,6 +64,7 @@ def convert_cameras(cameras): camera.T = cam["T"] camera.width = cam["width"] camera.height = cam["height"] + camera.type = cam.get("type", "pinhole") c_cameras.append(camera) return c_cameras %} diff --git a/tests/poses_e1.json b/tests/poses_e1.json new file mode 100644 index 0000000..cbb565e --- /dev/null +++ b/tests/poses_e1.json @@ -0,0 +1,2428 @@ +{ + "2D": [ + [ + [ + [ + 586.444519, + 401.750183, + 0.820265 + ], + [ + 586.444519, + 365.564056, + 0.695295 + ], + [ + 569.743225, + 359.996979, + 0.785734 + ], + [ + 466.751984, + 346.079224, + 0.34613 + ], + [ + 447.267151, + 334.945038, + 0.823077 + ], + [ + 516.855835, + 538.143982, + 0.637653 + ], + [ + 294.172058, + 515.87561, + 0.771036 + ], + [ + 617.063538, + 730.20874, + 0.695584 + ], + [ + 280.254333, + 799.797424, + 0.849441 + ], + [ + 586.444519, + 588.247864, + 0.735019 + ], + [ + 522.422913, + 702.373291, + 0.760872 + ], + [ + 547.474854, + 1061.450806, + 0.648139 + ], + [ + 433.349396, + 1075.36853, + 0.627626 + ], + [ + 586.444519, + 1406.610718, + 0.748141 + ], + [ + 541.907776, + 1445.580322, + 0.758235 + ], + [ + 625.414185, + 1701.666626, + 0.817159 + ], + [ + 586.444519, + 1821.359253, + 0.82306 + ], + [ + 490.412125, + 1068.409668, + 0.627626 + ], + [ + 405.513947, + 527.009796, + 0.637653 + ], + [ + 457.009567, + 340.512131, + 0.34613 + ] + ], + [ + [ + 1877.246094, + 856.544189, + 0.595382 + ], + [ + 1915.675903, + 830.924377, + 0.542184 + ], + [ + 1850.203003, + 828.077698, + 0.576842 + ], + [ + 1962.64563, + 768.298035, + 0.685699 + ], + [ + 1803.233276, + 768.298035, + 0.673036 + ], + [ + 2042.351929, + 789.647949, + 0.720335 + ], + [ + 1713.563721, + 758.334778, + 0.741636 + ], + [ + 2019.578735, + 1010.263367, + 0.705026 + ], + [ + 1693.637207, + 971.833557, + 0.740833 + ], + [ + 1951.259033, + 1173.945801, + 0.81354 + ], + [ + 1730.643677, + 1156.865845, + 0.816643 + ], + [ + 1959.799072, + 996.03009, + 0.645995 + ], + [ + 1803.233276, + 990.336792, + 0.664078 + ], + [ + 1968.338989, + 1243.688721, + 0.636642 + ], + [ + 1764.803467, + 1230.878784, + 0.624803 + ], + [ + 1964.06897, + 1494.19397, + 0.646227 + ], + [ + 1759.110107, + 1479.960693, + 0.570511 + ], + [ + 1881.516174, + 993.183441, + 0.645995 + ], + [ + 1877.957825, + 773.991364, + 0.720335 + ], + [ + 1882.939453, + 768.298035, + 0.673036 + ] + ], + [ + [ + 1164.451416, + 1106.615601, + 0.410828 + ], + [ + 1177.49353, + 1086.12085, + 0.256671 + ], + [ + 1167.246094, + 1085.189209, + 0.270091 + ], + [ + 1118.804077, + 994.826172, + 0.317784 + ], + [ + 1107.625122, + 1040.473511, + 0.465395 + ], + [ + 1127.188232, + 1046.994507, + 0.521245 + ], + [ + 1041.483032, + 1114.068237, + 0.510617 + ], + [ + 1138.367188, + 1194.18396, + 0.428227 + ], + [ + 1113.2146, + 1266.847046, + 0.625443 + ], + [ + 1151.409302, + 1270.573364, + 0.228929 + ], + [ + 1202.646118, + 1401.926025, + 0.757737 + ], + [ + 982.793579, + 1228.652344, + 0.360501 + ], + [ + 910.130493, + 1265.915527, + 0.610264 + ], + [ + 1170.040894, + 1222.131348, + 0.669607 + ], + [ + 1102.967163, + 1302.24707, + 0.605092 + ], + [ + 1085.267212, + 1414.968018, + 0.49768 + ], + [ + 1034.030396, + 1480.178589, + 0.629825 + ], + [ + 946.462036, + 1247.283936, + 0.360501 + ], + [ + 1084.335632, + 1080.531372, + 0.510617 + ], + [ + 1113.2146, + 1017.649841, + 0.317784 + ] + ] + ], + [ + [ + [ + 792.985168, + 488.730225, + 0.816858 + ], + [ + 792.985168, + 449.774933, + 0.620579 + ], + [ + 780.998901, + 446.778351, + 0.814569 + ], + [ + 646.153564, + 440.785217, + 0.486272 + ], + [ + 715.074524, + 434.792114, + 0.818813 + ], + [ + 592.215454, + 599.603027, + 0.632262 + ], + [ + 628.174194, + 593.609924, + 0.738518 + ], + [ + 676.119202, + 842.324646, + 0.507434 + ], + [ + 685.108887, + 896.262756, + 0.817609 + ], + [ + 780.998901, + 716.468994, + 0.559884 + ], + [ + 885.878601, + 815.355591, + 0.747285 + ], + [ + 673.12262, + 1177.939697, + 0.509636 + ], + [ + 757.026367, + 1177.939697, + 0.571866 + ], + [ + 816.957642, + 1585.472168, + 0.716527 + ], + [ + 978.772034, + 1576.482422, + 0.743776 + ], + [ + 909.851074, + 1948.056274, + 0.79372 + ], + [ + 1032.710205, + 2016.977173, + 0.832122 + ], + [ + 715.074493, + 1177.939697, + 0.509636 + ], + [ + 610.194824, + 596.606476, + 0.632262 + ], + [ + 680.614044, + 437.788666, + 0.486272 + ] + ], + [ + [ + 1921.291992, + 867.76062, + 0.627656 + ], + [ + 1937.811646, + 850.139526, + 0.571059 + ], + [ + 1901.468262, + 847.936951, + 0.599368 + ], + [ + 1954.331421, + 818.201416, + 0.363133 + ], + [ + 1850.807739, + 792.871094, + 0.676324 + ], + [ + 1949.926147, + 842.430359, + 0.61665 + ], + [ + 1727.460327, + 813.796143, + 0.551175 + ], + [ + 1936.710327, + 996.614563, + 0.584852 + ], + [ + 1702.130005, + 1015.336975, + 0.657905 + ], + [ + 1926.798584, + 1136.481689, + 0.640658 + ], + [ + 1783.627441, + 1149.69751, + 0.690278 + ], + [ + 1858.516968, + 1024.147461, + 0.50641 + ], + [ + 1739.574829, + 1026.350098, + 0.392596 + ], + [ + 1891.556396, + 1213.573853, + 0.518543 + ], + [ + 1705.43396, + 1214.675171, + 0.503026 + ], + [ + 1903.670898, + 1453.660767, + 0.441064 + ], + [ + 1671.293213, + 1453.660767, + 0.509325 + ], + [ + 1799.045898, + 1025.248779, + 0.392596 + ], + [ + 1838.693237, + 828.113251, + 0.551175 + ], + [ + 1902.56958, + 805.536255, + 0.363133 + ] + ] + ], + [ + [ + [ + 1584.487915, + 31.315681, + 0.574659 + ], + [ + 1498.523315, + 80.438278, + 0.259805 + ], + [ + 1599.838745, + 65.087471, + 0.260052 + ], + [ + 1492.383057, + 58.947144, + 0.715421 + ], + [ + 1676.592773, + 46.666492, + 0.60861 + ], + [ + 1409.488647, + 221.665756, + 0.759999 + ], + [ + 1876.15332, + 206.314941, + 0.723209 + ], + [ + 1323.524048, + 593.155396, + 0.759023 + ], + [ + 1986.679199, + 550.173157, + 0.791953 + ], + [ + 1483.172485, + 488.769897, + 0.48789 + ], + [ + 1790.188721, + 519.471497, + 0.535276 + ], + [ + 1618.259644, + 915.522461, + 0.595968 + ], + [ + 1882.293701, + 887.890991, + 0.596845 + ], + [ + 1682.733032, + 1376.046875, + 0.757403 + ], + [ + 1946.76709, + 1277.801636, + 0.758871 + ], + [ + 1750.276611, + 1778.238159, + 0.842891 + ], + [ + 2020.450928, + 1627.800171, + 0.798013 + ], + [ + 1750.276672, + 901.706726, + 0.595968 + ], + [ + 1642.820984, + 213.990349, + 0.723209 + ], + [ + 1584.487915, + 52.806818, + 0.60861 + ] + ], + [ + [ + 1459.141724, + 648.495544, + 0.538235 + ], + [ + 1453.557007, + 624.760315, + 0.463931 + ], + [ + 1453.557007, + 644.306946, + 0.55307 + ], + [ + 1429.821777, + 621.967957, + 0.438082 + ], + [ + 1418.652222, + 610.798401, + 0.657799 + ], + [ + 1362.804565, + 673.626953, + 0.639155 + ], + [ + 1325.107422, + 649.891724, + 0.674553 + ], + [ + 1326.503662, + 823.019287, + 0.575675 + ], + [ + 1295.787476, + 838.37738, + 0.736937 + ], + [ + 1371.181763, + 936.110718, + 0.364674 + ], + [ + 1380.955078, + 950.072632, + 0.776394 + ], + [ + 1238.543701, + 870.489807, + 0.566664 + ], + [ + 1154.772217, + 864.905029, + 0.613551 + ], + [ + 1288.806519, + 1039.428833, + 0.706421 + ], + [ + 1158.960815, + 1084.106934, + 0.710281 + ], + [ + 1291.598877, + 1239.083984, + 0.760791 + ], + [ + 1135.225586, + 1307.497314, + 0.745805 + ], + [ + 1196.657959, + 867.697418, + 0.566664 + ], + [ + 1343.955994, + 661.759338, + 0.639155 + ], + [ + 1424.237, + 616.383179, + 0.438082 + ] + ], + [ + [ + 1043.253296, + 1044.014282, + 0.474804 + ], + [ + 1071.034302, + 1019.157593, + 0.201969 + ], + [ + 1027.169556, + 1023.544067, + 0.474133 + ], + [ + 889.726868, + 1030.854858, + 0.652043 + ], + [ + 983.304871, + 1033.779175, + 0.623944 + ], + [ + 790.300232, + 1140.516602, + 0.751941 + ], + [ + 1053.488403, + 1159.524536, + 0.703813 + ], + [ + 810.770447, + 1323.286133, + 0.400958 + ], + [ + 1136.831299, + 1343.756226, + 0.656609 + ], + [ + 936.515869, + 1329.134766, + 0.27971 + ], + [ + 1155.839355, + 1476.8125, + 0.679278 + ], + [ + 803.459656, + 1577.701294, + 0.66247 + ], + [ + 956.986084, + 1566.004028, + 0.552271 + ], + [ + 828.316345, + 1399.318237, + 0.325944 + ], + [ + 1097.353027, + 1389.08313, + 0.654764 + ], + [ + 866.332397, + 1766.319458, + 0.767627 + ], + [ + 1078.345093, + 1720.992554, + 0.806243 + ], + [ + 880.22287, + 1571.852661, + 0.552271 + ], + [ + 921.894318, + 1150.020569, + 0.703813 + ], + [ + 936.515869, + 1032.317017, + 0.623944 + ] + ] + ], + [ + [ + [ + 2762.009766, + 988.037231, + 0.69962 + ], + [ + 2762.009766, + 961.369324, + 0.684523 + ], + [ + 2739.151611, + 968.988708, + 0.692346 + ], + [ + 2826.774658, + 890.889893, + 0.556459 + ], + [ + 2729.627197, + 860.412292, + 0.334766 + ], + [ + 3032.498291, + 946.130554, + 0.680876 + ], + [ + 2836.298828, + 957.559631, + 0.607035 + ], + [ + 3068.69043, + 1241.382202, + 0.726923 + ], + [ + 2828.679443, + 1149.949463, + 0.689518 + ], + [ + 2901.063721, + 1407.104126, + 0.678151 + ], + [ + 2763.914551, + 1321.385864, + 0.747111 + ], + [ + 3196.31543, + 1182.331909, + 0.586568 + ], + [ + 3055.356445, + 1186.141602, + 0.521409 + ], + [ + 3038.212891, + 1464.249512, + 0.673856 + ], + [ + 3002.020752, + 1437.581665, + 0.630603 + ], + [ + 3021.069336, + 1824.266113, + 0.760358 + ], + [ + 2986.781982, + 1677.592651, + 0.695419 + ], + [ + 3125.835938, + 1184.236755, + 0.521409 + ], + [ + 2934.39856, + 951.845093, + 0.607035 + ], + [ + 2778.200928, + 875.651093, + 0.334766 + ] + ], + [ + [ + 2182.750488, + 595.482788, + 0.571952 + ], + [ + 2205.417236, + 577.673218, + 0.579801 + ], + [ + 2155.226807, + 577.673218, + 0.551522 + ], + [ + 2226.464844, + 537.19696, + 0.614739 + ], + [ + 2111.512451, + 525.863586, + 0.704306 + ], + [ + 2242.655518, + 632.720947, + 0.646608 + ], + [ + 2032.178833, + 624.625671, + 0.643798 + ], + [ + 2302.560303, + 781.673584, + 0.780493 + ], + [ + 1972.273926, + 788.14978, + 0.75168 + ], + [ + 2226.464844, + 666.721008, + 0.745683 + ], + [ + 2093.702881, + 752.530701, + 0.617065 + ], + [ + 2187.607666, + 953.292969, + 0.602368 + ], + [ + 2058.08374, + 940.340576, + 0.584836 + ], + [ + 2163.322021, + 1212.341064, + 0.730568 + ], + [ + 2022.4646, + 1210.722046, + 0.712647 + ], + [ + 2135.798096, + 1430.912964, + 0.667071 + ], + [ + 1973.893066, + 1432.531982, + 0.718494 + ], + [ + 2122.845703, + 946.816772, + 0.584836 + ], + [ + 2137.417175, + 628.673309, + 0.643798 + ], + [ + 2168.988647, + 531.530273, + 0.614739 + ] + ], + [ + [ + 3797.242676, + 705.084412, + 0.315929 + ], + [ + 3786.542725, + 699.734436, + 0.28284 + ], + [ + 3793.676025, + 701.517761, + 0.334861 + ], + [ + 3711.642822, + 697.951111, + 0.340671 + ], + [ + 3765.142822, + 701.517761, + 0.34888 + ], + [ + 3827.559082, + 888.767212, + 0.265411 + ], + [ + 3816.859131, + 715.784424, + 0.268588 + ], + [ + 3816.859131, + 888.767212, + 0.235078 + ], + [ + 3759.792725, + 976.15033, + 0.23254 + ], + [ + 3754.442627, + 1054.616699, + 0.254789 + ], + [ + 3752.659424, + 1058.18335, + 0.261917 + ], + [ + 3834.692383, + 1090.283325, + 0.289005 + ], + [ + 3831.125732, + 1086.716675, + 0.282356 + ], + [ + 3820.425781, + 1297.149292, + 0.278726 + ], + [ + 3820.425781, + 1295.365967, + 0.269026 + ], + [ + 3804.375977, + 1475.482178, + 0.415941 + ], + [ + 3800.809326, + 1477.265503, + 0.432785 + ], + [ + 3832.909058, + 1088.5, + 0.282356 + ], + [ + 3822.209106, + 802.275818, + 0.265411 + ], + [ + 3738.392822, + 699.734436, + 0.340671 + ] + ] + ], + [ + [ + [ + 2503.178711, + 511.942261, + 0.869774 + ], + [ + 2509.148193, + 476.124695, + 0.906625 + ], + [ + 2497.208984, + 482.094269, + 0.779866 + ], + [ + 2583.768066, + 455.23111, + 0.768626 + ], + [ + 2658.387939, + 440.307129, + 0.476695 + ], + [ + 2706.144775, + 583.577393, + 0.784576 + ], + [ + 2733.008057, + 574.622986, + 0.637316 + ], + [ + 2613.615967, + 905.935364, + 0.86065 + ], + [ + 2712.114258, + 870.117798, + 0.512528 + ], + [ + 2547.950439, + 685.060425, + 0.777839 + ], + [ + 2559.889648, + 831.315491, + 0.608923 + ], + [ + 2768.825439, + 1207.39978, + 0.706393 + ], + [ + 2753.901611, + 1204.415039, + 0.591266 + ], + [ + 2756.88623, + 1628.256104, + 0.792423 + ], + [ + 2738.977539, + 1580.49939, + 0.716203 + ], + [ + 2783.749512, + 2028.218872, + 0.904036 + ], + [ + 2762.855957, + 1840.176758, + 0.778414 + ], + [ + 2761.363525, + 1205.90741, + 0.591266 + ], + [ + 2719.576416, + 579.100189, + 0.637316 + ], + [ + 2621.078003, + 447.769119, + 0.476695 + ] + ], + [ + [ + 1580.184814, + 1136.080322, + 0.864181 + ], + [ + 1592.906494, + 1121.54126, + 0.663278 + ], + [ + 1592.906494, + 1110.637085, + 0.778013 + ], + [ + 1545.654663, + 1063.385254, + 0.462362 + ], + [ + 1571.0979, + 1068.837402, + 0.856699 + ], + [ + 1422.072998, + 1072.472168, + 0.521392 + ], + [ + 1525.663452, + 1108.819702, + 0.702077 + ], + [ + 1362.099487, + 1325.087524, + 0.565161 + ], + [ + 1523.846069, + 1325.087524, + 0.727009 + ], + [ + 1471.142212, + 1474.112549, + 0.673864 + ], + [ + 1601.993408, + 1470.477783, + 0.741698 + ], + [ + 1242.152588, + 1346.896118, + 0.458093 + ], + [ + 1271.230713, + 1354.165649, + 0.537714 + ], + [ + 1391.177612, + 1594.059448, + 0.622757 + ], + [ + 1358.464844, + 1650.398071, + 0.567126 + ], + [ + 1394.812378, + 1855.761719, + 0.700344 + ], + [ + 1373.003784, + 1999.334473, + 0.726287 + ], + [ + 1256.69165, + 1350.530884, + 0.458093 + ], + [ + 1473.868225, + 1090.645935, + 0.521392 + ], + [ + 1558.376282, + 1066.111328, + 0.462362 + ] + ] + ], + [ + [ + [ + 2754.70874, + 280.479126, + 0.588498 + ], + [ + 2782.545898, + 246.676743, + 0.478748 + ], + [ + 2740.790039, + 244.68837, + 0.58808 + ], + [ + 2862.080811, + 236.734879, + 0.655905 + ], + [ + 2730.848145, + 242.699997, + 0.490965 + ], + [ + 2953.546143, + 360.01413, + 0.815348 + ], + [ + 2712.952881, + 358.025757, + 0.792185 + ], + [ + 2969.453125, + 548.90979, + 0.859944 + ], + [ + 2651.313232, + 531.014404, + 0.719971 + ], + [ + 2842.197266, + 401.77002, + 0.837074 + ], + [ + 2651.313232, + 509.142273, + 0.769513 + ], + [ + 2826.290039, + 791.491577, + 0.753916 + ], + [ + 2687.104004, + 765.6427, + 0.755325 + ], + [ + 2802.429688, + 1051.96875, + 0.808718 + ], + [ + 2607.568848, + 1004.247681, + 0.807413 + ], + [ + 2770.615723, + 1274.666748, + 0.921249 + ], + [ + 2573.766602, + 1226.945679, + 0.810232 + ], + [ + 2756.697021, + 778.567139, + 0.753916 + ], + [ + 2833.249512, + 359.019943, + 0.792185 + ], + [ + 2796.464478, + 239.717438, + 0.490965 + ] + ], + [ + [ + 2951.904785, + 1098.442749, + 0.50479 + ], + [ + 2972.686035, + 1076.276123, + 0.465497 + ], + [ + 2954.675537, + 1065.192871, + 0.469321 + ], + [ + 3059.966797, + 1061.036621, + 0.500758 + ], + [ + 3032.258545, + 1058.265869, + 0.302203 + ], + [ + 3217.903564, + 1170.484131, + 0.642095 + ], + [ + 2982.383789, + 1102.598999, + 0.635163 + ], + [ + 3136.164307, + 1393.535278, + 0.687385 + ], + [ + 2895.103027, + 1286.858643, + 0.668362 + ], + [ + 3048.883545, + 1577.794922, + 0.791681 + ], + [ + 2823.061523, + 1497.44104, + 0.769008 + ], + [ + 3273.320068, + 1310.410645, + 0.692645 + ], + [ + 3123.695801, + 1285.473145, + 0.581978 + ], + [ + 3202.664062, + 1460.034912, + 0.703093 + ], + [ + 2956.061035, + 1342.275024, + 0.716519 + ], + [ + 3190.195557, + 1641.523804, + 0.784109 + ], + [ + 3010.092041, + 1568.097046, + 0.75861 + ], + [ + 3198.507935, + 1297.941895, + 0.581978 + ], + [ + 3100.143677, + 1136.541565, + 0.635163 + ], + [ + 3046.112671, + 1059.651245, + 0.302203 + ] + ], + [ + [ + 1798.739258, + 589.240417, + 0.401728 + ], + [ + 1763.102295, + 622.501587, + 0.172923 + ], + [ + 1991.179077, + 617.75, + 0.149252 + ], + [ + 1744.095825, + 617.75, + 0.678968 + ], + [ + 1898.522827, + 563.106628, + 0.657587 + ], + [ + 1708.458862, + 684.2724, + 0.771762 + ], + [ + 2083.835205, + 586.864624, + 0.644311 + ], + [ + 1732.216919, + 990.75061, + 0.609181 + ], + [ + 2140.854492, + 841.075195, + 0.496792 + ], + [ + 1782.108643, + 1138.050293, + 0.500588 + ], + [ + 2019.688721, + 1069.1521, + 0.281276 + ], + [ + 1898.522827, + 1152.305054, + 0.611441 + ], + [ + 2178.867188, + 1126.171265, + 0.59388 + ], + [ + 1893.77124, + 1468.286499, + 0.729235 + ], + [ + 2257.268799, + 1496.796021, + 0.729569 + ], + [ + 1910.401855, + 1803.274292, + 0.796372 + ], + [ + 2349.924805, + 1853.166138, + 0.779442 + ], + [ + 2038.695007, + 1139.238159, + 0.59388 + ], + [ + 1896.147034, + 635.568512, + 0.644311 + ], + [ + 1821.309326, + 590.428314, + 0.657587 + ] + ] + ], + [ + [ + [ + 1257.799316, + 648.234009, + 0.738 + ], + [ + 1260.217896, + 619.211243, + 0.453109 + ], + [ + 1262.636475, + 621.629822, + 0.683133 + ], + [ + 1119.941162, + 602.281311, + 0.679932 + ], + [ + 1228.776611, + 609.536987, + 0.760169 + ], + [ + 999.013062, + 744.976562, + 0.762772 + ], + [ + 1211.846558, + 723.209473, + 0.730877 + ], + [ + 1047.384277, + 960.228699, + 0.629692 + ], + [ + 1274.729248, + 969.902954, + 0.815546 + ], + [ + 1216.683716, + 916.694519, + 0.454837 + ], + [ + 1347.286133, + 923.950195, + 0.71598 + ], + [ + 1078.825684, + 1238.363403, + 0.632746 + ], + [ + 1236.032227, + 1231.107788, + 0.600578 + ], + [ + 1136.871094, + 1572.125244, + 0.760609 + ], + [ + 1364.216064, + 1552.776733, + 0.764956 + ], + [ + 1168.3125, + 1884.119873, + 0.868725 + ], + [ + 1395.657471, + 1910.724121, + 0.85819 + ], + [ + 1157.428955, + 1234.735596, + 0.600578 + ], + [ + 1105.42981, + 734.093018, + 0.730877 + ], + [ + 1174.358887, + 605.909149, + 0.679932 + ] + ], + [ + [ + 1883.21521, + 1061.810303, + 0.614467 + ], + [ + 1894.380737, + 1053.221436, + 0.492153 + ], + [ + 1878.920776, + 1049.785889, + 0.500141 + ], + [ + 1867.755249, + 1018.865906, + 0.330961 + ], + [ + 1839.411987, + 997.393738, + 0.67983 + ], + [ + 1863.460815, + 1058.374756, + 0.647193 + ], + [ + 1726.03894, + 1000.829285, + 0.574793 + ], + [ + 1838.553101, + 1172.606689, + 0.588403 + ], + [ + 1685.671265, + 1164.876709, + 0.675856 + ], + [ + 1835.117554, + 1279.967529, + 0.625191 + ], + [ + 1750.087769, + 1273.096436, + 0.671195 + ], + [ + 1763.829956, + 1183.772217, + 0.462728 + ], + [ + 1659.904663, + 1175.18335, + 0.514358 + ], + [ + 1807.633179, + 1334.936279, + 0.468959 + ], + [ + 1629.843628, + 1343.525146, + 0.565994 + ], + [ + 1805.915405, + 1522.173706, + 0.330793 + ], + [ + 1582.604736, + 1522.173706, + 0.559553 + ], + [ + 1711.86731, + 1179.477783, + 0.462728 + ], + [ + 1794.749878, + 1029.60202, + 0.574793 + ], + [ + 1853.583618, + 1008.129822, + 0.330961 + ] + ] + ], + [ + [ + [ + 2256.791504, + 1176.280029, + 0.821269 + ], + [ + 2238.097168, + 1151.354248, + 0.753025 + ], + [ + 2233.942871, + 1159.662842, + 0.622639 + ], + [ + 2279.640137, + 1072.422729, + 0.866536 + ], + [ + 2325.337402, + 1049.574219, + 0.553767 + ], + [ + 2425.040283, + 1163.817139, + 0.732518 + ], + [ + 2497.740479, + 1022.571289, + 0.570197 + ], + [ + 2535.128906, + 1496.1604, + 0.700084 + ], + [ + 2593.289062, + 1352.837402, + 0.349246 + ], + [ + 2427.117432, + 1701.797852, + 0.783497 + ], + [ + 2437.503174, + 1575.091919, + 0.499776 + ], + [ + 2769.846436, + 1465.003296, + 0.516128 + ], + [ + 2911.092285, + 1442.154663, + 0.557775 + ], + [ + 2599.520508, + 1847.197998, + 0.736352 + ], + [ + 2842.546631, + 1768.266479, + 0.730775 + ], + [ + 2609.90625, + 2160.846924, + 0.740163 + ], + [ + 2873.703613, + 2081.915527, + 0.771873 + ], + [ + 2840.46936, + 1453.578979, + 0.516128 + ], + [ + 2461.390381, + 1093.194214, + 0.570197 + ], + [ + 2302.48877, + 1060.998474, + 0.553767 + ] + ], + [ + [ + 2429.372803, + 755.027466, + 0.466354 + ], + [ + 2452.609375, + 716.853149, + 0.421846 + ], + [ + 2397.837402, + 718.512878, + 0.416725 + ], + [ + 2485.804443, + 687.807495, + 0.583437 + ], + [ + 2367.13208, + 678.678833, + 0.602918 + ], + [ + 2531.44751, + 774.114624, + 0.719233 + ], + [ + 2304.891357, + 780.753601, + 0.727789 + ], + [ + 2591.19873, + 933.450867, + 0.768496 + ], + [ + 2249.289551, + 924.322205, + 0.760328 + ], + [ + 2484.144531, + 818.098022, + 0.67916 + ], + [ + 2316.509521, + 903.575317, + 0.698838 + ], + [ + 2488.293945, + 1101.915771, + 0.504003 + ], + [ + 2353.02417, + 1101.915771, + 0.514464 + ], + [ + 2678.335693, + 1101.915771, + 0.308861 + ], + [ + 2201.156738, + 1101.915771, + 0.3035 + ], + [ + 2504.061523, + 1101.915771, + 0.089704 + ], + [ + 2201.156738, + 1101.915771, + 0.102631 + ], + [ + 2420.659058, + 1101.915771, + 0.504003 + ], + [ + 2418.169434, + 777.434113, + 0.719233 + ], + [ + 2426.468262, + 683.243164, + 0.583437 + ] + ] + ] + ], + "3D": [ + [ + [ + 1.929, + 0.46, + 0.292, + 0.939 + ], + [ + 1.921, + 0.426, + 0.34, + 0.974 + ], + [ + 1.946, + 0.496, + 0.345, + 0.973 + ], + [ + 1.982, + 0.323, + 0.353, + 0.976 + ], + [ + 2.056, + 0.497, + 0.351, + 0.983 + ], + [ + 2.017, + 0.186, + 0.172, + 0.989 + ], + [ + 2.224, + 0.556, + 0.176, + 0.985 + ], + [ + 1.911, + 0.146, + -0.12, + 0.971 + ], + [ + 2.254, + 0.657, + -0.124, + 0.982 + ], + [ + 1.888, + 0.347, + 0.074, + 0.972 + ], + [ + 2.007, + 0.647, + -0.071, + 0.993 + ], + [ + 2.087, + 0.282, + -0.444, + 0.965 + ], + [ + 2.171, + 0.542, + -0.436, + 0.96 + ], + [ + 2.084, + 0.347, + -0.916, + 0.968 + ], + [ + 2.138, + 0.684, + -0.88, + 0.941 + ], + [ + 2.136, + 0.346, + -1.368, + 0.991 + ], + [ + 2.227, + 0.704, + -1.328, + 0.968 + ], + [ + 2.136, + 0.424, + -0.447, + 0.977 + ], + [ + 2.113, + 0.374, + 0.174, + 0.994 + ], + [ + 2.025, + 0.405, + 0.358, + 0.971 + ] + ], + [ + [ + 0.652, + 1.257, + -0.334, + 0.981 + ], + [ + 0.642, + 1.278, + -0.311, + 0.972 + ], + [ + 0.727, + 1.254, + -0.314, + 0.995 + ], + [ + 0.568, + 1.267, + -0.23, + 0.973 + ], + [ + 0.678, + 1.176, + -0.212, + 0.993 + ], + [ + 0.372, + 1.242, + -0.294, + 0.961 + ], + [ + 0.701, + 0.987, + -0.222, + 0.948 + ], + [ + 0.333, + 1.145, + -0.593, + 0.972 + ], + [ + 0.676, + 0.868, + -0.539, + 0.996 + ], + [ + 0.464, + 1.145, + -0.796, + 0.982 + ], + [ + 0.695, + 1.005, + -0.76, + 0.994 + ], + [ + 0.272, + 0.969, + -0.633, + 0.994 + ], + [ + 0.392, + 0.76, + -0.605, + 0.979 + ], + [ + 0.352, + 1.011, + -0.956, + 0.995 + ], + [ + 0.42, + 0.69, + -0.947, + 0.984 + ], + [ + 0.311, + 0.986, + -1.37, + 0.986 + ], + [ + 0.426, + 0.568, + -1.355, + 0.99 + ], + [ + 0.32, + 0.869, + -0.618, + 0.991 + ], + [ + 0.527, + 1.099, + -0.269, + 0.97 + ], + [ + 0.633, + 1.214, + -0.222, + 0.975 + ] + ], + [ + [ + 1.159, + 0.145, + -0.715, + 0.915 + ], + [ + 1.175, + 0.151, + -0.687, + 0.902 + ], + [ + 1.148, + 0.14, + -0.682, + 0.919 + ], + [ + 1.121, + -0.009, + -0.623, + 0.976 + ], + [ + 1.203, + 0.049, + -0.653, + 0.949 + ], + [ + 1.102, + -0.176, + -0.728, + 0.965 + ], + [ + 1.331, + 0.076, + -0.759, + 0.989 + ], + [ + 1.084, + -0.164, + -0.979, + 0.955 + ], + [ + 1.293, + 0.15, + -1.007, + 0.987 + ], + [ + 1.138, + -0.079, + -1.111, + 0.879 + ], + [ + 1.186, + 0.189, + -1.249, + 0.984 + ], + [ + 1.431, + -0.356, + -1.043, + 0.963 + ], + [ + 1.549, + -0.196, + -1.053, + 0.958 + ], + [ + 1.072, + -0.21, + -1.057, + 0.958 + ], + [ + 1.295, + 0.076, + -1.066, + 0.974 + ], + [ + 1.272, + -0.321, + -1.419, + 0.985 + ], + [ + 1.411, + -0.083, + -1.414, + 0.985 + ], + [ + 1.492, + -0.277, + -1.048, + 0.965 + ], + [ + 1.223, + -0.054, + -0.745, + 0.972 + ], + [ + 1.162, + 0.02, + -0.638, + 0.968 + ] + ] + ] +} \ No newline at end of file diff --git a/tests/test_interface.py b/tests/test_interface.py index b7fcb77..631e47c 100644 --- a/tests/test_interface.py +++ b/tests/test_interface.py @@ -100,6 +100,28 @@ def main(): print(np.array(poses_3d)) print("") + # Load input data + roomparams = [[6.0, 5.0, 2.0], [1.5, 1.0, -0.5]] + cpath = "/SimplePoseTriangulation/data/e1/sample.json" + ppath = "/SimplePoseTriangulation/tests/poses_e1.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)) + print("") + triangulator.print_stats() print("")