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

45
spt/triangulator.cpp
View File

@ -605,8 +605,18 @@ void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &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<cv::Mat> &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<cv::Mat> &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<cv::Mat>, std::vector<cv::Mat>> 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