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

Refactor triangulation stages and camera model

Browse Source
This commit is contained in:
crosstyan
2026-03-11 23:42:07 +08:00
parent 24f74c87f1
commit 103aeb5887
10 changed files with 1268 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rpt/camera.cpp
+97 -78
View File
@@ -1,11 +1,60 @@
#include <array>
#include <cmath>
#include <iomanip>
#include <stdexcept>
#include <sstream>
#include <vector>
#include <cmath>
#include "camera.hpp"
#include "cached_camera.hpp"
namespace
{
std::array<std::array<float, 3>, 3> transpose3x3(const std::array<std::array<float, 3>, 3> &M)
{
return {{{M[0][0], M[1][0], M[2][0]},
{M[0][1], M[1][1], M[2][1]},
{M[0][2], M[1][2], M[2][2]}}};
}
std::array<std::array<float, 3>, 3> invert3x3(const std::array<std::array<float, 3>, 3> &M)
{
std::array<std::array<float, 3>, 3> adj = {
{{
M[1][1] * M[2][2] - M[1][2] * M[2][1],
M[0][2] * M[2][1] - M[0][1] * M[2][2],
M[0][1] * M[1][2] - M[0][2] * M[1][1],
},
{
M[1][2] * M[2][0] - M[1][0] * M[2][2],
M[0][0] * M[2][2] - M[0][2] * M[2][0],
M[0][2] * M[1][0] - M[0][0] * M[1][2],
},
{
M[1][0] * M[2][1] - M[1][1] * M[2][0],
M[0][1] * M[2][0] - M[0][0] * M[2][1],
M[0][0] * M[1][1] - M[0][1] * M[1][0],
}}};
float det = M[0][0] * adj[0][0] + M[0][1] * adj[1][0] + M[0][2] * adj[2][0];
if (std::fabs(det) < 1e-6f)
{
return {{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}};
}
float idet = 1.0f / det;
return {{
{{adj[0][0] * idet, adj[0][1] * idet, adj[0][2] * idet}},
{{adj[1][0] * idet, adj[1][1] * idet, adj[1][2] * idet}},
{{adj[2][0] * idet, adj[2][1] * idet, adj[2][2] * idet}},
}};
}
std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_fisheye(
const Camera &cam, float balance, std::pair<int, int> new_size);
std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_pinhole(
const Camera &cam, float alpha, std::pair<int, int> new_size);
} // namespace
// =================================================================================================
// =================================================================================================
@@ -63,7 +112,7 @@ std::string Camera::to_string() const
out << "'width': " << width << ", ";
out << "'height': " << height << ", ";
out << "'type': " << type;
out << "'model': '" << camera_model_name(model) << "'";
out << "}";
return out.str();
@@ -71,6 +120,33 @@ std::string Camera::to_string() const
// =================================================================================================
const char *camera_model_name(CameraModel model)
{
switch (model)
{
case CameraModel::Pinhole:
return "pinhole";
case CameraModel::Fisheye:
return "fisheye";
}
return "unknown";
}
CameraModel parse_camera_model(const std::string &value)
{
if (value == "pinhole")
{
return CameraModel::Pinhole;
}
if (value == "fisheye")
{
return CameraModel::Fisheye;
}
throw std::invalid_argument("Unsupported camera model: " + value);
}
// =================================================================================================
std::ostream &operator<<(std::ostream &out, const Camera &cam)
{
out << cam.to_string();
@@ -80,93 +156,33 @@ std::ostream &operator<<(std::ostream &out, const Camera &cam)
// =================================================================================================
// =================================================================================================
CameraInternal::CameraInternal(const Camera &cam)
CachedCamera cache_camera(const Camera &cam)
{
this->cam = cam;
this->invR = transpose3x3(cam.R);
const std::array<std::array<float, 3>, 3> invR = transpose3x3(cam.R);
// Camera center:
// C = -(Rᵀ * t) = -(Rᵀ * (R * (T * -1))) = -(Rᵀ * (R * -T)) = -(Rᵀ * -R * T) = -(-T) = T
this->center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]};
const std::array<float, 3> center = {cam.T[0][0], cam.T[1][0], cam.T[2][0]};
// Undistort camera matrix
// As with the undistortion, the own implementation avoids some overhead compared to OpenCV
if (cam.type == "fisheye")
std::array<std::array<float, 3>, 3> newK;
if (cam.model == CameraModel::Fisheye)
{
newK = calc_optimal_camera_matrix_fisheye(1.0, {cam.width, cam.height});
newK = calc_optimal_camera_matrix_fisheye(cam, 1.0f, {cam.width, cam.height});
}
else
{
newK = calc_optimal_camera_matrix_pinhole(1.0, {cam.width, cam.height});
newK = calc_optimal_camera_matrix_pinhole(cam, 1.0f, {cam.width, cam.height});
}
this->invK = invert3x3(newK);
const std::array<std::array<float, 3>, 3> invK = invert3x3(newK);
return CachedCamera {cam, invR, center, newK, invK};
}
// =================================================================================================
std::array<std::array<float, 3>, 3> CameraInternal::transpose3x3(
const std::array<std::array<float, 3>, 3> &M)
{
return {{{M[0][0], M[1][0], M[2][0]},
{M[0][1], M[1][1], M[2][1]},
{M[0][2], M[1][2], M[2][2]}}};
}
// =================================================================================================
std::array<std::array<float, 3>, 3> CameraInternal::invert3x3(
const std::array<std::array<float, 3>, 3> &M)
{
// Compute the inverse using the adjugate method
// See: https://scicomp.stackexchange.com/a/29206
std::array<std::array<float, 3>, 3> adj = {
{{
M[1][1] * M[2][2] - M[1][2] * M[2][1],
M[0][2] * M[2][1] - M[0][1] * M[2][2],
M[0][1] * M[1][2] - M[0][2] * M[1][1],
},
{
M[1][2] * M[2][0] - M[1][0] * M[2][2],
M[0][0] * M[2][2] - M[0][2] * M[2][0],
M[0][2] * M[1][0] - M[0][0] * M[1][2],
},
{
M[1][0] * M[2][1] - M[1][1] * M[2][0],
M[0][1] * M[2][0] - M[0][0] * M[2][1],
M[0][0] * M[1][1] - M[0][1] * M[1][0],
}}};
float det = M[0][0] * adj[0][0] + M[0][1] * adj[1][0] + M[0][2] * adj[2][0];
if (std::fabs(det) < 1e-6f)
{
return {{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}};
}
float idet = 1.0f / det;
std::array<std::array<float, 3>, 3> inv = {
{{
adj[0][0] * idet,
adj[0][1] * idet,
adj[0][2] * idet,
},
{
adj[1][0] * idet,
adj[1][1] * idet,
adj[1][2] * idet,
},
{
adj[2][0] * idet,
adj[2][1] * idet,
adj[2][2] * idet,
}}};
return inv;
}
// =================================================================================================
void CameraInternal::undistort_point_pinhole(std::array<float, 3> &p, const std::vector<float> &k)
void undistort_point_pinhole(std::array<float, 3> &p, const std::array<float, 5> &k)
{
// Following: cv::cvUndistortPointsInternal
// Uses only the distortion coefficients: [k1, k2, p1, p2, k3]
@@ -202,7 +218,7 @@ void CameraInternal::undistort_point_pinhole(std::array<float, 3> &p, const std:
// =================================================================================================
void CameraInternal::undistort_point_fisheye(std::array<float, 3> &p, const std::vector<float> &k)
void undistort_point_fisheye(std::array<float, 3> &p, const std::array<float, 5> &k)
{
// Following: cv::fisheye::undistortPoints
// Uses only the distortion coefficients: [k1, k2, k3, k4]
@@ -250,8 +266,10 @@ void CameraInternal::undistort_point_fisheye(std::array<float, 3> &p, const std:
// =================================================================================================
std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_fisheye(
float balance, std::pair<int, int> new_size)
namespace
{
std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_fisheye(
const Camera &cam, float balance, std::pair<int, int> new_size)
{
// Following: cv::fisheye::estimateNewCameraMatrixForUndistortRectify
// https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/fisheye.cpp#L630
@@ -355,8 +373,8 @@ std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_f
// =================================================================================================
std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_pinhole(
float alpha, std::pair<int, int> new_size)
std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_pinhole(
const Camera &cam, float alpha, std::pair<int, int> new_size)
{
// Following: cv::getOptimalNewCameraMatrix
// https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/calibration_base.cpp#L1565
@@ -479,3 +497,4 @@ std::array<std::array<float, 3>, 3> CameraInternal::calc_optimal_camera_matrix_p
{0.0, 0.0, 1.0}}};
return newK;
}
} // namespace