Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions docs/content/calibration.md
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,13 @@

retinify::CalibrationParameters supports the pinhole camera model.

- Intrinsic Parameters: retinify::Intrinsics
- Intrinsic Parameters: retinify::PinholeIntrinsics

- Focal lengths: `fx`, `fy`
- Principal point: `cx`, `cy`
- Skew coefficient: `skew`

- Distortion Coefficients: retinify::Distortion
- Distortion Coefficients: retinify::DistortionParameters

- Radial distortion coefficients: `k1`, `k2`, `k3`, `k4`, `k5`, `k6`
- Tangential distortion coefficients: `p1`, `p2`
Expand Down
34 changes: 17 additions & 17 deletions retinify/include/retinify/geometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -205,8 +205,8 @@ RETINIFY_API auto Exp(const Vec3d &vec) noexcept -> Mat3x3d;
RETINIFY_API auto Log(const Mat3x3d &mat) noexcept -> Vec3d;

/// @brief
/// Camera intrinsic parameters with focal lengths, principal point, and skew
struct Intrinsics
/// Pinhole camera intrinsic parameters with focal lengths, principal point, and skew
struct PinholeIntrinsics
{
/// @brief
/// Focal length in x (in pixels)
Expand All @@ -224,7 +224,7 @@ struct Intrinsics
/// Skew coefficient
double skew{0};

[[nodiscard]] auto operator==(const Intrinsics &other) const noexcept -> bool
[[nodiscard]] auto operator==(const PinholeIntrinsics &other) const noexcept -> bool
{
return fx == other.fx && //
fy == other.fy && //
Expand All @@ -236,7 +236,7 @@ struct Intrinsics

/// @brief
/// Rational distortion model with 8 coefficients: (k1, k2, p1, p2, k3, k4, k5, k6)
struct Distortion
struct DistortionParameters
{
double k1{0};
double k2{0};
Expand All @@ -247,7 +247,7 @@ struct Distortion
double k5{0};
double k6{0};

[[nodiscard]] auto operator==(const Distortion &other) const noexcept -> bool
[[nodiscard]] auto operator==(const DistortionParameters &other) const noexcept -> bool
{
return k1 == other.k1 && //
k2 == other.k2 && //
Expand All @@ -265,17 +265,17 @@ struct Distortion
struct CalibrationParameters
{
/// @brief
/// Intrinsics for the left camera
Intrinsics leftIntrinsics{};
/// Pinhole intrinsics for the left camera
PinholeIntrinsics leftIntrinsics{};
/// @brief
/// Distortion for the left camera
Distortion leftDistortion{};
/// Distortion parameters for the left camera
DistortionParameters leftDistortion{};
/// @brief
/// Intrinsics for the right camera
Intrinsics rightIntrinsics{};
/// Pinhole intrinsics for the right camera
PinholeIntrinsics rightIntrinsics{};
/// @brief
/// Distortion for the right camera
Distortion rightDistortion{};
/// Distortion parameters for the right camera
DistortionParameters rightDistortion{};
/// @brief
/// Rotation matrix
Mat3x3d rotation{};
Expand Down Expand Up @@ -320,7 +320,7 @@ struct CalibrationParameters
/// Distorted 2D point (in pixel coordinates)
/// @return
/// Undistorted 2D point (normalized image coordinates)
RETINIFY_API auto UndistortPoint(const Intrinsics &intrinsics, const Distortion &distortion, const Point2d &point) noexcept -> Point2d;
RETINIFY_API auto UndistortPoint(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Point2d &point) noexcept -> Point2d;

/// @brief
/// Distort a normalized 2D point using the given camera intrinsics and distortion parameters
Expand All @@ -332,7 +332,7 @@ RETINIFY_API auto UndistortPoint(const Intrinsics &intrinsics, const Distortion
/// Undistorted 2D point (normalized image coordinates)
/// @return
/// Distorted 2D point (in pixel coordinates)
RETINIFY_API auto DistortPoint(const Intrinsics &intrinsics, const Distortion &distortion, const Point2d &point) noexcept -> Point2d;
RETINIFY_API auto DistortPoint(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Point2d &point) noexcept -> Point2d;

/// @brief
/// Perform stereo rectification for a pair of cameras
Expand Down Expand Up @@ -370,7 +370,7 @@ RETINIFY_API auto DistortPoint(const Intrinsics &intrinsics, const Distortion &d
/// and -1 applies the default behavior
/// @return
/// A Status object that indicates whether the operation was successful
RETINIFY_API auto StereoRectify(const Intrinsics &intrinsics1, const Distortion &distortion1, const Intrinsics &intrinsics2, const Distortion &distortion2, const Mat3x3d &rotation, const Vec3d &translation, std::uint32_t imageWidth, std::uint32_t imageHeight, Mat3x3d &rotation1, Mat3x3d &rotation2, Mat3x4d &projectionMatrix1, Mat3x4d &projectionMatrix2, Mat4x4d &reprojectionMatrix, double alpha) noexcept -> Status;
RETINIFY_API auto StereoRectify(const PinholeIntrinsics &intrinsics1, const DistortionParameters &distortion1, const PinholeIntrinsics &intrinsics2, const DistortionParameters &distortion2, const Mat3x3d &rotation, const Vec3d &translation, std::uint32_t imageWidth, std::uint32_t imageHeight, Mat3x3d &rotation1, Mat3x3d &rotation2, Mat3x4d &projectionMatrix1, Mat3x4d &projectionMatrix2, Mat4x4d &reprojectionMatrix, double alpha) noexcept -> Status;

/// @brief
/// Initialize undistort and rectify maps for image remapping
Expand All @@ -396,7 +396,7 @@ RETINIFY_API auto StereoRectify(const Intrinsics &intrinsics1, const Distortion
/// Stride of a row in mapY (in bytes)
/// @return
/// A Status object that indicates whether the operation was successful
RETINIFY_API auto InitUndistortRectifyMap(const Intrinsics &intrinsics, const Distortion &distortion, const Mat3x3d &rotation, const Mat3x4d &projectionMatrix, std::uint32_t imageWidth, std::uint32_t imageHeight, float *mapX, std::size_t mapXStride, float *mapY, std::size_t mapYStride) noexcept -> Status;
RETINIFY_API auto InitUndistortRectifyMap(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Mat3x3d &rotation, const Mat3x4d &projectionMatrix, std::uint32_t imageWidth, std::uint32_t imageHeight, float *mapX, std::size_t mapXStride, float *mapY, std::size_t mapYStride) noexcept -> Status;

/// @brief
/// Initialize identity maps for undistortion/rectification
Expand Down
14 changes: 7 additions & 7 deletions retinify/src/geometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -290,7 +290,7 @@ enum class BaselineAxis : std::uint8_t
return Exp(Multiply(cross, scale));
}

[[nodiscard]] auto ComputePrincipalPoint(const Intrinsics &intrinsics, const Distortion &distortion, const Mat3x3d &rectifiedRotation, double newFocalLength, double width, double height) noexcept -> Point2d
[[nodiscard]] auto ComputePrincipalPoint(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Mat3x3d &rectifiedRotation, double newFocalLength, double width, double height) noexcept -> Point2d
{
const std::array<Point2d, 4> imageCorners{Point2d{0.0, 0.0}, Point2d{width - 1.0, 0.0}, Point2d{0.0, height - 1.0}, Point2d{width - 1.0, height - 1.0}};

Expand Down Expand Up @@ -358,7 +358,7 @@ constexpr double kInfinity = std::numeric_limits<double>::infinity();
return index == 0 || index == lastIndex;
}

auto ComputeRectifiedInnerOuterRectangles(const Intrinsics &intrinsics, const Distortion &distortion, const Mat3x3d &rectifiedRotation, const Mat3x3d &newCameraMatrix, std::uint32_t imageWidth, std::uint32_t imageHeight, Rect2d &inner, Rect2d &outer) noexcept -> void
auto ComputeRectifiedInnerOuterRectangles(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Mat3x3d &rectifiedRotation, const Mat3x3d &newCameraMatrix, std::uint32_t imageWidth, std::uint32_t imageHeight, Rect2d &inner, Rect2d &outer) noexcept -> void
{
constexpr int kGridSize = 9;
constexpr int kLastIndex = kGridSize - 1;
Expand Down Expand Up @@ -465,7 +465,7 @@ auto ComputeRectifiedInnerOuterRectangles(const Intrinsics &intrinsics, const Di
}
}

[[nodiscard]] auto ComputeRectifiedFocalLengthScale(const Intrinsics &intrinsics1, const Distortion &distortion1, const Mat3x3d &rectifiedRotation1, const Intrinsics &intrinsics2, const Distortion &distortion2, const Mat3x3d &rectifiedRotation2, double focalLength, const Point2d &principalPoint1, const Point2d &principalPoint2, std::uint32_t imageWidth, std::uint32_t imageHeight, double alpha) noexcept -> double
[[nodiscard]] auto ComputeRectifiedFocalLengthScale(const PinholeIntrinsics &intrinsics1, const DistortionParameters &distortion1, const Mat3x3d &rectifiedRotation1, const PinholeIntrinsics &intrinsics2, const DistortionParameters &distortion2, const Mat3x3d &rectifiedRotation2, double focalLength, const Point2d &principalPoint1, const Point2d &principalPoint2, std::uint32_t imageWidth, std::uint32_t imageHeight, double alpha) noexcept -> double
{
if (alpha < 0.0)
{
Expand Down Expand Up @@ -524,7 +524,7 @@ auto ComputeRectifiedInnerOuterRectangles(const Intrinsics &intrinsics, const Di
}
} // namespace

auto UndistortPoint(const Intrinsics &intrinsics, const Distortion &distortion, const Point2d &point) noexcept -> Point2d
auto UndistortPoint(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Point2d &point) noexcept -> Point2d
{
const double inverseFocalX = Reciprocal(intrinsics.fx, 1.0);
const double inverseFocalY = Reciprocal(intrinsics.fy, 1.0);
Expand Down Expand Up @@ -556,7 +556,7 @@ auto UndistortPoint(const Intrinsics &intrinsics, const Distortion &distortion,
return {undistortedX, undistortedY};
}

auto DistortPoint(const Intrinsics &intrinsics, const Distortion &distortion, const Point2d &point) noexcept -> Point2d
auto DistortPoint(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Point2d &point) noexcept -> Point2d
{
const double undistortedX = point[0];
const double undistortedY = point[1];
Expand All @@ -579,7 +579,7 @@ auto DistortPoint(const Intrinsics &intrinsics, const Distortion &distortion, co
return {distortedX * intrinsics.fx + intrinsics.cx, distortedY * intrinsics.fy + intrinsics.cy};
}

auto StereoRectify(const Intrinsics &intrinsics1, const Distortion &distortion1, const Intrinsics &intrinsics2, const Distortion &distortion2, const Mat3x3d &rotation, const Vec3d &translation, std::uint32_t imageWidth, std::uint32_t imageHeight, Mat3x3d &rectifiedRotation1, Mat3x3d &rectifiedRotation2, Mat3x4d &projectionMatrix1, Mat3x4d &projectionMatrix2, Mat4x4d &reprojectionMatrix, double alpha) noexcept -> Status
auto StereoRectify(const PinholeIntrinsics &intrinsics1, const DistortionParameters &distortion1, const PinholeIntrinsics &intrinsics2, const DistortionParameters &distortion2, const Mat3x3d &rotation, const Vec3d &translation, std::uint32_t imageWidth, std::uint32_t imageHeight, Mat3x3d &rectifiedRotation1, Mat3x3d &rectifiedRotation2, Mat3x4d &projectionMatrix1, Mat3x4d &projectionMatrix2, Mat4x4d &reprojectionMatrix, double alpha) noexcept -> Status
{
if ((imageWidth == 0U) || (imageHeight == 0U))
{
Expand Down Expand Up @@ -636,7 +636,7 @@ auto StereoRectify(const Intrinsics &intrinsics1, const Distortion &distortion1,
return Status{};
}

auto InitUndistortRectifyMap(const Intrinsics &intrinsics, const Distortion &distortion, const Mat3x3d &rotation, const Mat3x4d &projectionMatrix, std::uint32_t imageWidth, std::uint32_t imageHeight, float *mapX, std::size_t mapXStride, float *mapY, std::size_t mapYStride) noexcept -> Status
auto InitUndistortRectifyMap(const PinholeIntrinsics &intrinsics, const DistortionParameters &distortion, const Mat3x3d &rotation, const Mat3x4d &projectionMatrix, std::uint32_t imageWidth, std::uint32_t imageHeight, float *mapX, std::size_t mapXStride, float *mapY, std::size_t mapYStride) noexcept -> Status
{
if (mapX == nullptr || mapY == nullptr)
{
Expand Down
8 changes: 4 additions & 4 deletions retinify/src/io.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ template <typename T> [[nodiscard]] auto ReadIntegral(const nlohmann::json &obj,
return ReadValue(obj, key, [](const nlohmann::json &value) { return value.is_number_integer() || value.is_number_unsigned(); }, out);
}

[[nodiscard]] auto SerializeIntrinsics(const Intrinsics &intrinsics) -> nlohmann::json
[[nodiscard]] auto SerializeIntrinsics(const PinholeIntrinsics &intrinsics) -> nlohmann::json
{
return {{kKeyFx, intrinsics.fx}, //
{kKeyFy, intrinsics.fy}, //
Expand All @@ -100,7 +100,7 @@ template <typename T> [[nodiscard]] auto ReadIntegral(const nlohmann::json &obj,
{kKeySkew, intrinsics.skew}};
}

[[nodiscard]] auto SerializeDistortion(const Distortion &distortion) -> nlohmann::json
[[nodiscard]] auto SerializeDistortion(const DistortionParameters &distortion) -> nlohmann::json
{
return {{kKeyK1, distortion.k1}, //
{kKeyK2, distortion.k2}, //
Expand All @@ -126,7 +126,7 @@ template <typename T> [[nodiscard]] auto ReadIntegral(const nlohmann::json &obj,
{kKeyCalibrationTime, parameters.calibrationTime}};
}

[[nodiscard]] auto DeserializeIntrinsics(const nlohmann::json &value, Intrinsics &intrinsics) -> bool
[[nodiscard]] auto DeserializeIntrinsics(const nlohmann::json &value, PinholeIntrinsics &intrinsics) -> bool
{
if (!value.is_object())
{
Expand All @@ -140,7 +140,7 @@ template <typename T> [[nodiscard]] auto ReadIntegral(const nlohmann::json &obj,
ReadNumber(value, kKeySkew, intrinsics.skew);
}

[[nodiscard]] auto DeserializeDistortion(const nlohmann::json &value, Distortion &distortion) -> bool
[[nodiscard]] auto DeserializeDistortion(const nlohmann::json &value, DistortionParameters &distortion) -> bool
{
if (!value.is_object())
{
Expand Down
Loading