diff --git a/.gitignore b/.gitignore index ba39cc5..ff901d6 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ Manifest.toml +Manifest-v1.12.toml diff --git a/Project.toml b/Project.toml index 41ca82f..2587418 100644 --- a/Project.toml +++ b/Project.toml @@ -1,19 +1,23 @@ name = "CameraModels" uuid = "0d57b887-6120-40e6-8b8c-29d3116bc0c1" -keywords = ["camera", "model", "pinhole", "distortion", "robotics", "images"] +keywords = ["camera", "model", "pinhole", "distortion", "robotics", "images", "vision"] version = "0.2.4" [deps] DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" +GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326" LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +LoopVectorization = "bdcacae8-1622-11e9-2a5c-532679323890" RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" [compat] DocStringExtensions = "0.8, 0.9" +GeometryBasics = "0.5.10" LieGroups = "0.1" +LoopVectorization = "0.12.173" RecursiveArrayTools = "3.27.0" Rotations = "1" StaticArrays = "1" diff --git a/README.md b/README.md index 04fe8d1..f5ff39c 100644 --- a/README.md +++ b/README.md @@ -26,11 +26,16 @@ The image convention is intended to be the best compromise between [JuliaImages' ## Camera frame and pixel indices -![Images Convention](docs/imgs/JuliaRoboticsImgConv.png) + +![Images Convention](docs/imgs/diagram1.svg) +![Image array diagram](docs/imgs/diagram2.svg) + ## Robotics reference to camera frame -![Image m_T_c](docs/imgs/m_T_c.png) + +![Image m_T_c](docs/imgs/diagram3.svg) + # Roadmap diff --git a/docs/imgs/cameramodels.mathcha b/docs/imgs/cameramodels.mathcha new file mode 100644 index 0000000..1a4cff5 Binary files /dev/null and b/docs/imgs/cameramodels.mathcha differ diff --git a/docs/imgs/diagram1.svg b/docs/imgs/diagram1.svg new file mode 100644 index 0000000..25810a0 --- /dev/null +++ b/docs/imgs/diagram1.svg @@ -0,0 +1,11 @@ + + + + + + diff --git a/docs/imgs/diagram2.svg b/docs/imgs/diagram2.svg new file mode 100644 index 0000000..bf8ec09 --- /dev/null +++ b/docs/imgs/diagram2.svg @@ -0,0 +1,11 @@ + + + + + + diff --git a/docs/imgs/diagram3.svg b/docs/imgs/diagram3.svg new file mode 100644 index 0000000..3090b60 --- /dev/null +++ b/docs/imgs/diagram3.svg @@ -0,0 +1,11 @@ + + + + + + diff --git a/src/CameraModels.jl b/src/CameraModels.jl index f850e34..0cc7ea6 100644 --- a/src/CameraModels.jl +++ b/src/CameraModels.jl @@ -1,31 +1,32 @@ module CameraModels -using LinearAlgebra -using LieGroups -using DocStringExtensions -using StaticArrays -import Rotations as Rot_ -import Base: getindex, getproperty, show +using + LinearAlgebra, + LieGroups, + DocStringExtensions, + StaticArrays, + GeometryBasics + + using RecursiveArrayTools: ArrayPartition -# exports -include("ExportAPI.jl") +using LoopVectorization: @tturbo +# will be deprecated soon, when radialDistortion! is implemented on the GPU with KA.jl -# data types -include("entities/GeneralTypes.jl") -include("entities/CameraCalibration.jl") -include("services/CameraCalibration.jl") +import Base: + getindex, + getproperty, + show -# legacy implementations -include("Deprecated.jl") -# function logic -include("services/Prototypes.jl") +include("ExportAPI.jl") +include("entities/GeneralTypes.jl") +include("entities/CameraCalibration.jl") +include("services/Helper.jl") +include("services/Display.jl") include("services/CameraServices.jl") include("services/Utils.jl") - - end # module diff --git a/src/Deprecated.jl b/src/Deprecated.jl deleted file mode 100644 index ffc3d51..0000000 --- a/src/Deprecated.jl +++ /dev/null @@ -1,302 +0,0 @@ - -## ================================================================================================ -## consolidated types from various repos in Julia ecosystem -## ================================================================================================ - -@deprecate project(cm::CameraModelFull, pt::AbstractVector{<:Real}) project(cm.ci,pt) # drops extrinsics - -# function project( -# cm::CameraModelFull, -# pt::AbstractVector{<:Real} -# ) -# res = Vector{Float64}(2) -# project!(res, cm, pt) -# return res -# end - -""" - CameraCalibrationT - -A Union type for users to implement against both `struct`` and `mutable struct` definitions of `CameraCalibration` -""" -CameraCalibrationT = Union{<:CameraCalibration, <:CameraCalibrationMutable} - -@deprecate CameraExtrinsic(R::AbstractMatrix=[1 0 0; 0 1 0; 0 0 1.], t::AbstractVector=[0,0,0.]) ArrayPartition(SVector(t...),SMatrix(R)) - -# Camera extrinsic must be world in camera frame (cRw) -# Base.@kwdef struct CameraExtrinsic{T <: Real} -# R::SMatrix{3,3,T,9} = one(Rot_.RotMatrix{3, Float64}).mat -# t::SVector{3,T} = SVector(0,0,0.) -# end - - -@deprecate PixelCoordinate(row,col) PixelIndex(row,col) - -# CameraCalibration( -# height::Int= 480, -# width::Int = 640, -# center::Union{<:AbstractVector{<:Real},<:PixelCoordinate} = [width/2;height/2], -# focal::AbstractVector{<:Real} = 1.1*[height; height], # just emperical default -# kc::AbstractVector{<:Real} = SVector{5}(zeros(5)), -# skew::Real = 0.0, -# K::AbstractMatrix =[[focal[1];skew;center[1]]';[0.0;focal[2];center[2]]';[0.0;0;1]'], -# ) = CameraCalibration(height, width, kc, SMatrix{3,3}(K), SMatrix{3,3}(inv(K)) ) -# CameraCalibrationMutable(; -# width::Int = 640, -# height::Int= 480, -# center::Union{<:AbstractVector{<:Real},<:PixelCoordinate} = [width/2;height/2], -# focal::AbstractVector{<:Real} = 1.1*[height; height], # just emperical default -# kc::AbstractVector{<:Real} = MVector{5}(zeros(5)), -# skew::Real = 0.0, -# K::AbstractMatrix =[[focal[1];skew;center[1]]';[0.0;focal[2];center[2]]';[0.0;0;1]'], -# ) = CameraCalibrationMutable(height, width, kc, MMatrix{3,3}(K), MMatrix{3,3}(inv(K)) ) - - - -@deprecate point2pixel(model, pt) project(model, pt[[1;3;2]]) -@deprecate pixel2ray(model, px) backproject(model, px)[[1;3;2]] - - -# """ -# point2pixel(model::Pinhole, pointincamera::$(Point3)) - -# Return a transformation that converts real-world coordinates -# to camera coordinates. This currently ignores any tangential -# distortion between the lens and the image plane. -# """ -# function point2pixel(model::CameraCalibrationT, pointincamera::Point3) -# column = model.prinicipalpoint[1] + model.focallength[1] * pointincamera[1] / pointincamera[2] -# row = model.prinicipalpoint[2] - model.focallength[2] * pointincamera[3] / pointincamera[2] -# return PixelCoordinate(column, row) -# end - - -# """ -# pixel2ray(model::Pinhole, pixelcoordinate::$(PixelCoordinate)) - -# Return a transformation that converts real-world coordinates -# to camera coordinates. This currently ignores any tangential -# distortion between the lens and the image plane. -# """ -# function pixel2ray(model::CameraCalibrationT, pixelcoordinate::PixelCoordinate) -# x = (pixelcoordinate[1] - model.prinicipalpoint[1]) / model.focallength[1] -# z = -(pixelcoordinate[2] - model.prinicipalpoint[2]) / model.focallength[2] -# return Vector3(x, 1, z) -# end - - -export CameraModel # being replaced by AbstractCameraModel -CameraModel = (@warn("CameraModels.CameraModel is deprecated, use CameraModels.AbstractCameraModel instead");AbstractCameraModel) -# abstract type CameraModel end - -@warn "RadialDistortion is deprecated, use CameraCalibration instead" -# Base.@kwdef struct RadialDistortion{N, R <: Real, K <: AbstractVector} -# Ki::SVector{N,R} = SVector(0.0) # -# center::SVector{2,R} = SVector{2,R}(0.0,0.0) # SVector{2,R} # [h,w] -# # _radius2::Matrix{R} # perhaps SizedArray{R,2} or StaticArray{R,2} or GPUArray{R,2} depending on performance -# end - - -@deprecate columns(w...;kw...) width(w...;kw...) -@deprecate rows(w...;kw...) height(w...;kw...) - -# sensorsize(cameramodel::CameraModel) = SVector{2}(width(cameramodel), height(cameramodel)) - - -export CameraModelandParameters -# const CameraModelandParameters = (@warn("CameraModels.CameraModelandParameters is deprecated, use CameraModels.CameraCalibration instead.");CameraCalibration) - -function CameraModelandParameters( - width::Int, - height::Int, - fc::AbstractVector{<:Real}, - cc::AbstractVector{<:Real}, - skew::Real, - kc::AbstractVector{<:Real}, - K::AbstractMatrix{<:Real} = [[fc[1];skew;cc[1]]';[0.0;fc[2];cc[2]]';[0.0;0;1]'], # legacy constructor - Ki::AbstractMatrix{<:Real} = inv(K) - ) - # - @warn("CameraModels.CameraModelandParameters is deprecated, use CameraModels.CameraCalibration instead.") - CameraCalibration(;width,height,K=SMatrix{3,3}(K)) -end - - - -# """ -# Data structure for a Camera model with parameters. -# Use `CameraModel(width,height,fc,cc,skew,kc)` for easy construction. -# """ -# struct CameraModelandParameters <: AbstractCameraModel -# width::Int # image width -# height::Int # image height -# fc::Vector{Float64} # focal length in x and y -# cc::Vector{Float64} # camera center -# skew::Float64 # skew value -# kc::Vector{Float64} # distortion coefficients up to fifth order -# K::Matrix{Float64} # 3x3 camera calibration matrix (Camera intrinsics) -# Ki::Matrix{Float64} # inverse of a 3x3 camera calibratio matrix -# end - - - -export PinholeCamera - -# ## From JuliaRobotics/Caesar.jl -# """ -# $TYPEDEF - -# Pinhole Camera model is the most simplistic. - -# Notes -# - https://en.wikipedia.org/wiki/Pinhole_camera -# - Standard Julia *[Images.jl](https://juliaimages.org/latest/)-frame* convention is, `size(img) <==> (i,j) <==> (height,width) <==> (y,x)`, -# - Common *camera-frame* in computer vision and robotics, `(x,y) <==> (width,height) <==> (j,i)`, -# - Using top left corner of image as `(0,0)` in all cases. -# - Direct comparison with [OpenCV convention](https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html) is: -# - `(x,y) [CamXYZ] <==> (j,i) [Images.jl] <==> (u,v) [OpenCV]` -- look very carfully at `(u,v) <==> (j,i)` in *image-frame* -# - Always follow right hand rule for everything. - -# DevNotes -# - https://en.wikipedia.org/wiki/Distortion_(optics) -# """ -# struct PinholeCamera{R <: Real} <: AbstractCameraModel -# K::SMatrix{3,3,R} -# end - -## From JuliaRobotics/Caesar.jl -# const PinholeCamera = (@warn("CameraModels.PinholeCamera is deprecated, use CameraModels.CameraCalibrationMutable instead."); CameraCalibration) - -function PinholeCamera( - K_::AbstractMatrix=[[510;0;320.0]';[0.0;510;240]';[0.0;0;1]']; # legacy constructor - width::Int=round(Int, K_[1,3]*2), - height::Int=round(Int, K_[2,3]*2), - f_w::Real=K_[1,1], - f_h::Real=K_[2,2], - c_w::Real=K_[1,3], - c_h::Real=K_[2,3], - shear::Real=K_[1,2], - K::AbstractMatrix=[[f_w;shear;c_w]';[0.0;f_h;c_h]';[0.0;0;1]'], # consolidated matrix K -) - # - @warn "CameraModels.PinholeCamera is deprecated, use CameraModels.CameraCalibrationMutable instead." - if 3 < size(K_,1) - @warn "PinholeCamera(arg), 3 < size(arg,1), assuming legacy constructor as img as input argument." - return CameraCalibrationMutable(K_) # as though img=K_ - end - CameraCalibrationMutable(;width,height,K=MMatrix{3,3}(K)) -end - -# @deprecate PinholeCamera(img::AbstractMatrix) CameraCalibrationMutable(img) - -# function PinholeCamera(img::AbstractMatrix{T}) where T -# f_w, c_w, c_h = size(img, 1), size(img, 2)/2, size(img, 1)/2 -# f_h = f_w -# @info "Assuming default PinholeCamera from image $(size(img)):" f_w f_h c_w c_h -# PinholeCamera(;f_w, f_h, c_w, c_h) -# end - -@deprecate c_w(w...) pp_w(w...) -@deprecate c_h(w...) pp_h(w...) -# f_w(pc::Union{CameraCalibration,CameraCalibrationMutable}) = pc.K[1,1] -# f_h(pc::Union{CameraCalibration,CameraCalibrationMutable}) = pc.K[2,2] -# shear(pc::Union{CameraCalibration,CameraCalibrationMutable}) = pc.K[1,2] -# pp_w(pc::Union{CameraCalibration,CameraCalibrationMutable}) = pc.K[1,3] -# pp_h(pc::Union{CameraCalibration,CameraCalibrationMutable}) = pc.K[2,3] - -# set_f_w!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1,1] = val) -# set_f_h!(pc::CameraCalibrationMutable, val::Real) = (pc.K[2,2] = val) -# set_shear!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1,2] = val) -# set_pp_w!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1,3] = val) -# set_pp_h!(pc::CameraCalibrationMutable, val::Real) = (pc.K[2,3] = val) - - -export Pinhole - - -## From yakir12/CameraModels.jl -# const Pinhole = (@warn("CameraModels.Pinhole is deprecated, use CameraModels.CameraCalibration instead."); CameraCalibration) -function Pinhole(columns::Int,rows::Int,prinicipalpoint,focallength::Vector2 ) - @warn "CameraModels.Pinhole is deprecated, use CameraModels.CameraCalibration instead." - f_w,f_h = focallength[1], focallength[2] - c_w,c_h = prinicipalpoint[1], prinicipalpoint[2] - K = SMatrix{3,3}([[f_w;0.0;c_w]';[0.0;f_h;c_h]';[0.0;0;1.]'] ) - CameraCalibration(; - height=rows, - width=columns, - K - ) -end - -function CameraIntrinsic( - K_::AbstractMatrix=[[510;0;320.0]';[0.0;510;240]';[0.0;0;1]']; # legacy constructor - x0=320.0,y0=240.0,fx=510.0,fy=510.0,s=0.0, # legacy function support - K::AbstractMatrix=[[fx;s;x0]';[0.0;fy;y0]';[0.0;0;1]'], # consolidated matrix K - width::Int=round(Int, K[1,3]*2), - height::Int=round(Int, K[2,3]*2), -) - # - @warn "CameraModels.CameraIntrinsic is deprecated, use CameraModels.CameraCalibration instead." - CameraCalibration(;width,height,K) -end -export CameraIntrinsic - -# Base.@kwdef struct CameraIntrinsic -# K::Array{Float64,2} -# end -# CameraIntrinsic(;x0=320.0,y0=240.0,fx=510.0,fy=510.0,s=0.0) = CameraIntrinsic([[fx;s;x0]';[0.0;fy;y0]';[0.0;0;1]']) - -## From JuliaRobotics/Caesar.jl -# const CameraIntrinsic = (@warn("CameraModels.CameraIntrinsic is deprecated, use CameraModels.CameraCalibration instead.");CameraCalibration) - - -function Base.getproperty( - x::CameraCalibration, # Union{<:Pinhole, CameraModelandParameters}, - f::Symbol, -) - if f == :skew - getfield(x, :K)[1,2] - elseif f == :columns - getfield(x, :width) - elseif f == :rows - getfield(x, :height) - elseif f == :prinicipalpoint || f == :cc - SA[(getfield(x, :K)[1:2, 3])...] - elseif f == :focallength || f == :fc - K = getfield(x, :K) - SA[K[1,1];K[2,2]] - else - getfield(x, f) - end -end -# function Base.getproperty(x::CameraModelandParameters, f::Symbol) -# if f == :skew -# getfield(x, :K)[1,2] -# elseif f == :columns -# getfield(x, :width) -# elseif f == :rows -# getfield(x, :height) -# elseif f == :prinicipalpoint || f == :cc -# SA[(getfield(x, :K)[1:2, 3])...] -# elseif f == :focallength || f == :fc -# K = getfield(x, :K) -# SA[K[1,1];K[2,2]] -# else -# getfield(x, f) -# end -# end - -# ## From yakir12/CameraModels.jl -# struct Pinhole <: CameraModel -# # note order of elements has been changed from original source so that inner constructor can be removed. -# columns::Int -# rows::Int -# prinicipalpoint::PixelCoordinate # in pixels -# focallength::Vector2 # in pixels -# end - - - - - -# \ No newline at end of file diff --git a/src/ExportAPI.jl b/src/ExportAPI.jl index bd31a09..cd7210b 100644 --- a/src/ExportAPI.jl +++ b/src/ExportAPI.jl @@ -1,24 +1,45 @@ - - - +# Abstract type export AbstractCameraModel -export CameraCalibration, CameraCalibrationMutable - -export toNonhomogeneous -export CameraSkewDistortion - -export undistortPoint -export Ray, PixelIndex -export pixel2ray, point2pixel, canreproject, sensorsize #, origin, direction, -export project, projectHomogeneous -export backproject, backprojectHomogeneous -export pp_w, pp_h, f_w, f_h - -export lookdirection, updirection #, columns, rows - -export intersectLineToPlane3D, intersectRayToPlane - -# suppressing super general signatures likely to have conflicts. -# TODO adopt common Julia definition for points and vectors, maybe something from JuliaGeometry, etc. -# export Vector3, Vector2, Point3 \ No newline at end of file +# Camera structures +export + CameraCalibration, + CameraCalibrationMutable + + +export + toNonhomogeneous, + undistortPoint, + Ray, + PixelIndex, + canreproject, + sensorsize, + project, + projectHomogeneous, + backproject, + backprojectHomogeneous, + pp_w, + pp_h, + f_w, + f_h, + shear, + set_f_h!, + set_f_w!, + set_pp_h!, + set_pp_w!, + width, + height, + direction, + lookdirection, + updirection, + canreproject, + intersectLineToPlane3D, + intersectRayToPlane + + +# Elemnents from JuliaGeometry/GeometryBasics +export + Vector2, + Vector3, + Point3, + origin3d # Origin Point3 diff --git a/src/entities/CameraCalibration.jl b/src/entities/CameraCalibration.jl index dd22c5d..91a1766 100644 --- a/src/entities/CameraCalibration.jl +++ b/src/entities/CameraCalibration.jl @@ -1,18 +1,18 @@ - """ - $TYPEDEF + $(TYPEDEF) -Standard pinhole camera model with distortion parameters (aka camera intrinsics). +Standard pinhole camera model with distortion parameters (aka camera intrinsics). -Notes: +Notes : - Image origin assumed as top-left. - Keeping with Images.jl, - width of the image are matrix columns from left to right. - height of the image are matrix rows from top to bottom. - E.g. `mat[i,j] == img[h,w] == mat[h,w] == img[i,j]` - - This is to leverage the unified Julia Arrays infrastructure, incl vectors, view, Static, CPU, GPU, etc. + - This is to leverage the unified Julia Arrays infrastructure, + including vectors, view, Static, CPU, GPU, etc... -Legacy Comments: +Legacy Comments : ---------------- Pinhole Camera model is the most simplistic. @@ -21,65 +21,62 @@ Notes - https://en.wikipedia.org/wiki/Pinhole_camera - Standard Julia *[Images.jl](https://juliaimages.org/latest/)-frame* convention is, `size(img) <==> (i,j) <==> (height,width) <==> (y,x)`, - Common *camera-frame* in computer vision and robotics, `(x,y) <==> (width,height) <==> (j,i)`, - - Using top left corner of image as `(0,0)` in all cases. + - Using top left corner of image as `(0,0)` in all cases. - Direct comparison with [OpenCV convention](https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html) is: - - `(x,y) [CamXYZ] <==> (j,i) [Images.jl] <==> (u,v) [OpenCV]` -- look very carfully at `(u,v) <==> (j,i)` in *image-frame* + - `(x,y) [CamXYZ] <==> (j,i) [Images.jl] <==> (u,v) [OpenCV]` -- look very carefully at `(u,v) <==> (j,i)` in *image-frame* - Always follow right hand rule for everything. - An abstract type [`AbstractCameraModel`](@ref) is provided to develop implementations against `struct` and `mutable struct` types. -DevNotes +DevNotes : - https://en.wikipedia.org/wiki/Distortion_(optics) -Also see: [`AbstractCameraModel`](@ref) [`CameraCalibrationMutable`](@ref), (TODO: `ProjectiveCameraModel`) +Also see : + [`AbstractCameraModel`](@ref), + [`CameraCalibrationMutable`](@ref), + (TODO: `ProjectiveCameraModel`) """ -Base.@kwdef struct CameraCalibration{R <: Real,N} <: AbstractCameraModel - """ number of pixels from top to bottom """ - height::Int = 480 - """ number of pixels from left to right """ - width::Int = 640 - """ distortion coefficients up to fifth order """ - kc::SVector{N,R} = SVector(zeros(5)...) - """ 3x3 camera calibration matrix """ - K::SMatrix{3,3,R,9} = SMatrix{3,3}([[1.1*height;0.0;width/2]';[0.0;1.1*height;height/2]';[0.0;0;1.]'] ) - """ inverse of a 3x3 camera calibration matrix """ - Ki::SMatrix{3,3,R,9} = inv(K) +Base.@kwdef struct CameraCalibration{R <: Real, N} <: AbstractCameraModel + """ number of pixels from top to bottom """ + height::Int = 480 + """ number of pixels from left to right """ + width::Int = 640 + """ distortion coefficients up to fifth order """ + kc::SVector{N, R} = @SVector zeros(5) + """ 3x3 camera calibration matrix """ + + K::SMatrix{3, 3, R, 9} = @SMatrix[ + 1.1 * height 0.0 width / 2 + 0.0 1.1 * height height / 2 + 0.0 0.0 1.0 + ] + """ inverse of a 3x3 camera calibration matrix """ + Ki::SMatrix{3, 3, R, 9} = inv(K) end """ - $TYPEDEF - -See [`CameraCalibraton`](@ref). -""" -Base.@kwdef mutable struct CameraCalibrationMutable{R <: Real,N} <: AbstractCameraModel - """ number of pixels from top to bottom """ - height::Int = 480 - """ number of pixels from left to right """ - width::Int = 640 - """ distortion coefficients up to fifth order """ - kc::MVector{N,R} = MVector(zeros(5)...) - """ 3x3 camera calibration matrix """ - K::MMatrix{3,3,R} = MMatrix{3,3}([[1.1*height;0.0;width/2]';[0.0;1.1*height;height/2]';[0.0;0;1.]'] ) - """ inverse of a 3x3 camera calibration matrix """ - Ki::MMatrix{3,3,R} = inv(K) -end - - - + $(TYPEDEF) +See [`CameraCalibration`](@ref). -## =========================================================================== -## Legacy types that are not so easy to consolidate (not exported) DO NOT USE -## =========================================================================== +kc, K and Ki are stored as Mutable Vectors and Mutable Matrices (`MVector`, `MMatrix`), +compared to `CameraCalibration` - -Base.@kwdef struct CameraModelFull - ci::CameraCalibration = CameraCalibration() - ce::ArrayPartition = ArrayPartition(SVector(0.,0.,0.),SMatrix{3,3}(1.,0.,0.,0.,1.,0.,0.,0.,1.)) # CameraExtrinsic() +""" +Base.@kwdef mutable struct CameraCalibrationMutable{R <: Real, N} <: AbstractCameraModel + """ number of pixels from top to bottom """ + height::Int = 480 + """ number of pixels from left to right """ + width::Int = 640 + """ distortion coefficients up to fifth order """ + kc::MVector{N, R} = @MVector zeros(5) + """ 3x3 camera calibration matrix """ + K::MMatrix{3, 3, R} = @MMatrix[ + 1.1 * height 0.0 width / 2 + 0.0 1.1 * height height / 2 + 0.0 0.0 1.0 + ] + """ inverse of a 3x3 camera calibration matrix """ + Ki::MMatrix{3, 3, R} = inv(K) end - - - - -# \ No newline at end of file diff --git a/src/entities/GeneralTypes.jl b/src/entities/GeneralTypes.jl index 2b6af02..a672b45 100644 --- a/src/entities/GeneralTypes.jl +++ b/src/entities/GeneralTypes.jl @@ -1,3 +1,5 @@ +# Abstract type +abstract type AbstractCameraModel end struct PixelIndex{VALID, T <: Real} @@ -5,31 +7,29 @@ struct PixelIndex{VALID, T <: Real} col::T depth::T end -PixelIndex(u::T, v::T; valid::Bool=true, depth = T(0)) where {T <: Real} = PixelIndex{valid,T}(u, v, depth) - -function Base.getindex(p::PixelIndex,i::Int) - if i === 1 - p.row - elseif i === 2 - p.col - elseif i === 3 - p.depth - else - DomainError("Camera only has rows and columns, cannot index to $i") - end -end +PixelIndex( + u::T, + v::T; + valid::Bool = true, + depth = zero(T) +) where {T <: Real} = PixelIndex{valid, T}(u, v, depth) -const Vector2 = SVector{2, Float64} -const Point3 = SVector{3, Float64} -const Vector3 = SVector{3, Float64} +Base.getindex(p::PixelIndex, i::Int) = + i == 1 ? p.row : + i == 2 ? p.col : + i == 3 ? p.depth : + throw(DomainError(i, "Camera only has rows, columns and depth")) -# Abstract type -abstract type AbstractCameraModel end -origin3d = zeros(Point3) +const Vector2 = Vec{2, Float64} +const Point3 = Point{3, Float64} +const Vector3 = Vec{3, Float64} + + +origin3d = Point3(0, 0, 0) struct Ray origin::Point3 direction::Vector3 -end \ No newline at end of file +end diff --git a/src/services/CameraCalibration.jl b/src/services/CameraCalibration.jl deleted file mode 100644 index f79aa74..0000000 --- a/src/services/CameraCalibration.jl +++ /dev/null @@ -1,37 +0,0 @@ - -function Base.show(io::IO, ::MIME"text/plain", cam::CameraCalibration) - println(io,"CameraCalibration {") - println(io," sensorsize (w,h) = ", sensorsize(cam)) - println(io," principal_point (w,h) = ", pp_w(cam), ",", pp_h(cam)) - println(io," focal_length (w,h) = ", f_w(cam), ",", f_h(cam)) - println(io," shear = ", shear(cam)) - println(io," radtan coeff = ", cam.kc) - println(io, "}") - nothing -end - -Base.show(io::IO, ::MIME"text/markdown", cam::CameraCalibration) = show(io, MIME("text/plain"), cam) -Base.show(io::IO, cam::CameraCalibration) = show(io, MIME("text/plain"), cam) - - - -""" - $SIGNATURES - -Constructor helper assuming you just have a camera image and need to start somewhere for a basic camera model. - -Notes: -- Calibration will incorrect but hopefully in a distant ballpark to get the calibration process started. -- See [AprilTags.jl Calibration section](https://juliarobotics.org/AprilTags.jl/latest/#Camera-Calibration-1) for code and help. -""" -function CameraCalibrationMutable(img::AbstractMatrix{T}) where T - height, width = size(img) - # emperical assumption usually seen for focal length - f_w = 1.1*height - f_h = f_w - c_w, c_h = width/2, height/2 - K = MMatrix{3,3}([[f_w;0.0;c_w]';[0.0;f_h;c_h]';[0.0;0;1.]'] ) - @info "Assuming default CameraCalibrationMutable from image size(img)=(rows,cols)=$(size(img)):" f_w f_h c_w c_h - CameraCalibrationMutable(;width, height, K) -end - diff --git a/src/services/CameraServices.jl b/src/services/CameraServices.jl index b03eaac..b2f9f60 100644 --- a/src/services/CameraServices.jl +++ b/src/services/CameraServices.jl @@ -1,67 +1,174 @@ +""" + origin(ray) -## consolidated functions, first baseline +Return the origin of the ray as a `Vector3`. +""" +origin(vector::Union{<:AbstractVector{<:Real}, <:Vector3}) = origin3d +origin(ray::Ray) = ray.origin -## ========================================================================================= -## Parameter functions +""" + lookdirection(camera::AbstractCameraModel) -## From yakir12/CameraModels.jl -origin(vector::Union{<:AbstractVector{<:Real},<:Vector3}) = origin3d -origin(ray::Ray) = ray.origin -lookdirection(cameramodel::AbstractCameraModel) = SVector{3}(0,1,0) -updirection(cameramodel::AbstractCameraModel) = SVector{3}(0,0,1) +Return the lookdirection of this camera model. +""" +lookdirection(cameramodel::AbstractCameraModel) = Vector3(0, 1, 0) + +""" + updirection(camera::AbstractCameraModel) + +Return the updirection of this camera model. +""" +updirection(cameramodel::AbstractCameraModel) = Vector3(0, 0, 1) + +""" + width(model::AbstractCameraModel) + +Returns the width (columns) of the camera sensor. +""" width(cameramodel::AbstractCameraModel) = cameramodel.width -height(cameramodel::AbstractCameraModel) = cameramodel.height -direction(vector::Union{<:AbstractVector{<:Real},<:Vector3}) = vector + +""" + height(model::AbstractCameraModel) + +Returns the height (rows) of the camera sensor. +""" +height(cameramodel::AbstractCameraModel) = cameramodel.height + +""" + direction(ray) + +Return the direction of the ray as a `Vector3`. +""" +direction(vector::Union{<:AbstractVector{<:Real}, <:Vector3}) = vector direction(ray::Ray) = ray.direction -sensorsize(cameramodel::AbstractCameraModel) = SVector{2}(width(cameramodel), height(cameramodel)) -## From JuliaRobotics/Caesar.jl -f_w(pc::AbstractCameraModel) = pc.K[1,1] -f_h(pc::AbstractCameraModel) = pc.K[2,2] -shear(pc::AbstractCameraModel) = pc.K[1,2] -pp_w(pc::AbstractCameraModel) = pc.K[1,3] -pp_h(pc::AbstractCameraModel) = pc.K[2,3] +""" + sensorsize(model::AbstractCameraModel) + +Return the size of the camera sensor. +By default calling out to width(model) and height(model) to build a Vec{2} + +`sensorsize(cameramodel::AbstractCameraModel) = Vec{2}(width(cameramodel), height(cameramodel))` +""" +sensorsize(cameramodel::AbstractCameraModel) = Vec{2}(width(cameramodel), height(cameramodel)) + + +""" + f_w(pc::AbstractCameraModel) + +Return the focal length in the width direction. +""" +f_w(pc::AbstractCameraModel) = pc.K[1, 1] + +""" + f_h(pc::AbstractCameraModel) + +Return the focal length in the height direction. +""" +f_h(pc::AbstractCameraModel) = pc.K[2, 2] + +""" + shear(pc::AbstractCameraModel) + +Return the shear parameter of the camera. +""" +shear(pc::AbstractCameraModel) = pc.K[1, 2] + +""" + pp_w(pc::AbstractCameraModel) + +Return the principal point in the width direction. +""" +pp_w(pc::AbstractCameraModel) = pc.K[1, 3] + +""" + pp_h(pc::AbstractCameraModel) + +Return the principal point in the height direction. +""" +pp_h(pc::AbstractCameraModel) = pc.K[2, 3] + +""" + set_f_w!(pc::CameraCalibrationMutable, val::Real) + +Set the focal length in the width direction. +""" +set_f_w!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1, 1] = val) + +""" + set_f_h!(pc::CameraCalibrationMutable, val::Real) + +Set the focal length in the height direction. +""" +set_f_h!(pc::CameraCalibrationMutable, val::Real) = (pc.K[2, 2] = val) -set_f_w!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1,1] = val) -set_f_h!(pc::CameraCalibrationMutable, val::Real) = (pc.K[2,2] = val) -set_shear!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1,2] = val) -set_pp_w!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1,3] = val) -set_pp_h!(pc::CameraCalibrationMutable, val::Real) = (pc.K[2,3] = val) +""" + set_shear!(pc::CameraCalibrationMutable, val::Real) +Set the shear parameter of the camera. """ - canreproject(camera::CameraModel) +set_shear!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1, 2] = val) -Confirms if point2pixel is implemented for this camera model. +""" + set_pp_w!(pc::CameraCalibrationMutable, val::Real) + +Set the principal point in the width direction. +""" +set_pp_w!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1, 3] = val) + +""" + set_pp_h!(pc::CameraCalibrationMutable, val::Real) + +Set the principal point in the height direction. +""" +set_pp_h!(pc::CameraCalibrationMutable, val::Real) = (pc.K[2, 3] = val) + +""" + canreproject(camera::AbstractCameraModel) + +Confirms if project is implemented for this camera model. """ canreproject(camera::AbstractCameraModel) = true -## computational functions +## Computational functions ## ========================================================================================= ## FROM SCENE IN FRONT OF CAMERA TO IMAGE -- I.E. PROJECT ## ========================================================================================= +""" + $(SIGNATURES) + +Undistort a point using the camera calibration parameters. -function undistortPoint(cam::CameraCalibration, xy, iter_num=3) - k1, k2, p1, p2, k3 = cam.kc[1:5] - fx, fy = f_w(cam), f_h(cam) # cam.K[1, 1], cam.K[2, 2] - cx, cy = pp_w(cam), pp_h(cam) # cam.K[1:2, 3] - x, y = xy[1], xy[2] - x = (x - cx) / fx - x0 = x - y = (y - cy) / fy - y0 = y - for _ in 1:iter_num - r2 = x^2 + y^2 - k_inv = 1 / (1 + k1*r2 + k2*r2^2 + k3*r2^3) - delta_x = 2*p1*x*y + p2 * (r2 + 2*x^2) - delta_y = p1*(r2 + 2*y^2) + 2*p2*x*y - x = (x0 - delta_x) * k_inv - y = (y0 - delta_y) * k_inv - end - return SA[x*fx+cx; y*fy+cy] +Parameters : +- `cam`: camera calibration +- `xy`: pixel coordinates +- `iter_num`: number of iterations for the undistortion process +""" +function undistortPoint( + cam::CameraCalibration, + xy, + iter_num = 3 + ) + k1, k2, p1, p2, k3 = cam.kc[1:5] + fx, fy = f_w(cam), f_h(cam) # cam.K[1, 1], cam.K[2, 2] + cx, cy = pp_w(cam), pp_h(cam) # cam.K[1:2, 3] + x, y = xy[1], xy[2] + x = (x - cx) / fx + y = (y - cy) / fy + x0, y0 = x, y + for _ in 1:iter_num + r2 = x^2 + y^2 + k_inv = inv((1 + k1 * r2 + k2 * r2^2 + k3 * r2^3)) + delta_x = 2 * p1 * x * y + p2 * (r2 + 2 * x^2) + delta_y = p1 * (r2 + 2 * y^2) + 2 * p2 * x * y + x = (x0 - delta_x) * k_inv + y = (y0 - delta_y) * k_inv + end + return Point(x * fx + cx, y * fy + cy) end @@ -69,75 +176,75 @@ end ## FROM SCENE IN FRONT OF CAMERA TO IMAGE -- I.E. PROJECT ## ========================================================================================= +""" + $(SIGNATURES) + +Parameters : + `ret::AbstractVector{<:Real}`: The output vector to store the projected pixel coordinates. + `ci::CameraCalibration`: The camera calibration parameters. + `c_T_r::ArrayPartition`: The transformation matrix from camera frame to reference frame. + `r_P::AbstractVector{<:Real}`: The 3D point in homogeneous coordinates (camera frame). -## From JuliaRobotics/SensorFeatureTracking.jl + +Project a 3D point in homogeneous coordinates `r_P` (camera frame) to a `PixelIndex`. +""" function project!( - ret::AbstractVector{<:Real}, - ci::CameraCalibration, #CameraIntrinsic, - c_T_r::ArrayPartition, - r_P::AbstractVector{<:Real} -) - res = ci.K*(c_T_r.x[2]*r_P + c_T_r.x[1]) - ret[1:2] ./= res[3] - PixelIndex(ret[1], ret[2]) + ret::AbstractVector{<:Real}, + ci::CameraCalibration, + c_T_r::ArrayPartition, + r_P::AbstractVector{<:Real} + ) + res = ci.K * (c_T_r.x[2] * r_P + c_T_r.x[1]) + ret[1:2] ./= res[3] + return PixelIndex(ret[1], ret[2]) end """ - $SIGNATURES + $(SIGNATURES) Project a world scene onto an image. -Return a transformation that converts real-world coordinates -to camera coordinates. This currently ignores any tangential -distortion between the lens and the image plane. +Returns the pixel location onto which the 3D coordinate `r_P` is projected. +This currently ignores any tangential distortion between the lens and the image plane. Notes -- `r_P` is a point in reference frame tranformed the camera's reference frame: +- `r_P` is a point in reference frame transformed the camera's reference frame: - `c_P = c_T_r * r_P` -Deprecates: -- yakir12: `point2pixel`: @deprecate point2pixel(model, pt) project(model, pt[[1;3;2]]) - Also see: [`backproject`](@ref) """ function project( - model::AbstractCameraModel, - r_P::Union{<:AbstractVector{<:Real}, <:Point3}; - c_T_r = ArrayPartition( SVector(0.,0.,0.), SMatrix{3,3}(1.,0.,0.,0.,1.,0.,0.,0.,1.) ) - ) - # - ret = MVector(0.,0.) - project!(ret, model, c_T_r, r_P) - # column = pp_w(model) + f_w(model) * c_P[1] / c_P[3] - # row = pp_h(model) - f_h(model) * c_P[2] / c_P[3] - # return PixelIndex(column, row) + model::AbstractCameraModel, + r_P::Union{<:AbstractVector{<:Real}, <:Point3}; + c_T_r = ArrayPartition(Vector3(0.0, 0.0, 0.0), SMatrix{3, 3}(1.0 * I(3))) + ) + ret = MVector(0.0, 0.0) + return project!(ret, model, c_T_r, r_P) end -project!( - ret::AbstractVector{<:Real}, - cm::CameraModelFull, - pt::AbstractVector{<:Real}) = project!(ret, cm.ci, cm.ce, pt) +""" + $(SIGNATURES) -## homogeneous point coords xyzw (stereo cameras) -# xyzw are in the camera frame (c_), i.e. x-columns, y-rows, z-forward +Project a 3D point in homogeneous coordinates `c_Ph` (camera frame) to a `PixelIndex`. +""" function projectHomogeneous( - cam::AbstractCameraModel, - c_Ph::AbstractVector, -) - # left cam - x,y,z,w = (c_Ph...,) - fx_z = f_w(cam) / z - fy_z = f_h(cam) / z - col = x * fx_z + pp_w(cam) # add center to get PixelIndex - row = y * fy_z + pp_h(cam) - # infront or behind - depth=z/w - PixelIndex(row, col; depth, valid = (w==0&&0 0) || depth > 0 + + return PixelIndex(col, row; depth, valid) +end ## ========================================================================================= @@ -146,63 +253,51 @@ end """ - $SIGNATURES + $(SIGNATURES) Backproject from an image into a world scene. -Return a transformation that converts real-world coordinates -to camera coordinates. This currently ignores any tangential -distortion between the lens and the image plane. - -Deprecates: -- yakir12: `pixel2ray`: @deprecate pixel2ray(model, px) backproject(model, px)[[1;3;2]] +Returns the ray in space (direction vector) corresponding to this `pixelIndex`. +This currently ignores any tangential distortion between the lens and the image plane. Also see: [`project`](@ref) """ function backproject( - model::AbstractCameraModel, - px_coord::Union{<:AbstractVector{<:Real}, <:PixelIndex} - ) - # - col = (px_coord[1] - pp_w(model)) / f_w(model) - row = -(px_coord[2] - pp_h(model)) / f_h(model) - return Vector3(col, row, 1) + model::AbstractCameraModel, + px_coord::Union{<:AbstractVector{<:Real}, <:PixelIndex} + ) + col = (px_coord[1] - pp_w(model)) / f_w(model) + row = -(px_coord[2] - pp_h(model)) / f_h(model) + return Vector3(col, row, 1) end -# # camera measurements (u,v), (u2,v) -# lx = (u-center[1])*baseline -# ly = (v-center[2])*baseline -# lz = _f*baseline -# lw = u - u2 -# lw<0 ? @warn("backprojecting negative disparity\n") : nothing -# # homogeneous point coords -# return (lz, lx, ly, lw) - ## ========================================================================================= ## RESIDUAL FUNCTION FOR OPTIMIZATION OR LOSS ## ========================================================================================= +""" + $(SIGNATURES) + +Compute the residual between the observed pixel coordinates and the projected pixel coordinates. -# pinhole camera model -# (x, y)/f = (X, Y)/Z +Parameters : +- `res`: residual vector to be filled +- `z`: depth vector +- `ci`: camera calibration (camera intrinsic) +- `ce`: camera extrinsics +- `pt`: pixel index or vector of pixel coordinates +""" function cameraResidual!( - res::AbstractVector{<:Real}, - z::AbstractVector{<:Real}, - ci::CameraCalibration, #CameraIntrinsic, - ce::ArrayPartition, - pt::Union{PixelIndex,<:AbstractVector{<:Real}}, - ) - # in place memory operations - project!(res, ci, ce, pt) - res[1:2] .*= -1.0 - res[1:2] += z[1:2] - nothing + res::AbstractVector{<:Real}, + z::AbstractVector{<:Real}, + ci::CameraCalibration, + ce::ArrayPartition, + pt::Union{PixelIndex, <:AbstractVector{<:Real}}, + ) + # in place memory operations + project!(res, ci, ce, pt) + res[1:2] .*= -1.0 + res[1:2] += z[1:2] + return nothing end - - - - - - -## \ No newline at end of file diff --git a/src/services/Display.jl b/src/services/Display.jl new file mode 100644 index 0000000..d13e2f2 --- /dev/null +++ b/src/services/Display.jl @@ -0,0 +1,44 @@ +function Base.show(io::IO, ::MIME"text/plain", cam::CameraCalibration) + println(io, "CameraCalibration {") + println(io, " sensor size (width, height) = ", sensorsize(cam)) + println(io, " principal point (width, height) = ", pp_w(cam), ", ", pp_h(cam)) + println(io, " focal length (width, height) = ", f_w(cam), ", ", f_h(cam)) + println(io, " shear coefficient = ", shear(cam)) + println(io, " radial/tangential coefficients = ", cam.kc) + println(io, "}") + return nothing +end + +Base.show( + io::IO, + ::MIME"text/markdown", + cam::CameraCalibration +) = show(io, MIME("text/plain"), cam) + +Base.show( + io::IO, + cam::CameraCalibration +) = show(io, MIME("text/plain"), cam) + + +function Base.show(io::IO, ::MIME"text/plain", cam::CameraCalibrationMutable) + println(io, "CameraCalibrationMutable {") + println(io, " sensor size (width, height) = ", sensorsize(cam)) + println(io, " principal point (width, height) = ", pp_w(cam), ", ", pp_h(cam)) + println(io, " focal length (width, height) = ", f_w(cam), ", ", f_h(cam)) + println(io, " shear coefficient = ", shear(cam)) + println(io, " radial/tangential coefficients = ", cam.kc) + println(io, "}") + return nothing +end + +Base.show( + io::IO, + ::MIME"text/markdown", + cam::CameraCalibrationMutable +) = show(io, MIME("text/plain"), cam) + +Base.show( + io::IO, + cam::CameraCalibrationMutable +) = show(io, MIME("text/plain"), cam) diff --git a/src/services/Helper.jl b/src/services/Helper.jl new file mode 100644 index 0000000..5665d9d --- /dev/null +++ b/src/services/Helper.jl @@ -0,0 +1,41 @@ +""" + $(SIGNATURES) + +Constructor helper assuming you just have a camera image and need to start somewhere for a basic camera model. + +Notes: +- Calibration will incorrect but hopefully in a distant ballpark to get the calibration process started. +- See [AprilTags.jl Calibration section](https://juliarobotics.org/AprilTags.jl/latest/#Camera-Calibration-1) for code and help. +""" +function CameraCalibration(img::AbstractMatrix{T}) where {T} + height, width = size(img) + # emperical assumption usually seen for focal length + f_w = f_h = 1.1 * height + c_w, c_h = width / 2, height / 2 + K = @SMatrix [ + f_w 0.0 c_w + 0.0 f_h c_h + 0.0 0.0 1.0 + ] + @info "Assuming default CameraCalibration from image size(img)=(rows,cols)=$(size(img)):" f_w f_h c_w c_h + return CameraCalibration(; width, height, K) +end + +""" + $(SIGNATURES) + +Mutable version of CameraCalibration(img) helper function. +""" +function CameraCalibrationMutable(img::AbstractMatrix{T}) where {T} + height, width = size(img) + # emperical assumption usually seen for focal length + f_w = f_h = 1.1 * height + c_w, c_h = width / 2, height / 2 + K = @MMatrix [ + f_w 0.0 c_w + 0.0 f_h c_h + 0.0 0.0 1.0 + ] + @info "Assuming default CameraCalibrationMutable from image size(img)=(rows,cols)=$(size(img)):" f_w f_h c_w c_h + return CameraCalibrationMutable(; width, height, K) +end diff --git a/src/services/Prototypes.jl b/src/services/Prototypes.jl deleted file mode 100644 index 50db479..0000000 --- a/src/services/Prototypes.jl +++ /dev/null @@ -1,68 +0,0 @@ - -""" - origin(ray) - -Return the direction of the ray as a (Vector3) -""" -function origin end - -""" - direction(ray) - -Return the origin of ray, typically just a zero $(Point3) for normal cameras. -""" -function direction end - -""" - columns(model::AbstractCameraModel) - -Returns the width of the camera sensor. -""" -function width end - -""" - rows(model::AbstractCameraModel) - -Returns the height of the camera sensor. -""" -function height end - -""" - sensorsize(model::AbstractCameraModel) - -Return the size of the camera sensor. By default calling out to columns(model) and rows(model) to build an SVector{2} - -`sensorsize(cameramodel::AbstractCameraModel) = SVector{2}(columns(cameramodel), rows(cameramodel))` -""" -function sensorsize end - -""" - pixel2ray(cameramodel::AbstractCameraModel, pixelIndex::$(PixelIndex)) - -Returns the ray in space (origin + direction) corresponding to this `pixelIndex`. -""" -function pixel2ray end -function backproject end - -""" - point2pixel(camera::AbstractCameraModel, pointincamera::$(Point3)) - -Returns the pixel location onto which the 3D coordinate `pointincamera` is projected. -""" -function point2pixel end -function project end - -""" - lookdirection(camera::AbstractCameraModel) - -Return the lookdirection of this camera model. -""" -function lookdirection end - -""" - updirection(camera::AbstractCameraModel) - -Return the updirection of this camera model. -""" -function updirection end - diff --git a/src/services/Utils.jl b/src/services/Utils.jl index 0338177..b2f82fa 100644 --- a/src/services/Utils.jl +++ b/src/services/Utils.jl @@ -1,25 +1,18 @@ - -toNonhomogeneous(_Ph::AbstractVector) = SVector((_Ph[1:end-1]...,) ./ _Ph[end]) - - """ - CameraModel(width,height,fc,cc,skew,kc) + $(SIGNATURES) -Constructor helper for creating a camera model. +Converts a homogeneous point to a non-homogeneous point, i.e. divides by the last element. """ -function CameraSkewDistortion(width,height,fc,cc,skew,kc) - KK = [fc[1] skew cc[1]; - 0 fc[2] cc[2]; - 0 0 1] - # KK = [fc[1] skew*fc[1] cc[1]; - # 0 fc[2] cc[2]; - # 0 0 1] - Ki = inv(KK) - CameraModelandParameters(width,height,fc,cc,skew,kc,KK,Ki) +function toNonhomogeneous(_Ph::AbstractVector) + if length(_Ph) == 4 + return Point3(_Ph[1] / _Ph[4], _Ph[2] / _Ph[4], _Ph[3] / _Ph[4]) + end + return Point2(_Ph[1] / _Ph[3], _Ph[2] / _Ph[3]) end + """ - $SIGNATURES + $(SIGNATURES) Slightly general Radial Distortion type, currently limited to StaticArrays.jl on CPU, @@ -50,59 +43,107 @@ yu = yc + (yd + yc) / (1 + K1*(r^2) + K2*(r^4) + ...) ``` DevNotes (Contributions welcome): -- TODO manage image clamping if `dest` is too small and data should be cropped out. -- TODO buffer radii matrix for better reuse on repeat image size sequences -- TODO dispatch with either CUDA.jl or AMDGPU.jl <:AbstractArray objects. -- TODO use Tullio.jl with multithreading and GPU -- TODO check if LoopVectorization.jl tools like `@avx` help performance +- TODO : provide a GPU implementation + -> either via dispatch with AMDGPU and CUDA (should be avoided) + -> or using a vendor-agnostic code with KernelAbstractions.jl """ function radialDistortion!( - cc::CameraCalibration{<:Real,N}, - dest::AbstractMatrix, - src::AbstractMatrix -) where N - # loop over entire image - for h_d in size(src,1), w_d in size(src,2) - # temporary coordinates - @inbounds h_ = h_d - cc.center[1] - @inbounds w_ = w_d - cc.center[2] - # calculate the radius from distortion center - _radius2 = h_^2 + w_^2 - # calculate the denominator - _denomin = 1 - @inbounds @fastmath for k in 1:N - _denomin += cc.kc[k]*(_radius2^k) + cc::CameraCalibration{<:Real, N}, + dest::AbstractMatrix, + src::AbstractMatrix + ) where {N} + + h, w = size(dest) + @assert size(src) == (h, w) "Sizes of src and dest must be the same" + @assert (cc.height, cc.width) == (h, w) "Calibration size mismatches image size" + inv_fx = inv(cc.K[1, 1]) + inv_fy = inv(cc.K[2, 2]) + cx = cc.K[1, 3] + cy = cc.K[2, 3] + k1, k2, p1, p2, k3 = cc.kc + @tturbo for u in 1:w + # Calculate x_norm once per column + x_norm = (u - cx) * inv_fx + x_sq = x_norm^2 + + for v in 1:h + y_norm = (v - cy) * inv_fy + y_sq = y_norm^2 + + # --- Distortion Logic (Brown-Conrady) --- + r_sq = x_sq + y_sq + + # Radial component: (1 + k1*r^2 + k2*r^4 + k3*r^6) + radial_term = 1.0 + r_sq * (k1 + r_sq * (k2 + r_sq * k3)) + #radial_term = evalpoly(r_sq, (1, k1, k2, k3)) + + # Tangential component + xy_2 = 2.0 * x_norm * y_norm + x_tangential = p1 * xy_2 + p2 * (r_sq + 2.0 * x_sq) + y_tangential = p1 * (r_sq + 2.0 * y_sq) + p2 * xy_2 + + # Distorted normalized coordinates + x_dist = x_norm * radial_term + x_tangential + y_dist = y_norm * radial_term + y_tangential + + # --- Project back to pixel coordinates --- + # Nearest neighbor interpolation + u_src = round(Int, (x_dist * cc.K[1, 1]) + cx) + v_src = round(Int, (y_dist * cc.K[2, 2]) + cy) + + # no bounds check -> not ideal + + @inbounds dest[v, u] = src[v_src, u_src] + + # with bounds check : + #if 1 <= u_src <= w && 1 <= v_src <= h + #else + # # Pixel is out of bounds (black border) + # @inbounds dest[v, u] = 0 + #end end - # calculate the new 'undistorted' coordinates and set equal to incoming image - @inbounds @fastmath h_u = cc.center[1] + h_/_denomin - @inbounds @fastmath w_u = cc.center[2] + h_/_denomin - dest[h_u,w_u] = src[h_d,w_d] end - nothing + return nothing end +""" + intersectLineToPlane3D(planenorm, planepnt, raydir, raypnt) -> point + +Compute the unique intersection point between an infinite 3-D line (ray) and a plane. +# Arguments +- `planenorm::AbstractVector{<:Real}` – plane normal vector (need not be unit-length) +- `planepnt::AbstractVector{<:Real}` – any point lying on the plane +- `raydir::AbstractVector{<:Real}` – direction vector of the line (need not be unit-length) +- `raypnt::AbstractVector{<:Real}` – any point lying on the line +# Returns +- `ψ::Vector{<:Real}` – coordinates of the intersection point +# Throws +- `ErrorException` if the line is parallel to the plane (no intersection or line lies in plane). +""" function intersectLineToPlane3D( - planenorm::AbstractVector{<:Real}, - planepnt::AbstractVector{<:Real}, - raydir::AbstractVector{<:Real}, - raypnt::AbstractVector{<:Real} -) - ndotu = dot(planenorm, raydir) - if ndotu ≈ 0 error("no intersection or line is within plane") end + planenorm::AbstractVector{<:Real}, + planepnt::AbstractVector{<:Real}, + raydir::AbstractVector{<:Real}, + raypnt::AbstractVector{<:Real} + ) + n_dot_u = dot(planenorm, raydir) + if n_dot_u ≈ 0 + error("no intersection or line is within plane") + end - w = raypnt - planepnt - si = -dot(planenorm, w) / ndotu - ψ = w .+ si .* raydir .+ planepnt + w = raypnt - planepnt + si = -dot(planenorm, w) / n_dot_u # ray parameter at intersection + ψ = w .+ si .* raydir .+ planepnt return ψ end """ - $SIGNATURES + $(SIGNATURES) -Ray trace from pixel coords to a floor in local level reference which is assumed +Ray trace from pixel coords to a floor in local level reference which is assumed aligned with gravity. Returns intersect in local level frame (coordinates). Notes @@ -110,7 +151,7 @@ Notes - Just assume world is local level, i.e. `l_nFL = w_nFL` and `l_FL = w_FL`. - User must provide (assumed dynamic) local level transform via `l_T_ex` -- see example below! - Coordinate chain used is from ( pixel-array (a) --> camera (c) --> extrinsic (ex) --> level (l) ) - - `c_H_a` from pixel-array to camera (as homography matrix) + - `c_H_a` from pixel-array to camera (as homography matrix) - `a_F` feature in array pixel frame - `l_T_ex` extrinsic in body (or extrinsic to local level), SE3 Manifold element using ArrayPartition - `ex_T_c` camera in extrinsic (or camera to extrinsic) @@ -127,7 +168,7 @@ ci,cj = 360,640 # assuming 720x1280 image c_H_a = [0 1 -cj; 1 0 -ci; 0 0 f] # camera matrix # body to extrinsic of camera -- e.g. camera looking down 0.2 and left 0.2 -# local level to body to extrinsic transform +# local level to body to extrinsic transform l_T_b = ArrayPartition([0;0;0.], R0) b_T_ex = ArrayPartition([0;0;0.], exp_lie(Mr, hat(Mr, R0, [0;0.2;0.2]))) l_T_ex = compose(M, l_T_b, b_T_ex) # this is where body reference is folded in. @@ -149,30 +190,42 @@ l_Forb = intersectRayToPlane( See also: `CameraModels.intersectLineToPlane3D` """ function intersectRayToPlane( - c_H_a::AbstractMatrix{<:Real}, - a_F::AbstractVector{<:Real}, - l_nFL::AbstractVector{<:Real}, - l_FL::AbstractVector{<:Real}; - M = SpecialEuclideanGroup(3; variant = :right), - l_T_ex = ArrayPartition([0;0;0.], exp(SpecialOrthogonalGroup(3), hat(LieAlgebra(SpecialOrthogonalGroup(3)), [0;0.2;0.]))), - ex_T_c = ArrayPartition([0;0;0.], [0 0 1; -1 0 0; 0 -1 0.]), -) + c_H_a::AbstractMatrix{<:Real}, + a_F::AbstractVector{<:Real}, + l_nFL::AbstractVector{<:Real}, + l_FL::AbstractVector{<:Real}; + M = SpecialEuclideanGroup(3; variant = :right), + l_T_ex = ArrayPartition( + [0, 0, 0.0], + exp( + SpecialOrthogonalGroup(3), + hat(LieAlgebra(SpecialOrthogonalGroup(3)), [0, 0.2, 0.0]) + ) + ), + ex_T_c = ArrayPartition( + [0, 0, 0.0], + [ + 0.0 0.0 1.0 + -1.0 0.0 0.0 + 0.0 -1.0 0.0 + ] + ), + ) # camera in level (or camera to level) manifold element as ArrayPartition l_T_c = compose(M, l_T_ex, ex_T_c) - + ## convert pixel location to ray in camera then level frame c_F = c_H_a * a_F - # unit vector ray direction from camera in camera coordinates + # unit vector ray direction from camera in camera coordinates c_uV = c_F ./ norm(c_F) # get ray direction in level coordinates l_uV = l_T_c.x[2] * c_uV - + # and ray position from camera center in level frame l_P = l_T_c.x[1] - + ## planar floor surface projection l_nFL_ = l_nFL ./ norm(l_nFL) - - intersectLineToPlane3D(l_nFL_, l_FL, l_uV, l_P) # returns intersect in local level. -end + return intersectLineToPlane3D(l_nFL_, l_FL, l_uV, l_P) # returns intersect in local level. +end diff --git a/test/CameraTestBench.jl b/test/CameraTestBench.jl index cb7d9dd..4fc833f 100644 --- a/test/CameraTestBench.jl +++ b/test/CameraTestBench.jl @@ -1,18 +1,21 @@ - using LinearAlgebra -function run_test_bench(model::C, pixel_accuracy::Float64 = 1e-5, ray_accuracy::Float64 = 1e-4) where C <: CameraModel +function run_test_bench( + model::C, + pixel_accuracy::Float64 = 1.0e-5, + ray_accuracy::Float64 = 1.0e-4 + ) where {C <: AbstractCameraModel} - @testset "Check basics and interface implementation for $(C)." begin + return @testset "Check basics and interface implementation for $(C)." begin forward = lookdirection(model) up = updirection(model) right = cross(forward, up) @testset "Test model axes" begin - @test norm(forward) ≈ 1.0 - @test norm(up) ≈ 1.0 - @test norm(right) ≈ 1.0 + @test norm(forward) ≈ 1.0 + @test norm(up) ≈ 1.0 + @test norm(right) ≈ 1.0 end pixelsize = sensorsize(model) @@ -23,22 +26,26 @@ function run_test_bench(model::C, pixel_accuracy::Float64 = 1e-5, ray_accuracy:: some_pixel_location = image_centre + slight_offset # Get the ray that belongs to that pixel. - ray = pixel2ray(model, some_pixel_location) - + # backproject returns a direction vector, + # and we assume origin is (0,0,0) for now if not specified + # actually Ray is defined as struct Ray origin::Point3 direction::Vector3 + dir = backproject(model, some_pixel_location) + ray = Ray(Point3(0, 0, 0), dir) + # Generate a 3D point along that ray. point = CameraModels.origin(ray) + 4.2 .* CameraModels.direction(ray) println(model) - # Some models might not implement point2pixel. + # Some models might not implement project. if canreproject(model) - reprojection = point2pixel(model, point) # CameraModels.project(model, [-point[2]; -point[3]; point[1]]) # point2pixel(model, point) + reprojection = project(model, point) @test_broken some_pixel_location[1] ≈ reprojection[1] atol = pixel_accuracy @test_broken some_pixel_location[2] ≈ reprojection[2] atol = pixel_accuracy - else - @info "point2pixel not implemented for $(C)." + else + @info "project not implemented for $(C)." end end -end \ No newline at end of file +end diff --git a/test/Pinhole.jl b/test/Pinhole.jl index 522a821..b2e5b57 100644 --- a/test/Pinhole.jl +++ b/test/Pinhole.jl @@ -7,40 +7,3 @@ @test height(ccm) == 80 @test width(ccm) == 120 end - - -principal_point = PixelIndex(55.4, 49.6) -focal_length = CameraModels.Vector2(61.2, 66.4) - - -model = Pinhole(100, 100, principal_point, focal_length) - - -run_test_bench(model) - -@testset "Check Legacy Pinhole model." begin - @show some_point = CameraModels.Point3(0, 1, 0) - @show should_be_principal_point = point2pixel(model, some_point) - @test_broken principal_point[1] ≈ should_be_principal_point[1] - @test_broken principal_point[2] ≈ should_be_principal_point[2] - - ray = pixel2ray(model, principal_point) - @test CameraModels.direction(ray) ≈ CameraModels.Vector3(0,1,0) - @test CameraModels.origin(ray) ≈ CameraModels.Point3(0,0,0) - -end - -@testset "Check Legacy PinholeCamera" begin - - PinholeCamera() - - @info "Legacy test on PinholeCamera from generic image" - img = zeros(80,120) # a landscape image size (height, width) as per Images.jl - ccm = PinholeCamera(img) - - @test height(ccm) == 80 - @test width(ccm) == 120 - -end - -# \ No newline at end of file diff --git a/test/SkewDistortion.jl b/test/SkewDistortion.jl deleted file mode 100644 index ff9134a..0000000 --- a/test/SkewDistortion.jl +++ /dev/null @@ -1,22 +0,0 @@ - - - -## Tests from SensorFeatureTracking.jl - -@testset "Test CameraSkewDistortion" begin - -focald = 520.0 -cu = 320.0 -cv = 240.0 -cam = CameraSkewDistortion(640,480,[focald, focald],[cv, cu], 0., [0]) - - -focald = 520.0 -cu = 0.0 -cv = 0.0 # centred around zero for test data -cam = CameraSkewDistortion(640,480,[focald, focald],[cv, cu], 0., [0]) - - -end - -# \ No newline at end of file diff --git a/test/multiview_manifolds.jl b/test/multiview_manifolds.jl index 97211e9..4cbee4c 100644 --- a/test/multiview_manifolds.jl +++ b/test/multiview_manifolds.jl @@ -1,99 +1,61 @@ - -# - -# using Revise using Test import CameraModels using Optim, LieGroups using StaticArrays using LieGroups: SpecialEuclideanProductPoint -# using ManifoldDiff -# import FiniteDifferences as FD - -## M = SpecialEuclideanGroup(3; variant = :right) -## @testset "Multiview optimization of point in front of 2 cameras" begin -## - -cam = CameraModels.CameraCalibration() - -obs1 = CameraModels.PixelIndex(240, 320) -obs2 = CameraModels.PixelIndex(240, 315) - -w_T_c1 = ArrayPartition([0; 0 ;0.],[0 0 1; -1 0 0; 0 -1 0.]) -w_T_c2 = ArrayPartition([0;-0.1;0.],[0 0 1; -1 0 0; 0 -1 0.]) - -# w -# [ -# 0 -0.1 0 -# ] -# = -# w -# [ -# 0 0 1 -# -1 0 0 -# 0 -1 0 -# ] -# c -# [ -# 0.1 0 0 -# ] - -## - -function projectPointFrom(cam, c_H_w, w_Ph) - c_Ph = c_H_w*w_Ph |> SVector{4} - CameraModels.projectHomogeneous(cam,c_Ph) -end -function cameraResidual(cam, meas, M, w_T_c, w_Ph, κ=1000) - pred = projectPointFrom(cam, inv(convert(AbstractMatrix, SpecialEuclideanProductPoint(w_T_c))), w_Ph) - # experimental cost function to try force bad reprojects in front of the camera during optimization - κ*(abs(pred.depth) - pred.depth)^2 + (meas[1]-pred[1])^2 + (meas[2]-pred[2])^2 -end + cam = CameraModels.CameraCalibration() -function cost(w_Ph) - cameraResidual(cam, obs1, M, w_T_c1, w_Ph) + cameraResidual(cam, obs2, M, w_T_c2, w_Ph) -end + obs1 = CameraModels.PixelIndex(320, 240) + obs2 = CameraModels.PixelIndex(315, 240) -## + w_T_c1 = ArrayPartition([0; 0 ;0.0], [0 0 1; -1 0 0; 0 -1 0.0]) + w_T_c2 = ArrayPartition([0;-0.1;0.0], [0 0 1; -1 0 0; 0 -1 0.0]) -w_Ph = SVector(10.,0.,0.,1.) -cost(w_Ph) + function projectPointFrom(cam, c_H_w, w_Ph) + c_Ph = c_H_w * w_Ph |> SVector{4} + CameraModels.projectHomogeneous(cam, c_Ph) + end -cost(SVector(0.1,0.,0.,1.)) -cost(SVector(0.5,0.,0.,1.)) + function cameraResidual(cam, meas, M, w_T_c, w_Ph, κ = 1000) + pred = projectPointFrom(cam, inv(convert(AbstractMatrix, SpecialEuclideanProductPoint(w_T_c))), w_Ph) + # experimental cost function to try force bad reprojects in front of the camera during optimization + κ * (abs(pred.depth) - pred.depth)^2 + (meas[1] - pred[1])^2 + (meas[2] - pred[2])^2 + end -## + function cost(w_P) + w_Ph = if length(w_P) == 3 + SVector(w_P[1], w_P[2], w_P[3], 1.0) + else + w_P + end + cameraResidual(cam, obs1, M, w_T_c1, w_Ph) + cameraResidual(cam, obs2, M, w_T_c2, w_Ph) + end -w_Res = Optim.optimize( - cost, - [1;0;0;1.], - LBFGS(), - # ParticleSwarm(; lower = [0.,-1.,-1.,0.], - # upper = [99999.;1;1;9999], - # n_particles = 10), - # Optim.Options(g_tol = 1e-12, - # iterations = 100, - # store_trace = false, - # show_trace = true); - # autodiff=:forward -) + w_Ph = SVector(10.0, 0.0, 0.0, 1.0) -@show w_Res.minimizer + cost(w_Ph) -## + cost(SVector(0.1, 0.0, 0.0, 1.0)) + cost(SVector(0.5, 0.0, 0.0, 1.0)) -@show w_P3 = w_Res.minimizer |> CameraModels.toNonhomogeneous -@test isapprox([10.56;0;0], w_P3; atol=1e-3) -## -end + w_Res = Optim.optimize( + cost, + [1.0;0.0;0.0], + LBFGS(), + ) -## \ No newline at end of file + # @show w_Res.minimizer + # @show w_P3 = w_Res.minimizer + w_P3 = w_Res.minimizer + @test isapprox([10.56;0;0], w_P3; atol = 1.0e-3) + +end diff --git a/test/runtests.jl b/test/runtests.jl index 206ab18..e3dc222 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -11,16 +11,12 @@ CameraModels.height(m::SomeTestModel) = 11 CameraModels.width(m::SomeTestModel) = 22 @testset "Test sensorsize using rows and columns." begin - model = SomeTestModel() - @test sensorsize(model) == SVector{2}(22,11) + @test sensorsize(model) == SVector{2}(22, 11) end - include("testutils.jl") include("multiview_manifolds.jl") include("CameraTestBench.jl") include("Pinhole.jl") -include("SkewDistortion.jl") - diff --git a/test/testutils.jl b/test/testutils.jl index 5a69c1c..16bf28d 100644 --- a/test/testutils.jl +++ b/test/testutils.jl @@ -2,63 +2,100 @@ using Test using CameraModels import LieGroups using LieGroups: - SpecialEuclideanGroup, - SpecialOrthogonalGroup, - hat, - exp, - compose, - LieAlgebra + SpecialEuclideanGroup, + SpecialOrthogonalGroup, + hat, + exp, + compose, + LieAlgebra @testset "Test intersect of line and plane" begin -# Define plane -floornorm = [0;0;1.] -floorcenter = [0;0;5.] -# Define ray -raydir = [0;-1;-1.] -raypnt = [0; 0;10.] + # Define plane + floornorm = [0, 0, 1.0] + floorcenter = [0, 0, 5.0] + # Define ray + raydir = [0, -1, -1.0] + raypnt = [0, 0, 10.0] -pt = intersectLineToPlane3D(floornorm, floorcenter, raydir, raypnt) -@test isapprox([0;-5;5.], pt) + pt = intersectLineToPlane3D(floornorm, floorcenter, raydir, raypnt) + @test isapprox([0, -5, 5.0], pt) end @testset "Test raytracing to plane" begin -M = SpecialEuclideanGroup(3; variant = :right) -Mr = SpecialOrthogonalGroup(3) -R0 = [1 0 0; 0 1 0; 0 0 1.] - - -## Camera setup -f = 800.0 # pixels -ci,cj = 360,640 # assuming 720x1280 image -# going from imaging array to camera frame -c_H_a = [0 1 -cj; 1 0 -ci; 0 0 f] # camera matrix -a_Forb = [360; 640; 1.0] -l_nFL = [0; -0.05; 1.] -l_FL = [0; 0; -2.] - -# local level to body to extrinsic transform -l_T_b = ArrayPartition([0;0;0.], R0) -b_T_ex = ArrayPartition([0;0;0.], exp(Mr, hat(LieAlgebra(Mr), [0;0.2;0.2]))) -l_T_ex = compose(M, l_T_b, b_T_ex) - -# Ray trace -l_Forb = intersectRayToPlane( - c_H_a, - a_Forb, - l_nFL, - l_FL; - l_T_ex -) - - -## Place the body somewhere in the world -w_T_b = ArrayPartition([0.;0.;2.], LieGroups.exp(Mr, LieGroups.hat(LieAlgebra(Mr), [0;0;0.]))) -# find feature points in the world frame -# _w_Forb = MJL.affine_matrix(M, w_T_b)*[l_Forb; 1.] - -end \ No newline at end of file + M = SpecialEuclideanGroup(3; variant = :right) + Mr = SpecialOrthogonalGroup(3) + R0 = [ + 1.0 0.0 0.0 + 0.0 1.0 0.0 + 0.0 0.0 1.0 + ] + + ## Camera setup + f = 800.0 # pixels + ci, cj = 360, 640 # assuming 720x1280 image + # going from imaging array to camera frame + c_H_a = [ + 0 1 -cj + 1 0 -ci + 0 0 f + ] # camera matrix + + a_Forb = [360, 640, 1.0] + l_nFL = [0, -0.05, 1.0] + l_FL = [0, 0, -2.0] + + # local level to body to extrinsic transform + l_T_b = ArrayPartition([0, 0, 0.0], R0) + b_T_ex = ArrayPartition( + [0;0;0.0], + exp(Mr, hat(LieAlgebra(Mr), [0, 0.2, 0.2])) + ) + l_T_ex = compose(M, l_T_b, b_T_ex) + + # Ray trace + l_Forb = intersectRayToPlane( + c_H_a, + a_Forb, + l_nFL, + l_FL; + l_T_ex + ) + + + ## Place the body somewhere in the world + w_T_b = ArrayPartition( + [0.0, 0.0, 2.0], + LieGroups.exp(Mr, LieGroups.hat(LieAlgebra(Mr), [0, 0, 0.0])) + ) + # find feature points in the world frame + # _w_Forb = MJL.affine_matrix(M, w_T_b)*[l_Forb; 1.] + + @test !isnothing(l_Forb) +end + + +@testset "radialDistortion! function test" begin + + test_camera = CameraCalibration( + height = 720, + width = 1280, + kc = @SVector[-0.12, 0.03, 0.0008, -0.0006, -0.004], + K = @SMatrix[ + 800.0 0.0 360.0 + 0.0 800.0 640.0 + 0.0 0.0 1.0 + ] + ) + + + src_mat = rand(Float32, 720, 1280) + dst_mat = deepcopy(src_mat) + + CameraModels.radialDistortion!(test_camera, dst_mat, src_mat) + @test dst_mat != src_mat +end