crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented fisheye camera triangulation.

Browse Source
This commit is contained in:
Daniel
2024-10-08 15:11:38 +02:00
parent 3318afc3f1
commit 99aa3bb301
15 changed files with 2979 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
data/e1/README.md Normal file
View File

@ -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
```

BIN
data/e1/c1_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 315 KiB

BIN
data/e1/c2_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 298 KiB

BIN
data/e1/c3_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 391 KiB

BIN
data/e1/c4_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 302 KiB

BIN
data/e1/c5_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 316 KiB

BIN
data/e1/c6_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 491 KiB

BIN
data/e1/c7_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 257 KiB

BIN
data/e1/c8_00002.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 332 KiB

483
data/e1/sample.json Normal file
View File

@ -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
]
]
}
]
}

2
scripts/test_triangulate.py
View File

@ -295,7 +295,7 @@ def main():
if not os.path.isdir(dirpath): if not os.path.isdir(dirpath):
continue 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 continue
# Load sample infos # Load sample infos

37
spt/triangulator.cpp
View File

@ -605,8 +605,18 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
int height = icam.cam.height; int height = icam.cam.height;
// Undistort camera matrix // Undistort camera matrix
cv::Mat newK = cv::getOptimalNewCameraMatrix( 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)); icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height));
}
for (size_t p = 0; p < poses.size(); ++p) for (size_t p = 0; p < poses.size(); ++p)
{ {
@ -615,7 +625,14 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
points = points.reshape(2); points = points.reshape(2);
// Undistort the points // Undistort the points
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); cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
}
// Update the original poses with the undistorted points // Update the original poses with the undistorted points
points = points.reshape(1); points = points.reshape(1);
@ -642,8 +659,15 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraIn
// Update the camera matrix // Update the camera matrix
icam.K = newK.clone(); icam.K = newK.clone();
if (icam.cam.type == "fisheye")
{
icam.DC = cv::Mat::zeros(4, 1, CV_32F);
}
else
{
icam.DC = cv::Mat::zeros(5, 1, CV_32F); icam.DC = cv::Mat::zeros(5, 1, CV_32F);
} }
}
// ================================================================================================= // =================================================================================================
@ -696,16 +720,11 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
} }
// Project points to image plane // Project points to image plane
// Since images are already undistorted, use the standard projection for all camera types
cv::Mat uv; cv::Mat uv;
if (icam.cam.type == "fisheye")
{
}
else
{
cv::Mat DCc = icam.DC.rowRange(0, 5);
cv::projectPoints( cv::projectPoints(
xyz, cv::Mat::zeros(3, 1, CV_32F), cv::Mat::zeros(3, 1, CV_32F), icam.K, DCc, uv); 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}); uv = uv.reshape(1, {xyz.rows, 2});
// Add scores again // Add scores again

1
swig/spt.i
View File

@ -64,6 +64,7 @@ def convert_cameras(cameras):
camera.T = cam["T"] camera.T = cam["T"]
camera.width = cam["width"] camera.width = cam["width"]
camera.height = cam["height"] camera.height = cam["height"]
camera.type = cam.get("type", "pinhole")
c_cameras.append(camera) c_cameras.append(camera)
return c_cameras return c_cameras
%} %}

2428
tests/poses_e1.json Normal file
View File

File diff suppressed because it is too large Load Diff

22
tests/test_interface.py
View File

@ -100,6 +100,28 @@ def main():
print(np.array(poses_3d)) print(np.array(poses_3d))
print("") 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() triangulator.print_stats()
print("") print("")