From 85df6a5ac7967a368557ff67bdf2a70d4b3e7b43 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thimot=C3=A9=20Dupuch?= Date: Sun, 16 Nov 2025 05:34:32 +0100 Subject: [PATCH 1/7] Formatting via Runic.jl --- src/CameraModels.jl | 2 - src/Deprecated.jl | 165 +++++++++++++-------------- src/ExportAPI.jl | 7 +- src/entities/CameraCalibration.jl | 56 +++++----- src/entities/GeneralTypes.jl | 12 +- src/services/CameraCalibration.jl | 37 +++--- src/services/CameraServices.jl | 180 +++++++++++++++--------------- src/services/Prototypes.jl | 2 - src/services/Utils.jl | 86 +++++++------- 9 files changed, 258 insertions(+), 289 deletions(-) diff --git a/src/CameraModels.jl b/src/CameraModels.jl index 4982a8b..2a3679e 100644 --- a/src/CameraModels.jl +++ b/src/CameraModels.jl @@ -26,6 +26,4 @@ include("services/CameraServices.jl") include("services/Utils.jl") - - end # module diff --git a/src/Deprecated.jl b/src/Deprecated.jl index ffc3d51..2d0e49e 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -1,12 +1,11 @@ - ## ================================================================================================ ## consolidated types from various repos in Julia ecosystem ## ================================================================================================ -@deprecate project(cm::CameraModelFull, pt::AbstractVector{<:Real}) project(cm.ci,pt) # drops extrinsics +@deprecate project(cm::CameraModelFull, pt::AbstractVector{<:Real}) project(cm.ci, pt) # drops extrinsics # function project( -# cm::CameraModelFull, +# cm::CameraModelFull, # pt::AbstractVector{<:Real} # ) # res = Vector{Float64}(2) @@ -21,7 +20,7 @@ A Union type for users to implement against both `struct`` and `mutable struct` """ 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)) +@deprecate CameraExtrinsic(R::AbstractMatrix = [1 0 0; 0 1 0; 0 0 1.0], t::AbstractVector = [0, 0, 0.0]) ArrayPartition(SVector(t...), SMatrix(R)) # Camera extrinsic must be world in camera frame (cRw) # Base.@kwdef struct CameraExtrinsic{T <: Real} @@ -30,7 +29,7 @@ CameraCalibrationT = Union{<:CameraCalibration, <:CameraCalibrationMutable} # end -@deprecate PixelCoordinate(row,col) PixelIndex(row,col) +@deprecate PixelCoordinate(row, col) PixelIndex(row, col) # CameraCalibration( # height::Int= 480, @@ -52,7 +51,6 @@ CameraCalibrationT = Union{<:CameraCalibration, <:CameraCalibrationMutable} # ) = 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]] @@ -61,7 +59,7 @@ CameraCalibrationT = Union{<:CameraCalibration, <:CameraCalibrationMutable} # point2pixel(model::Pinhole, pointincamera::$(Point3)) # Return a transformation that converts real-world coordinates -# to camera coordinates. This currently ignores any tangential +# to camera coordinates. This currently ignores any tangential # distortion between the lens and the image plane. # """ # function point2pixel(model::CameraCalibrationT, pointincamera::Point3) @@ -75,7 +73,7 @@ CameraCalibrationT = Union{<:CameraCalibration, <:CameraCalibrationMutable} # pixel2ray(model::Pinhole, pixelcoordinate::$(PixelCoordinate)) # Return a transformation that converts real-world coordinates -# to camera coordinates. This currently ignores any tangential +# to camera coordinates. This currently ignores any tangential # distortion between the lens and the image plane. # """ # function pixel2ray(model::CameraCalibrationT, pixelcoordinate::PixelCoordinate) @@ -86,19 +84,19 @@ CameraCalibrationT = Union{<:CameraCalibration, <:CameraCalibrationMutable} export CameraModel # being replaced by AbstractCameraModel -CameraModel = (@warn("CameraModels.CameraModel is deprecated, use CameraModels.AbstractCameraModel instead");AbstractCameraModel) -# abstract type CameraModel end +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) # +# 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...) +@deprecate columns(w...; kw...) width(w...; kw...) +@deprecate rows(w...; kw...) height(w...; kw...) # sensorsize(cameramodel::CameraModel) = SVector{2}(width(cameramodel), height(cameramodel)) @@ -107,22 +105,21 @@ 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)) + 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.") + return 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. @@ -139,7 +136,6 @@ end # end - export PinholeCamera # ## From JuliaRobotics/Caesar.jl @@ -152,7 +148,7 @@ export PinholeCamera # - 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* # - Always follow right hand rule for everything. @@ -168,23 +164,23 @@ export PinholeCamera # 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)) + 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 + return CameraCalibrationMutable(; width, height, K = MMatrix{3, 3}(K)) end # @deprecate PinholeCamera(img::AbstractMatrix) CameraCalibrationMutable(img) @@ -216,28 +212,28 @@ 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 - ) +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.0]']) + return 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) + 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." + return CameraCalibration(; width, height, K) end export CameraIntrinsic @@ -251,23 +247,23 @@ export CameraIntrinsic 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 + x::CameraCalibration, # Union{<:Pinhole, CameraModelandParameters}, + f::Symbol, + ) + return 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 @@ -280,7 +276,7 @@ end # SA[(getfield(x, :K)[1:2, 3])...] # elseif f == :focallength || f == :fc # K = getfield(x, :K) -# SA[K[1,1];K[2,2]] +# SA[K[1,1];K[2,2]] # else # getfield(x, f) # end @@ -296,7 +292,4 @@ end # end - - - -# \ No newline at end of file +# diff --git a/src/ExportAPI.jl b/src/ExportAPI.jl index bd31a09..bac4692 100644 --- a/src/ExportAPI.jl +++ b/src/ExportAPI.jl @@ -1,6 +1,3 @@ - - - export AbstractCameraModel export CameraCalibration, CameraCalibrationMutable @@ -10,7 +7,7 @@ export CameraSkewDistortion export undistortPoint export Ray, PixelIndex -export pixel2ray, point2pixel, canreproject, sensorsize #, origin, direction, +export pixel2ray, point2pixel, canreproject, sensorsize #, origin, direction, export project, projectHomogeneous export backproject, backprojectHomogeneous export pp_w, pp_h, f_w, f_h @@ -21,4 +18,4 @@ 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 +# export Vector3, Vector2, Point3 diff --git a/src/entities/CameraCalibration.jl b/src/entities/CameraCalibration.jl index dd22c5d..df7af79 100644 --- a/src/entities/CameraCalibration.jl +++ b/src/entities/CameraCalibration.jl @@ -1,4 +1,3 @@ - """ $TYPEDEF @@ -33,17 +32,17 @@ DevNotes 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{3, 3}([[1.1 * height;0.0;width / 2]';[0.0;1.1 * height;height / 2]';[0.0;0;1.0]']) + """ inverse of a 3x3 camera calibration matrix """ + Ki::SMatrix{3, 3, R, 9} = inv(K) end @@ -52,34 +51,29 @@ end 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) +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.0]']) + """ inverse of a 3x3 camera calibration matrix """ + Ki::MMatrix{3, 3, R} = inv(K) end - - - ## =========================================================================== ## Legacy types that are not so easy to consolidate (not exported) DO NOT USE ## =========================================================================== 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() + ci::CameraCalibration = CameraCalibration() + ce::ArrayPartition = ArrayPartition(SVector(0.0, 0.0, 0.0), SMatrix{3, 3}(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)) # CameraExtrinsic() end - - -# \ No newline at end of file +# diff --git a/src/entities/GeneralTypes.jl b/src/entities/GeneralTypes.jl index 2b6af02..a358625 100644 --- a/src/entities/GeneralTypes.jl +++ b/src/entities/GeneralTypes.jl @@ -1,16 +1,14 @@ - - struct PixelIndex{VALID, T <: Real} row::T 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) +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 +function Base.getindex(p::PixelIndex, i::Int) + return if i === 1 p.row - elseif i === 2 + elseif i === 2 p.col elseif i === 3 p.depth @@ -32,4 +30,4 @@ origin3d = zeros(Point3) 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 index f79aa74..c286f95 100644 --- a/src/services/CameraCalibration.jl +++ b/src/services/CameraCalibration.jl @@ -1,20 +1,18 @@ - 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 + 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, "}") + 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) - """ $SIGNATURES @@ -24,14 +22,13 @@ 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) +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.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/CameraServices.jl b/src/services/CameraServices.jl index b03eaac..7d014c2 100644 --- a/src/services/CameraServices.jl +++ b/src/services/CameraServices.jl @@ -1,32 +1,31 @@ - ## consolidated functions, first baseline ## ========================================================================================= ## Parameter functions ## From yakir12/CameraModels.jl -origin(vector::Union{<:AbstractVector{<:Real},<:Vector3}) = origin3d +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) +lookdirection(cameramodel::AbstractCameraModel) = SVector{3}(0, 1, 0) +updirection(cameramodel::AbstractCameraModel) = SVector{3}(0, 0, 1) width(cameramodel::AbstractCameraModel) = cameramodel.width -height(cameramodel::AbstractCameraModel) = cameramodel.height -direction(vector::Union{<:AbstractVector{<:Real},<:Vector3}) = vector +height(cameramodel::AbstractCameraModel) = cameramodel.height +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] - -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) +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] + +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) """ canreproject(camera::CameraModel) @@ -44,24 +43,24 @@ canreproject(camera::AbstractCameraModel) = true ## ========================================================================================= -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] +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] end @@ -72,14 +71,14 @@ end ## From JuliaRobotics/SensorFeatureTracking.jl 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, #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] + return PixelIndex(ret[1], ret[2]) end """ @@ -101,45 +100,45 @@ Deprecates: 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(SVector(0.0, 0.0, 0.0), SMatrix{3, 3}(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)) + ) + # + ret = MVector(0.0, 0.0) + return 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) end project!( - ret::AbstractVector{<:Real}, - cm::CameraModelFull, - pt::AbstractVector{<:Real}) = project!(ret, cm.ci, cm.ce, pt) + ret::AbstractVector{<:Real}, + cm::CameraModelFull, + pt::AbstractVector{<:Real} +) = project!(ret, cm.ci, cm.ce, pt) ## homogeneous point coords xyzw (stereo cameras) # xyzw are in the camera frame (c_), i.e. x-columns, y-rows, z-forward 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 Date: Sun, 16 Nov 2025 05:41:48 +0100 Subject: [PATCH 2/7] improved docs for intersectLineToPlane3D + getindex refactoring --- src/entities/GeneralTypes.jl | 16 +++++----------- src/services/Utils.jl | 22 +++++++++++++++++++--- 2 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/entities/GeneralTypes.jl b/src/entities/GeneralTypes.jl index a358625..55a652b 100644 --- a/src/entities/GeneralTypes.jl +++ b/src/entities/GeneralTypes.jl @@ -5,17 +5,11 @@ struct PixelIndex{VALID, T <: Real} 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) - return 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 +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")) const Vector2 = SVector{2, Float64} diff --git a/src/services/Utils.jl b/src/services/Utils.jl index 6c45ffb..14a4aef 100644 --- a/src/services/Utils.jl +++ b/src/services/Utils.jl @@ -82,20 +82,36 @@ function radialDistortion!( 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 + 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 + si = -dot(planenorm, w) / n_dot_u # ray parameter at intersection ψ = w .+ si .* raydir .+ planepnt return ψ end From 745cbd0a29e95e220ff4b20c2874048cb57e8d97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thimot=C3=A9=20Dupuch?= Date: Sun, 16 Nov 2025 06:23:57 +0100 Subject: [PATCH 3/7] CPU imlementation for radialDistortion! with @tturbo --- Project.toml | 3 +- src/CameraModels.jl | 1 + src/entities/CameraCalibration.jl | 4 +-- src/services/Utils.jl | 51 +++++++++++++++++++------------ 4 files changed, 36 insertions(+), 23 deletions(-) diff --git a/Project.toml b/Project.toml index e080a07..025e739 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ 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] @@ -10,6 +10,7 @@ Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" +# TODO : add LoopVectorization here [compat] DocStringExtensions = "0.8, 0.9" diff --git a/src/CameraModels.jl b/src/CameraModels.jl index 2a3679e..cd2625f 100644 --- a/src/CameraModels.jl +++ b/src/CameraModels.jl @@ -7,6 +7,7 @@ using StaticArrays import Rotations as Rot_ import Base: getindex, getproperty, show using RecursiveArrayTools: ArrayPartition +using LoopVectorization: @tturbo # exports include("ExportAPI.jl") diff --git a/src/entities/CameraCalibration.jl b/src/entities/CameraCalibration.jl index df7af79..daf4d56 100644 --- a/src/entities/CameraCalibration.jl +++ b/src/entities/CameraCalibration.jl @@ -22,7 +22,7 @@ Notes - 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* + - `(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. @@ -49,7 +49,7 @@ end """ $TYPEDEF -See [`CameraCalibraton`](@ref). +See [`CameraCalibration`](@ref). """ Base.@kwdef mutable struct CameraCalibrationMutable{R <: Real, N} <: AbstractCameraModel """ number of pixels from top to bottom """ diff --git a/src/services/Utils.jl b/src/services/Utils.jl index 14a4aef..7c6f48f 100644 --- a/src/services/Utils.jl +++ b/src/services/Utils.jl @@ -51,37 +51,48 @@ 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) + + center = SVector{2}(cc.center) + k = ntuple(i -> cc.kc[i], N) + H, W = size(src) + c₁, c₂ = center + + # pre-allocate buffers for the radius powers + buf = Vector{T}(undef, max(H, W)) + + @tturbo for h_d in 1:H, w_d in 1:W + h_ = h_d - c₁ + w_ = w_d - c₂ + r² = h_ * h_ + w_ * w_ + + num = one(T) + rⁿ = r² + @inbounds for i in 1:N + num += k[i] * rⁿ + rⁿ *= r² 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] + + h_u = c₁ + h_ / num + w_u = c₂ + w_ / num + + # nearest-neighbour lookup (round + clamp) + hi = clamp(round(Int, h_u), 1, H) + wi = clamp(round(Int, w_u), 1, W) + dest[h_d, w_d] = src[hi, wi] end return nothing end + """ intersectLineToPlane3D(planenorm, planepnt, raydir, raypnt) -> point From 5b9c433321ff13b5438f685d467c4666a2d3df60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thimot=C3=A9=20Dupuch?= Date: Sun, 16 Nov 2025 06:54:11 +0100 Subject: [PATCH 4/7] add LoopVectorization to the project's dependencies --- Project.toml | 3 ++- src/services/Utils.jl | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Project.toml b/Project.toml index 025e739..4b7aa99 100644 --- a/Project.toml +++ b/Project.toml @@ -6,14 +6,15 @@ version = "0.2.4" [deps] DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +LoopVectorization = "bdcacae8-1622-11e9-2a5c-532679323890" Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" -# TODO : add LoopVectorization here [compat] DocStringExtensions = "0.8, 0.9" +LoopVectorization = "0.12.173" Manifolds = "0.10, 0.11" RecursiveArrayTools = "3.27.0" Rotations = "1" diff --git a/src/services/Utils.jl b/src/services/Utils.jl index 7c6f48f..8d72a5a 100644 --- a/src/services/Utils.jl +++ b/src/services/Utils.jl @@ -76,7 +76,7 @@ function radialDistortion!( num = one(T) rⁿ = r² - @inbounds for i in 1:N + for i in 1:N num += k[i] * rⁿ rⁿ *= r² end From 03ce8bb53fec54071a4eda615ebd3e39468c8655 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thimot=C3=A9=20Dupuch?= Date: Wed, 10 Dec 2025 14:52:12 +0100 Subject: [PATCH 5/7] Refactor + test for radialDistortion! --- src/services/Utils.jl | 97 ++++++++++++++++++++------------- test/runtests.jl | 6 +-- test/testutils.jl | 122 +++++++++++++++++++++++++----------------- 3 files changed, 134 insertions(+), 91 deletions(-) diff --git a/src/services/Utils.jl b/src/services/Utils.jl index af5f391..a38561c 100644 --- a/src/services/Utils.jl +++ b/src/services/Utils.jl @@ -61,38 +61,59 @@ function radialDistortion!( src::AbstractMatrix ) where {N} - center = SVector{2}(cc.center) - k = ntuple(i -> cc.kc[i], N) - H, W = size(src) - c₁, c₂ = center - - # pre-allocate buffers for the radius powers - buf = Vector{T}(undef, max(H, W)) - - @tturbo for h_d in 1:H, w_d in 1:W - h_ = h_d - c₁ - w_ = w_d - c₂ - r² = h_ * h_ + w_ * w_ - - num = one(T) - rⁿ = r² - for i in 1:N - num += k[i] * rⁿ - rⁿ *= r² + 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 - - h_u = c₁ + h_ / num - w_u = c₂ + w_ / num - - # nearest-neighbour lookup (round + clamp) - hi = clamp(round(Int, h_u), 1, H) - wi = clamp(round(Int, w_u), 1, W) - dest[h_d, w_d] = src[hi, wi] end return nothing end - """ intersectLineToPlane3D(planenorm, planepnt, raydir, raypnt) -> point @@ -130,7 +151,7 @@ end """ $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 @@ -138,7 +159,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) @@ -155,7 +176,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. @@ -177,14 +198,14 @@ 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 1; -1 0 0; 0 -1 0.0]), + ) # camera in level (or camera to level) manifold element as ArrayPartition l_T_c = compose(M, l_T_ex, ex_T_c) diff --git a/test/runtests.jl b/test/runtests.jl index 206ab18..7a55bc3 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -11,16 +11,14 @@ 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..edfcb07 100644 --- a/test/testutils.jl +++ b/test/testutils.jl @@ -2,63 +2,87 @@ 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 1 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.] + +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{3, 3}( + [ + 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 = similar(src_mat) + + CameraModels.radialDistortion!(test_camera, dst_mat, src_mat) + +end From 5e95a46d2ab3cc0c815ae1c1ddd806bd0c94e978 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thimot=C3=A9=20Dupuch?= Date: Fri, 19 Dec 2025 16:01:10 +0100 Subject: [PATCH 6/7] remove deprecated code + GeometryBasics + code reorganization --- Manifest-v1.12.toml | 947 ++++++++++++++++++++++++++++++ Project.toml | 4 +- src/CameraModels.jl | 29 +- src/Deprecated.jl | 295 ---------- src/ExportAPI.jl | 4 +- src/entities/CameraCalibration.jl | 14 - src/entities/GeneralTypes.jl | 8 +- src/services/CameraServices.jl | 193 ++++-- src/services/Prototypes.jl | 66 --- src/services/Utils.jl | 28 +- test/CameraTestBench.jl | 13 +- test/Pinhole.jl | 37 -- test/SkewDistortion.jl | 7 +- test/multiview_manifolds.jl | 59 +- test/testutils.jl | 4 +- 15 files changed, 1142 insertions(+), 566 deletions(-) create mode 100644 Manifest-v1.12.toml delete mode 100644 src/Deprecated.jl delete mode 100644 src/services/Prototypes.jl diff --git a/Manifest-v1.12.toml b/Manifest-v1.12.toml new file mode 100644 index 0000000..cb33900 --- /dev/null +++ b/Manifest-v1.12.toml @@ -0,0 +1,947 @@ +# This file is machine-generated - editing it directly is not advised + +julia_version = "1.12.2" +manifest_format = "2.0" +project_hash = "d9960187b460e54c2195a0dd6dd4b32f8e7428ca" + +[[deps.ADTypes]] +git-tree-sha1 = "8b2b045b22740e4be20654175cc38291d48539db" +uuid = "47edcb42-4c32-4615-8424-f2b9edc5f35b" +version = "1.20.0" + + [deps.ADTypes.extensions] + ADTypesChainRulesCoreExt = "ChainRulesCore" + ADTypesConstructionBaseExt = "ConstructionBase" + ADTypesEnzymeCoreExt = "EnzymeCore" + + [deps.ADTypes.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + ConstructionBase = "187b0558-2788-49d3-abe0-74a17ed4e7c9" + EnzymeCore = "f151be2c-9106-41f4-ab19-57ee4f262869" + +[[deps.Accessors]] +deps = ["CompositionsBase", "ConstructionBase", "Dates", "InverseFunctions", "MacroTools"] +git-tree-sha1 = "856ecd7cebb68e5fc87abecd2326ad59f0f911f3" +uuid = "7d9f7c33-5ae7-4f3b-8dc6-eff91059b697" +version = "0.1.43" + + [deps.Accessors.extensions] + AxisKeysExt = "AxisKeys" + IntervalSetsExt = "IntervalSets" + LinearAlgebraExt = "LinearAlgebra" + StaticArraysExt = "StaticArrays" + StructArraysExt = "StructArrays" + TestExt = "Test" + UnitfulExt = "Unitful" + + [deps.Accessors.weakdeps] + AxisKeys = "94b1ba4f-4ee9-5380-92f1-94cde586c3c5" + IntervalSets = "8197267c-284f-5f27-9208-e0e47529a953" + LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" + StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" + StructArrays = "09ab397b-f2b6-538f-b94a-2f83cf4a842a" + Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" + Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d" + +[[deps.Adapt]] +deps = ["LinearAlgebra", "Requires"] +git-tree-sha1 = "7e35fca2bdfba44d797c53dfe63a51fabf39bfc0" +uuid = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" +version = "4.4.0" +weakdeps = ["SparseArrays", "StaticArrays"] + + [deps.Adapt.extensions] + AdaptSparseArraysExt = "SparseArrays" + AdaptStaticArraysExt = "StaticArrays" + +[[deps.AliasTables]] +deps = ["PtrArrays", "Random"] +git-tree-sha1 = "9876e1e164b144ca45e9e3198d0b689cadfed9ff" +uuid = "66dad0bd-aa9a-41b7-9441-69ab47430ed8" +version = "1.1.3" + +[[deps.ArgTools]] +uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" +version = "1.1.2" + +[[deps.ArnoldiMethod]] +deps = ["LinearAlgebra", "Random", "StaticArrays"] +git-tree-sha1 = "d57bd3762d308bded22c3b82d033bff85f6195c6" +uuid = "ec485272-7323-5ecc-a04f-4719b315124d" +version = "0.4.0" + +[[deps.ArrayInterface]] +deps = ["Adapt", "LinearAlgebra"] +git-tree-sha1 = "d81ae5489e13bc03567d4fbbb06c546a5e53c857" +uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9" +version = "7.22.0" + + [deps.ArrayInterface.extensions] + ArrayInterfaceBandedMatricesExt = "BandedMatrices" + ArrayInterfaceBlockBandedMatricesExt = "BlockBandedMatrices" + ArrayInterfaceCUDAExt = "CUDA" + ArrayInterfaceCUDSSExt = ["CUDSS", "CUDA"] + ArrayInterfaceChainRulesCoreExt = "ChainRulesCore" + ArrayInterfaceChainRulesExt = "ChainRules" + ArrayInterfaceGPUArraysCoreExt = "GPUArraysCore" + ArrayInterfaceMetalExt = "Metal" + ArrayInterfaceReverseDiffExt = "ReverseDiff" + ArrayInterfaceSparseArraysExt = "SparseArrays" + ArrayInterfaceStaticArraysCoreExt = "StaticArraysCore" + ArrayInterfaceTrackerExt = "Tracker" + + [deps.ArrayInterface.weakdeps] + BandedMatrices = "aae01518-5342-5314-be14-df237901396f" + BlockBandedMatrices = "ffab5731-97b5-5995-9138-79e8c1846df0" + CUDA = "052768ef-5323-5732-b1bb-66c8b64840ba" + CUDSS = "45b445bb-4962-46a0-9369-b4df9d0f772e" + ChainRules = "082447d4-558c-5d27-93f4-14fc19e9eca2" + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + GPUArraysCore = "46192b85-c4d5-4398-a991-12ede77f4527" + Metal = "dde4c033-4e86-420c-a63e-0dd931031962" + ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" + SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" + StaticArraysCore = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" + Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" + +[[deps.Artifacts]] +uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" +version = "1.11.0" + +[[deps.Base64]] +uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" +version = "1.11.0" + +[[deps.BitTwiddlingConvenienceFunctions]] +deps = ["Static"] +git-tree-sha1 = "f21cfd4950cb9f0587d5067e69405ad2acd27b87" +uuid = "62783981-4cbd-42fc-bca8-16325de8dc4b" +version = "0.1.6" + +[[deps.CPUSummary]] +deps = ["CpuId", "IfElse", "PrecompileTools", "Preferences", "Static"] +git-tree-sha1 = "f3a21d7fc84ba618a779d1ed2fcca2e682865bab" +uuid = "2a0fbf3d-bb9c-48f3-b0a9-814d99fd7ab9" +version = "0.2.7" + +[[deps.CameraModels]] +deps = ["DocStringExtensions", "LieGroups", "LinearAlgebra", "LoopVectorization", "RecursiveArrayTools", "Rotations", "StaticArrays"] +path = "." +uuid = "0d57b887-6120-40e6-8b8c-29d3116bc0c1" +version = "0.2.4" + +[[deps.CloseOpenIntervals]] +deps = ["Static", "StaticArrayInterface"] +git-tree-sha1 = "05ba0d07cd4fd8b7a39541e31a7b0254704ea581" +uuid = "fb6a15b2-703c-40df-9091-08a04967cfa9" +version = "0.1.13" + +[[deps.CommonWorldInvalidations]] +git-tree-sha1 = "ae52d1c52048455e85a387fbee9be553ec2b68d0" +uuid = "f70d9fcc-98c5-4d4a-abd7-e4cdeebd8ca8" +version = "1.0.0" + +[[deps.Compat]] +deps = ["TOML", "UUIDs"] +git-tree-sha1 = "9d8a54ce4b17aa5bdce0ea5c34bc5e7c340d16ad" +uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" +version = "4.18.1" +weakdeps = ["Dates", "LinearAlgebra"] + + [deps.Compat.extensions] + CompatLinearAlgebraExt = "LinearAlgebra" + +[[deps.CompilerSupportLibraries_jll]] +deps = ["Artifacts", "Libdl"] +uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" +version = "1.3.0+1" + +[[deps.CompositionsBase]] +git-tree-sha1 = "802bb88cd69dfd1509f6670416bd4434015693ad" +uuid = "a33af91c-f02d-484b-be07-31d278c5ca2b" +version = "0.1.2" +weakdeps = ["InverseFunctions"] + + [deps.CompositionsBase.extensions] + CompositionsBaseInverseFunctionsExt = "InverseFunctions" + +[[deps.ConstructionBase]] +git-tree-sha1 = "b4b092499347b18a015186eae3042f72267106cb" +uuid = "187b0558-2788-49d3-abe0-74a17ed4e7c9" +version = "1.6.0" + + [deps.ConstructionBase.extensions] + ConstructionBaseIntervalSetsExt = "IntervalSets" + ConstructionBaseLinearAlgebraExt = "LinearAlgebra" + ConstructionBaseStaticArraysExt = "StaticArrays" + + [deps.ConstructionBase.weakdeps] + IntervalSets = "8197267c-284f-5f27-9208-e0e47529a953" + LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" + StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" + +[[deps.CpuId]] +deps = ["Markdown"] +git-tree-sha1 = "fcbb72b032692610bfbdb15018ac16a36cf2e406" +uuid = "adafc99b-e345-5852-983c-f28acb93d879" +version = "0.3.1" + +[[deps.DataAPI]] +git-tree-sha1 = "abe83f3a2f1b857aac70ef8b269080af17764bbe" +uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" +version = "1.16.0" + +[[deps.DataStructures]] +deps = ["OrderedCollections"] +git-tree-sha1 = "e357641bb3e0638d353c4b29ea0e40ea644066a6" +uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" +version = "0.19.3" + +[[deps.Dates]] +deps = ["Printf"] +uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" +version = "1.11.0" + +[[deps.DifferentiationInterface]] +deps = ["ADTypes", "LinearAlgebra"] +git-tree-sha1 = "1d5a93ce22dfa78d202b3bd6ad8afa3d69fcd129" +uuid = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63" +version = "0.7.13" + + [deps.DifferentiationInterface.extensions] + DifferentiationInterfaceChainRulesCoreExt = "ChainRulesCore" + DifferentiationInterfaceDiffractorExt = "Diffractor" + DifferentiationInterfaceEnzymeExt = ["EnzymeCore", "Enzyme"] + DifferentiationInterfaceFastDifferentiationExt = "FastDifferentiation" + DifferentiationInterfaceFiniteDiffExt = "FiniteDiff" + DifferentiationInterfaceFiniteDifferencesExt = "FiniteDifferences" + DifferentiationInterfaceForwardDiffExt = ["ForwardDiff", "DiffResults"] + DifferentiationInterfaceGPUArraysCoreExt = "GPUArraysCore" + DifferentiationInterfaceGTPSAExt = "GTPSA" + DifferentiationInterfaceMooncakeExt = "Mooncake" + DifferentiationInterfacePolyesterForwardDiffExt = ["PolyesterForwardDiff", "ForwardDiff", "DiffResults"] + DifferentiationInterfaceReverseDiffExt = ["ReverseDiff", "DiffResults"] + DifferentiationInterfaceSparseArraysExt = "SparseArrays" + DifferentiationInterfaceSparseConnectivityTracerExt = "SparseConnectivityTracer" + DifferentiationInterfaceSparseMatrixColoringsExt = "SparseMatrixColorings" + DifferentiationInterfaceStaticArraysExt = "StaticArrays" + DifferentiationInterfaceSymbolicsExt = "Symbolics" + DifferentiationInterfaceTrackerExt = "Tracker" + DifferentiationInterfaceZygoteExt = ["Zygote", "ForwardDiff"] + + [deps.DifferentiationInterface.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + DiffResults = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" + Diffractor = "9f5e2b26-1114-432f-b630-d3fe2085c51c" + Enzyme = "7da242da-08ed-463a-9acd-ee780be4f1d9" + EnzymeCore = "f151be2c-9106-41f4-ab19-57ee4f262869" + FastDifferentiation = "eb9bf01b-bf85-4b60-bf87-ee5de06c00be" + FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" + FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000" + ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" + GPUArraysCore = "46192b85-c4d5-4398-a991-12ede77f4527" + GTPSA = "b27dd330-f138-47c5-815b-40db9dd9b6e8" + Mooncake = "da2b9cff-9c12-43a0-ae48-6db2b0edb7d6" + PolyesterForwardDiff = "98d1487c-24ca-40b6-b7ab-df2af84e126b" + ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" + SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" + SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5" + SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" + StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" + Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7" + Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" + Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" + +[[deps.DocStringExtensions]] +git-tree-sha1 = "7442a5dfe1ebb773c29cc2962a8980f47221d76c" +uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" +version = "0.9.5" + +[[deps.Downloads]] +deps = ["ArgTools", "FileWatching", "LibCURL", "NetworkOptions"] +uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" +version = "1.7.0" + +[[deps.EarCut_jll]] +deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] +git-tree-sha1 = "e3290f2d49e661fbd94046d7e3726ffcb2d41053" +uuid = "5ae413db-bbd1-5e63-b57d-d24a61df00f5" +version = "2.2.4+0" + +[[deps.Einsum]] +deps = ["Compat"] +git-tree-sha1 = "4a6b3eee0161c89700b6c1949feae8b851da5494" +uuid = "b7d42ee7-0b51-5a75-98ca-779d3107e4c0" +version = "0.4.1" + +[[deps.ExprTools]] +git-tree-sha1 = "27415f162e6028e81c72b82ef756bf321213b6ec" +uuid = "e2ba6199-217a-4e67-a87a-7c52f15ade04" +version = "0.1.10" + +[[deps.Extents]] +git-tree-sha1 = "b309b36a9e02fe7be71270dd8c0fd873625332b4" +uuid = "411431e0-e8b7-467b-b5e0-f676ba4f2910" +version = "0.1.6" + +[[deps.FileWatching]] +uuid = "7b1f6079-737a-58dc-b8bc-7a2ca5c1b5ee" +version = "1.11.0" + +[[deps.GPUArraysCore]] +deps = ["Adapt"] +git-tree-sha1 = "83cf05ab16a73219e5f6bd1bdfa9848fa24ac627" +uuid = "46192b85-c4d5-4398-a991-12ede77f4527" +version = "0.2.0" + +[[deps.GeometryBasics]] +deps = ["EarCut_jll", "Extents", "IterTools", "LinearAlgebra", "PrecompileTools", "Random", "StaticArrays"] +git-tree-sha1 = "1f5a80f4ed9f5a4aada88fc2db456e637676414b" +uuid = "5c1252a2-5f33-56bf-86c9-59e7332b4326" +version = "0.5.10" + + [deps.GeometryBasics.extensions] + GeometryBasicsGeoInterfaceExt = "GeoInterface" + + [deps.GeometryBasics.weakdeps] + GeoInterface = "cf35fbd7-0cd7-5166-be24-54bfbe79505f" + +[[deps.Graphs]] +deps = ["ArnoldiMethod", "DataStructures", "Inflate", "LinearAlgebra", "Random", "SimpleTraits", "SparseArrays", "Statistics"] +git-tree-sha1 = "d80e8b7220b884117938b2d219487eea06f9e42e" +uuid = "86223c79-3864-5bf0-83f7-82e725a168b6" +version = "1.13.2" + + [deps.Graphs.extensions] + GraphsSharedArraysExt = "SharedArrays" + + [deps.Graphs.weakdeps] + Distributed = "8ba89e20-285c-5b6f-9357-94700520ee1b" + SharedArrays = "1a1011a3-84de-559e-8e89-a11a2f7dc383" + +[[deps.HostCPUFeatures]] +deps = ["BitTwiddlingConvenienceFunctions", "IfElse", "Libdl", "Preferences", "Static"] +git-tree-sha1 = "af9ab7d1f70739a47f03be78771ebda38c3c71bf" +uuid = "3e5b6fbb-0976-4d2c-9146-d79de83f2fb0" +version = "0.1.18" + +[[deps.IfElse]] +git-tree-sha1 = "debdd00ffef04665ccbb3e150747a77560e8fad1" +uuid = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173" +version = "0.1.1" + +[[deps.Inflate]] +git-tree-sha1 = "d1b1b796e47d94588b3757fe84fbf65a5ec4a80d" +uuid = "d25df0c9-e2be-5dd7-82c8-3ad0b3e990b9" +version = "0.1.5" + +[[deps.InteractiveUtils]] +deps = ["Markdown"] +uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" +version = "1.11.0" + +[[deps.InverseFunctions]] +git-tree-sha1 = "a779299d77cd080bf77b97535acecd73e1c5e5cb" +uuid = "3587e190-3f89-42d0-90ee-14403ec27112" +version = "0.1.17" + + [deps.InverseFunctions.extensions] + InverseFunctionsDatesExt = "Dates" + InverseFunctionsTestExt = "Test" + + [deps.InverseFunctions.weakdeps] + Dates = "ade2ca70-3891-5945-98fb-dc099432e06a" + Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" + +[[deps.IrrationalConstants]] +git-tree-sha1 = "b2d91fe939cae05960e760110b328288867b5758" +uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" +version = "0.2.6" + +[[deps.IterTools]] +git-tree-sha1 = "42d5f897009e7ff2cf88db414a389e5ed1bdd023" +uuid = "c8e1da08-722c-5040-9ed9-7db0dc04731e" +version = "1.10.0" + +[[deps.JLLWrappers]] +deps = ["Artifacts", "Preferences"] +git-tree-sha1 = "0533e564aae234aff59ab625543145446d8b6ec2" +uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" +version = "1.7.1" + +[[deps.JuliaSyntaxHighlighting]] +deps = ["StyledStrings"] +uuid = "ac6e5ff7-fb65-4e79-a425-ec3bc9c03011" +version = "1.12.0" + +[[deps.Kronecker]] +deps = ["LinearAlgebra", "NamedDims", "SparseArrays", "StatsBase"] +git-tree-sha1 = "9253429e28cceae6e823bec9ffde12460d79bb38" +uuid = "2c470bb0-bcc8-11e8-3dad-c9649493f05e" +version = "0.5.5" + +[[deps.LayoutPointers]] +deps = ["ArrayInterface", "LinearAlgebra", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface"] +git-tree-sha1 = "a9eaadb366f5493a5654e843864c13d8b107548c" +uuid = "10f19ff3-798f-405d-979b-55457f8fc047" +version = "0.1.17" + +[[deps.LibCURL]] +deps = ["LibCURL_jll", "MozillaCACerts_jll"] +uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" +version = "0.6.4" + +[[deps.LibCURL_jll]] +deps = ["Artifacts", "LibSSH2_jll", "Libdl", "OpenSSL_jll", "Zlib_jll", "nghttp2_jll"] +uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" +version = "8.15.0+0" + +[[deps.LibGit2]] +deps = ["LibGit2_jll", "NetworkOptions", "Printf", "SHA"] +uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" +version = "1.11.0" + +[[deps.LibGit2_jll]] +deps = ["Artifacts", "LibSSH2_jll", "Libdl", "OpenSSL_jll"] +uuid = "e37daf67-58a4-590a-8e99-b0245dd2ffc5" +version = "1.9.0+0" + +[[deps.LibSSH2_jll]] +deps = ["Artifacts", "Libdl", "OpenSSL_jll"] +uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" +version = "1.11.3+1" + +[[deps.Libdl]] +uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" +version = "1.11.0" + +[[deps.LieGroups]] +deps = ["LinearAlgebra", "Manifolds", "ManifoldsBase", "Quaternions", "Random", "StaticArrays"] +git-tree-sha1 = "7d7b54219f0204d539af303655b24d9a81f61b23" +uuid = "6774de46-80ba-43f8-ba42-e41071ccfc5f" +version = "0.1.9" + + [deps.LieGroups.extensions] + LieGroupsRecursiveArrayToolsExt = "RecursiveArrayTools" + LieGroupsTestExt = "Test" + + [deps.LieGroups.weakdeps] + RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" + Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" + +[[deps.LinearAlgebra]] +deps = ["Libdl", "OpenBLAS_jll", "libblastrampoline_jll"] +uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +version = "1.12.0" + +[[deps.LinearMaps]] +deps = ["LinearAlgebra"] +git-tree-sha1 = "7f6be2e4cdaaf558623d93113d6ddade7b916209" +uuid = "7a12625a-238d-50fd-b39a-03d52299707e" +version = "3.11.4" + + [deps.LinearMaps.extensions] + LinearMapsChainRulesCoreExt = "ChainRulesCore" + LinearMapsSparseArraysExt = "SparseArrays" + LinearMapsStatisticsExt = "Statistics" + + [deps.LinearMaps.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" + Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" + +[[deps.LogExpFunctions]] +deps = ["DocStringExtensions", "IrrationalConstants", "LinearAlgebra"] +git-tree-sha1 = "13ca9e2586b89836fd20cccf56e57e2b9ae7f38f" +uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" +version = "0.3.29" + + [deps.LogExpFunctions.extensions] + LogExpFunctionsChainRulesCoreExt = "ChainRulesCore" + LogExpFunctionsChangesOfVariablesExt = "ChangesOfVariables" + LogExpFunctionsInverseFunctionsExt = "InverseFunctions" + + [deps.LogExpFunctions.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + ChangesOfVariables = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" + InverseFunctions = "3587e190-3f89-42d0-90ee-14403ec27112" + +[[deps.Logging]] +uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" +version = "1.11.0" + +[[deps.LoopVectorization]] +deps = ["ArrayInterface", "CPUSummary", "CloseOpenIntervals", "DocStringExtensions", "HostCPUFeatures", "IfElse", "LayoutPointers", "LinearAlgebra", "OffsetArrays", "PolyesterWeave", "PrecompileTools", "SIMDTypes", "SLEEFPirates", "Static", "StaticArrayInterface", "ThreadingUtilities", "UnPack", "VectorizationBase"] +git-tree-sha1 = "a9fc7883eb9b5f04f46efb9a540833d1fad974b3" +uuid = "bdcacae8-1622-11e9-2a5c-532679323890" +version = "0.12.173" + + [deps.LoopVectorization.extensions] + ForwardDiffExt = ["ChainRulesCore", "ForwardDiff"] + ForwardDiffNNlibExt = ["ForwardDiff", "NNlib"] + SpecialFunctionsExt = "SpecialFunctions" + + [deps.LoopVectorization.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" + NNlib = "872c559c-99b0-510c-b3b7-b6c96a88d5cd" + SpecialFunctions = "276daf66-3868-5448-9aa4-cd146d93841b" + +[[deps.MacroTools]] +git-tree-sha1 = "1e0228a030642014fe5cfe68c2c0a818f9e3f522" +uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" +version = "0.5.16" + +[[deps.ManifoldDiff]] +deps = ["ADTypes", "DifferentiationInterface", "LinearAlgebra", "ManifoldsBase", "Markdown", "Random", "Requires"] +git-tree-sha1 = "28c69b401c75dd3f3b34a39bb7cb7948a2aeb61d" +uuid = "af67fdf4-a580-4b9f-bbec-742ef357defd" +version = "0.4.5" + + [deps.ManifoldDiff.weakdeps] + FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" + FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000" + ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" + ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" + Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" + +[[deps.Manifolds]] +deps = ["ADTypes", "DifferentiationInterface", "Einsum", "Graphs", "Kronecker", "LinearAlgebra", "ManifoldDiff", "ManifoldsBase", "Markdown", "MatrixEquations", "Quaternions", "Random", "SimpleWeightedGraphs", "SpecialFunctions", "StaticArrays", "Statistics", "StatsBase"] +git-tree-sha1 = "83324dcdfa1d6308b587d82e7eed418e8ec698bc" +uuid = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" +version = "0.11.9" + + [deps.Manifolds.extensions] + ManifoldsBoundaryValueDiffEqExt = "BoundaryValueDiffEq" + ManifoldsDistributionsExt = ["Distributions", "RecursiveArrayTools"] + ManifoldsHybridArraysExt = "HybridArrays" + ManifoldsNLsolveExt = "NLsolve" + ManifoldsOrdinaryDiffEqDiffEqCallbacksExt = ["DiffEqCallbacks", "OrdinaryDiffEq", "RecursiveArrayTools"] + ManifoldsOrdinaryDiffEqExt = "OrdinaryDiffEq" + ManifoldsRecipesBaseExt = ["Colors", "RecipesBase"] + ManifoldsRecursiveArrayToolsExt = "RecursiveArrayTools" + ManifoldsTestExt = "Test" + + [deps.Manifolds.weakdeps] + BoundaryValueDiffEq = "764a87c0-6b3e-53db-9096-fe964310641d" + Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" + DiffEqCallbacks = "459566f4-90b8-5000-8ac3-15dfb0a30def" + Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" + HybridArrays = "1baab800-613f-4b0a-84e4-9cd3431bfbb9" + NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" + OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" + RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" + RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" + Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" + +[[deps.ManifoldsBase]] +deps = ["LinearAlgebra", "Markdown", "Printf", "Random"] +git-tree-sha1 = "b44090cd3c5bf1b1a290110470b95cc0e1f67d11" +uuid = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb" +version = "2.3.0" + + [deps.ManifoldsBase.extensions] + ManifoldsBasePlotsExt = "Plots" + ManifoldsBaseQuaternionsExt = "Quaternions" + ManifoldsBaseRecursiveArrayToolsExt = "RecursiveArrayTools" + ManifoldsBaseStatisticsExt = "Statistics" + + [deps.ManifoldsBase.weakdeps] + Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" + Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" + RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" + Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" + +[[deps.ManualMemory]] +git-tree-sha1 = "bcaef4fc7a0cfe2cba636d84cda54b5e4e4ca3cd" +uuid = "d125e4d3-2237-4719-b19c-fa641b8a4667" +version = "0.1.8" + +[[deps.Markdown]] +deps = ["Base64", "JuliaSyntaxHighlighting", "StyledStrings"] +uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" +version = "1.11.0" + +[[deps.MatrixEquations]] +deps = ["LinearAlgebra", "LinearMaps"] +git-tree-sha1 = "51f3fade0b4ff2cf90b36b3312425460631abb56" +uuid = "99c1a7ee-ab34-5fd5-8076-27c950a045f4" +version = "2.5.6" + +[[deps.Missings]] +deps = ["DataAPI"] +git-tree-sha1 = "ec4f7fbeab05d7747bdf98eb74d130a2a2ed298d" +uuid = "e1d29d7a-bbdc-5cf2-9ac0-f12de2c33e28" +version = "1.2.0" + +[[deps.MozillaCACerts_jll]] +uuid = "14a3606d-f60d-562e-9121-12d972cd8159" +version = "2025.5.20" + +[[deps.NamedDims]] +deps = ["LinearAlgebra", "Statistics"] +git-tree-sha1 = "f9e4a49ecd1ea2eccfb749a506fa882c094152b4" +uuid = "356022a1-0364-5f58-8944-0da4b18d706f" +version = "1.2.3" + + [deps.NamedDims.extensions] + AbstractFFTsExt = "AbstractFFTs" + ChainRulesCoreExt = "ChainRulesCore" + CovarianceEstimationExt = "CovarianceEstimation" + TrackerExt = "Tracker" + + [deps.NamedDims.weakdeps] + AbstractFFTs = "621f4979-c628-5d54-868e-fcf4e3e8185c" + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + CovarianceEstimation = "587fd27a-f159-11e8-2dae-1979310e6154" + Requires = "ae029012-a4dd-5104-9daa-d747884805df" + Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" + +[[deps.NetworkOptions]] +uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" +version = "1.3.0" + +[[deps.OffsetArrays]] +git-tree-sha1 = "117432e406b5c023f665fa73dc26e79ec3630151" +uuid = "6fe1bfb0-de20-5000-8ca7-80f57d26f881" +version = "1.17.0" +weakdeps = ["Adapt"] + + [deps.OffsetArrays.extensions] + OffsetArraysAdaptExt = "Adapt" + +[[deps.OpenBLAS_jll]] +deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"] +uuid = "4536629a-c528-5b80-bd46-f80d51c5b363" +version = "0.3.29+0" + +[[deps.OpenLibm_jll]] +deps = ["Artifacts", "Libdl"] +uuid = "05823500-19ac-5b8b-9628-191a04bc5112" +version = "0.8.7+0" + +[[deps.OpenSSL_jll]] +deps = ["Artifacts", "Libdl"] +uuid = "458c3c95-2e84-50aa-8efc-19380b2a3a95" +version = "3.5.4+0" + +[[deps.OpenSpecFun_jll]] +deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl"] +git-tree-sha1 = "1346c9208249809840c91b26703912dff463d335" +uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" +version = "0.5.6+0" + +[[deps.OrderedCollections]] +git-tree-sha1 = "05868e21324cede2207c6f0f466b4bfef6d5e7ee" +uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" +version = "1.8.1" + +[[deps.Pkg]] +deps = ["Artifacts", "Dates", "Downloads", "FileWatching", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "Random", "SHA", "TOML", "Tar", "UUIDs", "p7zip_jll"] +uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" +version = "1.12.0" + + [deps.Pkg.extensions] + REPLExt = "REPL" + + [deps.Pkg.weakdeps] + REPL = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" + +[[deps.PolyesterWeave]] +deps = ["BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "Static", "ThreadingUtilities"] +git-tree-sha1 = "645bed98cd47f72f67316fd42fc47dee771aefcd" +uuid = "1d0040c9-8b98-4ee7-8388-3f51789ca0ad" +version = "0.2.2" + +[[deps.PrecompileTools]] +deps = ["Preferences"] +git-tree-sha1 = "07a921781cab75691315adc645096ed5e370cb77" +uuid = "aea7be01-6a6a-4083-8856-8a6e6704d82a" +version = "1.3.3" + +[[deps.Preferences]] +deps = ["TOML"] +git-tree-sha1 = "522f093a29b31a93e34eaea17ba055d850edea28" +uuid = "21216c6a-2e73-6563-6e65-726566657250" +version = "1.5.1" + +[[deps.Printf]] +deps = ["Unicode"] +uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" +version = "1.11.0" + +[[deps.PtrArrays]] +git-tree-sha1 = "1d36ef11a9aaf1e8b74dacc6a731dd1de8fd493d" +uuid = "43287f4e-b6f4-7ad1-bb20-aadabca52c3d" +version = "1.3.0" + +[[deps.Quaternions]] +deps = ["LinearAlgebra", "Random", "RealDot"] +git-tree-sha1 = "4d8c1b7c3329c1885b857abb50d08fa3f4d9e3c8" +uuid = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" +version = "0.7.7" + +[[deps.Random]] +deps = ["SHA"] +uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" +version = "1.11.0" + +[[deps.RealDot]] +deps = ["LinearAlgebra"] +git-tree-sha1 = "9f0a1b71baaf7650f4fa8a1d168c7fb6ee41f0c9" +uuid = "c1ae055f-0cd5-4b69-90a6-9a35b1a98df9" +version = "0.1.0" + +[[deps.RecipesBase]] +deps = ["PrecompileTools"] +git-tree-sha1 = "5c3d09cc4f31f5fc6af001c250bf1278733100ff" +uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" +version = "1.3.4" + +[[deps.RecursiveArrayTools]] +deps = ["Adapt", "ArrayInterface", "DocStringExtensions", "GPUArraysCore", "LinearAlgebra", "RecipesBase", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface"] +git-tree-sha1 = "d7c53a81b0ab7f62842ff79d9e74e34562d5834e" +uuid = "731186ca-8d62-57ce-b412-fbd966d074cd" +version = "3.42.0" + + [deps.RecursiveArrayTools.extensions] + RecursiveArrayToolsFastBroadcastExt = "FastBroadcast" + RecursiveArrayToolsForwardDiffExt = "ForwardDiff" + RecursiveArrayToolsKernelAbstractionsExt = "KernelAbstractions" + RecursiveArrayToolsMeasurementsExt = "Measurements" + RecursiveArrayToolsMonteCarloMeasurementsExt = "MonteCarloMeasurements" + RecursiveArrayToolsReverseDiffExt = ["ReverseDiff", "Zygote"] + RecursiveArrayToolsSparseArraysExt = ["SparseArrays"] + RecursiveArrayToolsStructArraysExt = "StructArrays" + RecursiveArrayToolsTablesExt = ["Tables"] + RecursiveArrayToolsTrackerExt = "Tracker" + RecursiveArrayToolsZygoteExt = "Zygote" + + [deps.RecursiveArrayTools.weakdeps] + FastBroadcast = "7034ab61-46d4-4ed7-9d0f-46aef9175898" + ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" + KernelAbstractions = "63c18a36-062a-441e-b654-da1e3ab1ce7c" + Measurements = "eff96d63-e80a-5855-80a2-b1b0885c5ab7" + MonteCarloMeasurements = "0987c9cc-fe09-11e8-30f0-b96dd679fdca" + ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" + SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" + StructArrays = "09ab397b-f2b6-538f-b94a-2f83cf4a842a" + Tables = "bd369af6-aec1-5ad0-b16a-f7cc5008161c" + Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" + Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" + +[[deps.Requires]] +deps = ["UUIDs"] +git-tree-sha1 = "62389eeff14780bfe55195b7204c0d8738436d64" +uuid = "ae029012-a4dd-5104-9daa-d747884805df" +version = "1.3.1" + +[[deps.Rotations]] +deps = ["LinearAlgebra", "Quaternions", "Random", "StaticArrays"] +git-tree-sha1 = "5680a9276685d392c87407df00d57c9924d9f11e" +uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" +version = "1.7.1" +weakdeps = ["RecipesBase"] + + [deps.Rotations.extensions] + RotationsRecipesBaseExt = "RecipesBase" + +[[deps.RuntimeGeneratedFunctions]] +deps = ["ExprTools", "SHA", "Serialization"] +git-tree-sha1 = "2f609ec2295c452685d3142bc4df202686e555d2" +uuid = "7e49a35a-f44a-4d26-94aa-eba1b4ca6b47" +version = "0.5.16" + +[[deps.SHA]] +uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" +version = "0.7.0" + +[[deps.SIMDTypes]] +git-tree-sha1 = "330289636fb8107c5f32088d2741e9fd7a061a5c" +uuid = "94e857df-77ce-4151-89e5-788b33177be4" +version = "0.1.0" + +[[deps.SLEEFPirates]] +deps = ["IfElse", "Static", "VectorizationBase"] +git-tree-sha1 = "456f610ca2fbd1c14f5fcf31c6bfadc55e7d66e0" +uuid = "476501e8-09a2-5ece-8869-fb82de89a1fa" +version = "0.6.43" + +[[deps.SciMLPublic]] +git-tree-sha1 = "ed647f161e8b3f2973f24979ec074e8d084f1bee" +uuid = "431bcebd-1456-4ced-9d72-93c2757fff0b" +version = "1.0.0" + +[[deps.Serialization]] +uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" +version = "1.11.0" + +[[deps.SimpleTraits]] +deps = ["InteractiveUtils", "MacroTools"] +git-tree-sha1 = "be8eeac05ec97d379347584fa9fe2f5f76795bcb" +uuid = "699a6c99-e7fa-54fc-8d76-47d257e15c1d" +version = "0.9.5" + +[[deps.SimpleWeightedGraphs]] +deps = ["Graphs", "LinearAlgebra", "Markdown", "SparseArrays"] +git-tree-sha1 = "749a2b719ec7f34f280c0d97ac3dab5c89818631" +uuid = "47aef6b3-ad0c-573a-a1e2-d07658019622" +version = "1.5.1" + +[[deps.SortingAlgorithms]] +deps = ["DataStructures"] +git-tree-sha1 = "64d974c2e6fdf07f8155b5b2ca2ffa9069b608d9" +uuid = "a2af1166-a08f-5f64-846c-94a0d3cef48c" +version = "1.2.2" + +[[deps.SparseArrays]] +deps = ["Libdl", "LinearAlgebra", "Random", "Serialization", "SuiteSparse_jll"] +uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" +version = "1.12.0" + +[[deps.SpecialFunctions]] +deps = ["IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] +git-tree-sha1 = "f2685b435df2613e25fc10ad8c26dddb8640f547" +uuid = "276daf66-3868-5448-9aa4-cd146d93841b" +version = "2.6.1" + + [deps.SpecialFunctions.extensions] + SpecialFunctionsChainRulesCoreExt = "ChainRulesCore" + + [deps.SpecialFunctions.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + +[[deps.Static]] +deps = ["CommonWorldInvalidations", "IfElse", "PrecompileTools", "SciMLPublic"] +git-tree-sha1 = "49440414711eddc7227724ae6e570c7d5559a086" +uuid = "aedffcd0-7271-4cad-89d0-dc628f76c6d3" +version = "1.3.1" + +[[deps.StaticArrayInterface]] +deps = ["ArrayInterface", "Compat", "IfElse", "LinearAlgebra", "PrecompileTools", "Static"] +git-tree-sha1 = "96381d50f1ce85f2663584c8e886a6ca97e60554" +uuid = "0d7ed370-da01-4f52-bd93-41d350b8b718" +version = "1.8.0" +weakdeps = ["OffsetArrays", "StaticArrays"] + + [deps.StaticArrayInterface.extensions] + StaticArrayInterfaceOffsetArraysExt = "OffsetArrays" + StaticArrayInterfaceStaticArraysExt = "StaticArrays" + +[[deps.StaticArrays]] +deps = ["LinearAlgebra", "PrecompileTools", "Random", "StaticArraysCore"] +git-tree-sha1 = "b8693004b385c842357406e3af647701fe783f98" +uuid = "90137ffa-7385-5640-81b9-e52037218182" +version = "1.9.15" + + [deps.StaticArrays.extensions] + StaticArraysChainRulesCoreExt = "ChainRulesCore" + StaticArraysStatisticsExt = "Statistics" + + [deps.StaticArrays.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" + +[[deps.StaticArraysCore]] +git-tree-sha1 = "6ab403037779dae8c514bad259f32a447262455a" +uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" +version = "1.4.4" + +[[deps.Statistics]] +deps = ["LinearAlgebra"] +git-tree-sha1 = "ae3bb1eb3bba077cd276bc5cfc337cc65c3075c0" +uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" +version = "1.11.1" +weakdeps = ["SparseArrays"] + + [deps.Statistics.extensions] + SparseArraysExt = ["SparseArrays"] + +[[deps.StatsAPI]] +deps = ["LinearAlgebra"] +git-tree-sha1 = "178ed29fd5b2a2cfc3bd31c13375ae925623ff36" +uuid = "82ae8749-77ed-4fe6-ae5f-f523153014b0" +version = "1.8.0" + +[[deps.StatsBase]] +deps = ["AliasTables", "DataAPI", "DataStructures", "LinearAlgebra", "LogExpFunctions", "Missings", "Printf", "Random", "SortingAlgorithms", "SparseArrays", "Statistics", "StatsAPI"] +git-tree-sha1 = "be5733d4a2b03341bdcab91cea6caa7e31ced14b" +uuid = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91" +version = "0.34.9" + +[[deps.StyledStrings]] +uuid = "f489334b-da3d-4c2e-b8f0-e476e12c162b" +version = "1.11.0" + +[[deps.SuiteSparse_jll]] +deps = ["Artifacts", "Libdl", "libblastrampoline_jll"] +uuid = "bea87d4a-7f5b-5778-9afe-8cc45184846c" +version = "7.8.3+2" + +[[deps.SymbolicIndexingInterface]] +deps = ["Accessors", "ArrayInterface", "RuntimeGeneratedFunctions", "StaticArraysCore"] +git-tree-sha1 = "94c58884e013efff548002e8dc2fdd1cb74dfce5" +uuid = "2efcf032-c050-4f8e-a9bb-153293bab1f5" +version = "0.3.46" + + [deps.SymbolicIndexingInterface.extensions] + SymbolicIndexingInterfacePrettyTablesExt = "PrettyTables" + + [deps.SymbolicIndexingInterface.weakdeps] + PrettyTables = "08abe8d2-0d0c-5749-adfa-8a2ac140af0d" + +[[deps.TOML]] +deps = ["Dates"] +uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" +version = "1.0.3" + +[[deps.Tar]] +deps = ["ArgTools", "SHA"] +uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" +version = "1.10.0" + +[[deps.ThreadingUtilities]] +deps = ["ManualMemory"] +git-tree-sha1 = "d969183d3d244b6c33796b5ed01ab97328f2db85" +uuid = "8290d209-cae3-49c0-8002-c8c24d57dab5" +version = "0.5.5" + +[[deps.UUIDs]] +deps = ["Random", "SHA"] +uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" +version = "1.11.0" + +[[deps.UnPack]] +git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b" +uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" +version = "1.0.2" + +[[deps.Unicode]] +uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" +version = "1.11.0" + +[[deps.VectorizationBase]] +deps = ["ArrayInterface", "CPUSummary", "HostCPUFeatures", "IfElse", "LayoutPointers", "Libdl", "LinearAlgebra", "SIMDTypes", "Static", "StaticArrayInterface"] +git-tree-sha1 = "d1d9a935a26c475ebffd54e9c7ad11627c43ea85" +uuid = "3d5dd08c-fd9d-11e8-17fa-ed2836048c2f" +version = "0.21.72" + +[[deps.Zlib_jll]] +deps = ["Libdl"] +uuid = "83775a58-1f1d-513f-b197-d71354ab007a" +version = "1.3.1+2" + +[[deps.libblastrampoline_jll]] +deps = ["Artifacts", "Libdl"] +uuid = "8e850b90-86db-534c-a0d3-1478176c7d93" +version = "5.15.0+0" + +[[deps.nghttp2_jll]] +deps = ["Artifacts", "Libdl"] +uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" +version = "1.64.0+1" + +[[deps.p7zip_jll]] +deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"] +uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" +version = "17.7.0+0" diff --git a/Project.toml b/Project.toml index e619164..2587418 100644 --- a/Project.toml +++ b/Project.toml @@ -5,6 +5,7 @@ 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" @@ -14,8 +15,9 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" [compat] DocStringExtensions = "0.8, 0.9" -LoopVectorization = "0.12.173" +GeometryBasics = "0.5.10" LieGroups = "0.1" +LoopVectorization = "0.12.173" RecursiveArrayTools = "3.27.0" Rotations = "1" StaticArrays = "1" diff --git a/src/CameraModels.jl b/src/CameraModels.jl index 0f07a27..21557ca 100644 --- a/src/CameraModels.jl +++ b/src/CameraModels.jl @@ -1,28 +1,25 @@ 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 using LoopVectorization: @tturbo -# exports -include("ExportAPI.jl") +import Rotations as Rot_ +import Base: getindex, getproperty, show + -# data types +include("ExportAPI.jl") include("entities/GeneralTypes.jl") include("entities/CameraCalibration.jl") - include("services/CameraCalibration.jl") - -# legacy implementations -include("Deprecated.jl") - -# function logic -include("services/Prototypes.jl") include("services/CameraServices.jl") include("services/Utils.jl") diff --git a/src/Deprecated.jl b/src/Deprecated.jl deleted file mode 100644 index 2d0e49e..0000000 --- a/src/Deprecated.jl +++ /dev/null @@ -1,295 +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.0], t::AbstractVector = [0, 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.") - return 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 - return 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.0]']) - return 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." - return 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, - ) - return 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 - - -# diff --git a/src/ExportAPI.jl b/src/ExportAPI.jl index bac4692..fb227d2 100644 --- a/src/ExportAPI.jl +++ b/src/ExportAPI.jl @@ -7,7 +7,7 @@ export CameraSkewDistortion export undistortPoint export Ray, PixelIndex -export pixel2ray, point2pixel, canreproject, sensorsize #, origin, direction, +export canreproject, sensorsize #, origin, direction, export project, projectHomogeneous export backproject, backprojectHomogeneous export pp_w, pp_h, f_w, f_h @@ -18,4 +18,4 @@ 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 +export Vector3, Vector2, Point3 diff --git a/src/entities/CameraCalibration.jl b/src/entities/CameraCalibration.jl index daf4d56..72d4bb0 100644 --- a/src/entities/CameraCalibration.jl +++ b/src/entities/CameraCalibration.jl @@ -63,17 +63,3 @@ Base.@kwdef mutable struct CameraCalibrationMutable{R <: Real, N} <: AbstractCam """ inverse of a 3x3 camera calibration matrix """ Ki::MMatrix{3, 3, R} = inv(K) end - - -## =========================================================================== -## Legacy types that are not so easy to consolidate (not exported) DO NOT USE -## =========================================================================== - - -Base.@kwdef struct CameraModelFull - ci::CameraCalibration = CameraCalibration() - ce::ArrayPartition = ArrayPartition(SVector(0.0, 0.0, 0.0), SMatrix{3, 3}(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)) # CameraExtrinsic() -end - - -# diff --git a/src/entities/GeneralTypes.jl b/src/entities/GeneralTypes.jl index 55a652b..542f324 100644 --- a/src/entities/GeneralTypes.jl +++ b/src/entities/GeneralTypes.jl @@ -12,14 +12,14 @@ Base.getindex(p::PixelIndex, i::Int) = throw(DomainError(i, "Camera only has rows, columns and depth")) -const Vector2 = SVector{2, Float64} -const Point3 = SVector{3, Float64} -const Vector3 = SVector{3, Float64} +const Vector2 = Vec{2, Float64} +const Point3 = Point{3, Float64} +const Vector3 = Vec{3, Float64} # Abstract type abstract type AbstractCameraModel end -origin3d = zeros(Point3) +origin3d = Point3(0, 0, 0) struct Ray origin::Point3 diff --git a/src/services/CameraServices.jl b/src/services/CameraServices.jl index 7d014c2..357663c 100644 --- a/src/services/CameraServices.jl +++ b/src/services/CameraServices.jl @@ -1,41 +1,136 @@ -## consolidated functions, first baseline - -## ========================================================================================= -## Parameter functions +""" + origin(ray) -## From yakir12/CameraModels.jl +Return the origin of the ray as a `Vector3`. +""" 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) + +""" + lookdirection(camera::AbstractCameraModel) + +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(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 +""" + 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_shear!(pc::CameraCalibrationMutable, val::Real) + +Set the shear parameter of the camera. +""" set_shear!(pc::CameraCalibrationMutable, val::Real) = (pc.K[1, 2] = val) + +""" + 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::CameraModel) + canreproject(camera::AbstractCameraModel) -Confirms if point2pixel is implemented for this camera model. +Confirms if project is implemented for this camera model. """ canreproject(camera::AbstractCameraModel) = true -## computational functions +## Computational functions ## ========================================================================================= @@ -60,7 +155,7 @@ function undistortPoint(cam::CameraCalibration, xy, iter_num = 3) x = (x0 - delta_x) * k_inv y = (y0 - delta_y) * k_inv end - return SA[x * fx + cx; y * fy + cy] + return Point(x * fx + cx, y * fy + cy) end @@ -68,11 +163,9 @@ end ## FROM SCENE IN FRONT OF CAMERA TO IMAGE -- I.E. PROJECT ## ========================================================================================= - -## From JuliaRobotics/SensorFeatureTracking.jl function project!( ret::AbstractVector{<:Real}, - ci::CameraCalibration, #CameraIntrinsic, + ci::CameraCalibration, c_T_r::ArrayPartition, r_P::AbstractVector{<:Real} ) @@ -86,57 +179,47 @@ end 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.0, 0.0), SMatrix{3, 3}(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)) + c_T_r = ArrayPartition(Vector3(0.0, 0.0, 0.0), Mat{3,3}(1.0*I(3))) ) - # ret = MVector(0.0, 0.0) return 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) 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 + cam::AbstractCameraModel, + c_Ph::AbstractVector{<:Real} +) + x, y, z, w = c_Ph + + # Project to image plane + inv_z = 1 / z + col = x * f_w(cam) * inv_z + pp_w(cam) + row = y * f_h(cam) * inv_z + pp_h(cam) + + # Depth and validity (point must be in front of camera) depth = z / w - return PixelIndex(row, col; depth, valid = (w == 0&&0 < z) || 0 < depth) + valid = (w == 0 && z > 0) || depth > 0 + + return PixelIndex(col, row; depth, valid) end -# # right cam -# u2 = (x - w*baseline) * fz + center[1] ## ========================================================================================= @@ -149,12 +232,8 @@ end 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) """ @@ -167,6 +246,8 @@ function backproject( 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 @@ -187,7 +268,7 @@ end function cameraResidual!( res::AbstractVector{<:Real}, z::AbstractVector{<:Real}, - ci::CameraCalibration, #CameraIntrinsic, + ci::CameraCalibration, ce::ArrayPartition, pt::Union{PixelIndex, <:AbstractVector{<:Real}}, ) @@ -198,5 +279,3 @@ function cameraResidual!( return nothing end - -## diff --git a/src/services/Prototypes.jl b/src/services/Prototypes.jl deleted file mode 100644 index 63922cd..0000000 --- a/src/services/Prototypes.jl +++ /dev/null @@ -1,66 +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 a38561c..eeaa6b0 100644 --- a/src/services/Utils.jl +++ b/src/services/Utils.jl @@ -1,22 +1,26 @@ -toNonhomogeneous(_Ph::AbstractVector) = SVector((_Ph[1:(end - 1)]...,) ./ _Ph[end]) +""" + $SIGNATURES +Converts a homogeneous point to a non-homogeneous point, i.e. divides by the last element. +""" +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 """ - CameraModel(width,height,fc,cc,skew,kc) + $SIGNATURES Constructor helper for creating a camera model. """ 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) - return CameraModelandParameters(width, height, fc, cc, skew, kc, KK, Ki) + K = Mat{3, 3}( + fc[1], 0.0, 0.0, + skew, fc[2], 0.0, + cc[1], cc[2], 1.0) + return CameraCalibration(;width, height, K, kc=SVector(float.(kc)...)) end """ diff --git a/test/CameraTestBench.jl b/test/CameraTestBench.jl index cb7d9dd..1b6930a 100644 --- a/test/CameraTestBench.jl +++ b/test/CameraTestBench.jl @@ -1,7 +1,7 @@ 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 = 1e-5, ray_accuracy::Float64 = 1e-4) where C <: AbstractCameraModel @testset "Check basics and interface implementation for $(C)." begin @@ -23,20 +23,23 @@ 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)." + @info "project not implemented for $(C)." end 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 index ff9134a..bf4a28b 100644 --- a/test/SkewDistortion.jl +++ b/test/SkewDistortion.jl @@ -1,7 +1,4 @@ - - - -## Tests from SensorFeatureTracking.jl +# Tests from SensorFeatureTracking.jl @testset "Test CameraSkewDistortion" begin @@ -18,5 +15,3 @@ 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..0b4b202 100644 --- a/test/multiview_manifolds.jl +++ b/test/multiview_manifolds.jl @@ -1,48 +1,21 @@ - -# - -# 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) +obs1 = CameraModels.PixelIndex(320, 240) +obs2 = CameraModels.PixelIndex(315, 240) 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} @@ -55,11 +28,15 @@ function cameraResidual(cam, meas, M, w_T_c, w_Ph, κ=1000) κ*(abs(pred.depth) - pred.depth)^2 + (meas[1]-pred[1])^2 + (meas[2]-pred[2])^2 end -function cost(w_Ph) +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_Ph = SVector(10.,0.,0.,1.) @@ -68,32 +45,16 @@ cost(w_Ph) cost(SVector(0.1,0.,0.,1.)) cost(SVector(0.5,0.,0.,1.)) -## w_Res = Optim.optimize( cost, - [1;0;0;1.], + [1.0;0.0;0.0], 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 ) @show w_Res.minimizer - -## - -@show w_P3 = w_Res.minimizer |> CameraModels.toNonhomogeneous +@show w_P3 = w_Res.minimizer @test isapprox([10.56;0;0], w_P3; atol=1e-3) -## end - - -## \ No newline at end of file diff --git a/test/testutils.jl b/test/testutils.jl index edfcb07..390688c 100644 --- a/test/testutils.jl +++ b/test/testutils.jl @@ -81,8 +81,8 @@ end src_mat = rand(Float32, 720, 1280) - dst_mat = similar(src_mat) + dst_mat = deepcopy(src_mat) CameraModels.radialDistortion!(test_camera, dst_mat, src_mat) - + @test dst_mat != src_mat end From 3f63e84711f91979387c15962079d684386ca5b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thimot=C3=A9=20Dupuch?= Date: Sun, 21 Dec 2025 17:49:29 +0100 Subject: [PATCH 7/7] more refactoring + new diagrams --- .gitignore | 1 + Manifest-v1.12.toml | 947 ------------------------------ README.md | 9 +- docs/imgs/cameramodels.mathcha | Bin 0 -> 28426 bytes docs/imgs/diagram1.svg | 11 + docs/imgs/diagram2.svg | 11 + docs/imgs/diagram3.svg | 11 + src/CameraModels.jl | 15 +- src/ExportAPI.jl | 54 +- src/entities/CameraCalibration.jl | 43 +- src/entities/GeneralTypes.jl | 14 +- src/services/CameraCalibration.jl | 34 -- src/services/CameraServices.jl | 80 ++- src/services/Display.jl | 44 ++ src/services/Helper.jl | 41 ++ src/services/Utils.jl | 39 +- test/CameraTestBench.jl | 28 +- test/SkewDistortion.jl | 17 - test/multiview_manifolds.jl | 69 +-- test/runtests.jl | 2 - test/testutils.jl | 57 +- 21 files changed, 373 insertions(+), 1154 deletions(-) delete mode 100644 Manifest-v1.12.toml create mode 100644 docs/imgs/cameramodels.mathcha create mode 100644 docs/imgs/diagram1.svg create mode 100644 docs/imgs/diagram2.svg create mode 100644 docs/imgs/diagram3.svg delete mode 100644 src/services/CameraCalibration.jl create mode 100644 src/services/Display.jl create mode 100644 src/services/Helper.jl delete mode 100644 test/SkewDistortion.jl 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/Manifest-v1.12.toml b/Manifest-v1.12.toml deleted file mode 100644 index cb33900..0000000 --- a/Manifest-v1.12.toml +++ /dev/null @@ -1,947 +0,0 @@ -# This file is machine-generated - editing it directly is not advised - -julia_version = "1.12.2" -manifest_format = "2.0" -project_hash = "d9960187b460e54c2195a0dd6dd4b32f8e7428ca" - -[[deps.ADTypes]] -git-tree-sha1 = "8b2b045b22740e4be20654175cc38291d48539db" -uuid = "47edcb42-4c32-4615-8424-f2b9edc5f35b" -version = "1.20.0" - - [deps.ADTypes.extensions] - ADTypesChainRulesCoreExt = "ChainRulesCore" - ADTypesConstructionBaseExt = "ConstructionBase" - ADTypesEnzymeCoreExt = "EnzymeCore" - - [deps.ADTypes.weakdeps] - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - ConstructionBase = "187b0558-2788-49d3-abe0-74a17ed4e7c9" - EnzymeCore = "f151be2c-9106-41f4-ab19-57ee4f262869" - -[[deps.Accessors]] -deps = ["CompositionsBase", "ConstructionBase", "Dates", "InverseFunctions", "MacroTools"] -git-tree-sha1 = "856ecd7cebb68e5fc87abecd2326ad59f0f911f3" -uuid = "7d9f7c33-5ae7-4f3b-8dc6-eff91059b697" -version = "0.1.43" - - [deps.Accessors.extensions] - AxisKeysExt = "AxisKeys" - IntervalSetsExt = "IntervalSets" - LinearAlgebraExt = "LinearAlgebra" - StaticArraysExt = "StaticArrays" - StructArraysExt = "StructArrays" - TestExt = "Test" - UnitfulExt = "Unitful" - - [deps.Accessors.weakdeps] - AxisKeys = "94b1ba4f-4ee9-5380-92f1-94cde586c3c5" - IntervalSets = "8197267c-284f-5f27-9208-e0e47529a953" - LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" - StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" - StructArrays = "09ab397b-f2b6-538f-b94a-2f83cf4a842a" - Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" - Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d" - -[[deps.Adapt]] -deps = ["LinearAlgebra", "Requires"] -git-tree-sha1 = "7e35fca2bdfba44d797c53dfe63a51fabf39bfc0" -uuid = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" -version = "4.4.0" -weakdeps = ["SparseArrays", "StaticArrays"] - - [deps.Adapt.extensions] - AdaptSparseArraysExt = "SparseArrays" - AdaptStaticArraysExt = "StaticArrays" - -[[deps.AliasTables]] -deps = ["PtrArrays", "Random"] -git-tree-sha1 = "9876e1e164b144ca45e9e3198d0b689cadfed9ff" -uuid = "66dad0bd-aa9a-41b7-9441-69ab47430ed8" -version = "1.1.3" - -[[deps.ArgTools]] -uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" -version = "1.1.2" - -[[deps.ArnoldiMethod]] -deps = ["LinearAlgebra", "Random", "StaticArrays"] -git-tree-sha1 = "d57bd3762d308bded22c3b82d033bff85f6195c6" -uuid = "ec485272-7323-5ecc-a04f-4719b315124d" -version = "0.4.0" - -[[deps.ArrayInterface]] -deps = ["Adapt", "LinearAlgebra"] -git-tree-sha1 = "d81ae5489e13bc03567d4fbbb06c546a5e53c857" -uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9" -version = "7.22.0" - - [deps.ArrayInterface.extensions] - ArrayInterfaceBandedMatricesExt = "BandedMatrices" - ArrayInterfaceBlockBandedMatricesExt = "BlockBandedMatrices" - ArrayInterfaceCUDAExt = "CUDA" - ArrayInterfaceCUDSSExt = ["CUDSS", "CUDA"] - ArrayInterfaceChainRulesCoreExt = "ChainRulesCore" - ArrayInterfaceChainRulesExt = "ChainRules" - ArrayInterfaceGPUArraysCoreExt = "GPUArraysCore" - ArrayInterfaceMetalExt = "Metal" - ArrayInterfaceReverseDiffExt = "ReverseDiff" - ArrayInterfaceSparseArraysExt = "SparseArrays" - ArrayInterfaceStaticArraysCoreExt = "StaticArraysCore" - ArrayInterfaceTrackerExt = "Tracker" - - [deps.ArrayInterface.weakdeps] - BandedMatrices = "aae01518-5342-5314-be14-df237901396f" - BlockBandedMatrices = "ffab5731-97b5-5995-9138-79e8c1846df0" - CUDA = "052768ef-5323-5732-b1bb-66c8b64840ba" - CUDSS = "45b445bb-4962-46a0-9369-b4df9d0f772e" - ChainRules = "082447d4-558c-5d27-93f4-14fc19e9eca2" - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - GPUArraysCore = "46192b85-c4d5-4398-a991-12ede77f4527" - Metal = "dde4c033-4e86-420c-a63e-0dd931031962" - ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" - SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" - StaticArraysCore = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" - Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" - -[[deps.Artifacts]] -uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" -version = "1.11.0" - -[[deps.Base64]] -uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" -version = "1.11.0" - -[[deps.BitTwiddlingConvenienceFunctions]] -deps = ["Static"] -git-tree-sha1 = "f21cfd4950cb9f0587d5067e69405ad2acd27b87" -uuid = "62783981-4cbd-42fc-bca8-16325de8dc4b" -version = "0.1.6" - -[[deps.CPUSummary]] -deps = ["CpuId", "IfElse", "PrecompileTools", "Preferences", "Static"] -git-tree-sha1 = "f3a21d7fc84ba618a779d1ed2fcca2e682865bab" -uuid = "2a0fbf3d-bb9c-48f3-b0a9-814d99fd7ab9" -version = "0.2.7" - -[[deps.CameraModels]] -deps = ["DocStringExtensions", "LieGroups", "LinearAlgebra", "LoopVectorization", "RecursiveArrayTools", "Rotations", "StaticArrays"] -path = "." -uuid = "0d57b887-6120-40e6-8b8c-29d3116bc0c1" -version = "0.2.4" - -[[deps.CloseOpenIntervals]] -deps = ["Static", "StaticArrayInterface"] -git-tree-sha1 = "05ba0d07cd4fd8b7a39541e31a7b0254704ea581" -uuid = "fb6a15b2-703c-40df-9091-08a04967cfa9" -version = "0.1.13" - -[[deps.CommonWorldInvalidations]] -git-tree-sha1 = "ae52d1c52048455e85a387fbee9be553ec2b68d0" -uuid = "f70d9fcc-98c5-4d4a-abd7-e4cdeebd8ca8" -version = "1.0.0" - -[[deps.Compat]] -deps = ["TOML", "UUIDs"] -git-tree-sha1 = "9d8a54ce4b17aa5bdce0ea5c34bc5e7c340d16ad" -uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" -version = "4.18.1" -weakdeps = ["Dates", "LinearAlgebra"] - - [deps.Compat.extensions] - CompatLinearAlgebraExt = "LinearAlgebra" - -[[deps.CompilerSupportLibraries_jll]] -deps = ["Artifacts", "Libdl"] -uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" -version = "1.3.0+1" - -[[deps.CompositionsBase]] -git-tree-sha1 = "802bb88cd69dfd1509f6670416bd4434015693ad" -uuid = "a33af91c-f02d-484b-be07-31d278c5ca2b" -version = "0.1.2" -weakdeps = ["InverseFunctions"] - - [deps.CompositionsBase.extensions] - CompositionsBaseInverseFunctionsExt = "InverseFunctions" - -[[deps.ConstructionBase]] -git-tree-sha1 = "b4b092499347b18a015186eae3042f72267106cb" -uuid = "187b0558-2788-49d3-abe0-74a17ed4e7c9" -version = "1.6.0" - - [deps.ConstructionBase.extensions] - ConstructionBaseIntervalSetsExt = "IntervalSets" - ConstructionBaseLinearAlgebraExt = "LinearAlgebra" - ConstructionBaseStaticArraysExt = "StaticArrays" - - [deps.ConstructionBase.weakdeps] - IntervalSets = "8197267c-284f-5f27-9208-e0e47529a953" - LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" - StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" - -[[deps.CpuId]] -deps = ["Markdown"] -git-tree-sha1 = "fcbb72b032692610bfbdb15018ac16a36cf2e406" -uuid = "adafc99b-e345-5852-983c-f28acb93d879" -version = "0.3.1" - -[[deps.DataAPI]] -git-tree-sha1 = "abe83f3a2f1b857aac70ef8b269080af17764bbe" -uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" -version = "1.16.0" - -[[deps.DataStructures]] -deps = ["OrderedCollections"] -git-tree-sha1 = "e357641bb3e0638d353c4b29ea0e40ea644066a6" -uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" -version = "0.19.3" - -[[deps.Dates]] -deps = ["Printf"] -uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" -version = "1.11.0" - -[[deps.DifferentiationInterface]] -deps = ["ADTypes", "LinearAlgebra"] -git-tree-sha1 = "1d5a93ce22dfa78d202b3bd6ad8afa3d69fcd129" -uuid = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63" -version = "0.7.13" - - [deps.DifferentiationInterface.extensions] - DifferentiationInterfaceChainRulesCoreExt = "ChainRulesCore" - DifferentiationInterfaceDiffractorExt = "Diffractor" - DifferentiationInterfaceEnzymeExt = ["EnzymeCore", "Enzyme"] - DifferentiationInterfaceFastDifferentiationExt = "FastDifferentiation" - DifferentiationInterfaceFiniteDiffExt = "FiniteDiff" - DifferentiationInterfaceFiniteDifferencesExt = "FiniteDifferences" - DifferentiationInterfaceForwardDiffExt = ["ForwardDiff", "DiffResults"] - DifferentiationInterfaceGPUArraysCoreExt = "GPUArraysCore" - DifferentiationInterfaceGTPSAExt = "GTPSA" - DifferentiationInterfaceMooncakeExt = "Mooncake" - DifferentiationInterfacePolyesterForwardDiffExt = ["PolyesterForwardDiff", "ForwardDiff", "DiffResults"] - DifferentiationInterfaceReverseDiffExt = ["ReverseDiff", "DiffResults"] - DifferentiationInterfaceSparseArraysExt = "SparseArrays" - DifferentiationInterfaceSparseConnectivityTracerExt = "SparseConnectivityTracer" - DifferentiationInterfaceSparseMatrixColoringsExt = "SparseMatrixColorings" - DifferentiationInterfaceStaticArraysExt = "StaticArrays" - DifferentiationInterfaceSymbolicsExt = "Symbolics" - DifferentiationInterfaceTrackerExt = "Tracker" - DifferentiationInterfaceZygoteExt = ["Zygote", "ForwardDiff"] - - [deps.DifferentiationInterface.weakdeps] - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - DiffResults = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" - Diffractor = "9f5e2b26-1114-432f-b630-d3fe2085c51c" - Enzyme = "7da242da-08ed-463a-9acd-ee780be4f1d9" - EnzymeCore = "f151be2c-9106-41f4-ab19-57ee4f262869" - FastDifferentiation = "eb9bf01b-bf85-4b60-bf87-ee5de06c00be" - FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" - FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000" - ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" - GPUArraysCore = "46192b85-c4d5-4398-a991-12ede77f4527" - GTPSA = "b27dd330-f138-47c5-815b-40db9dd9b6e8" - Mooncake = "da2b9cff-9c12-43a0-ae48-6db2b0edb7d6" - PolyesterForwardDiff = "98d1487c-24ca-40b6-b7ab-df2af84e126b" - ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" - SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" - SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5" - SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" - StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" - Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7" - Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" - Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" - -[[deps.DocStringExtensions]] -git-tree-sha1 = "7442a5dfe1ebb773c29cc2962a8980f47221d76c" -uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" -version = "0.9.5" - -[[deps.Downloads]] -deps = ["ArgTools", "FileWatching", "LibCURL", "NetworkOptions"] -uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" -version = "1.7.0" - -[[deps.EarCut_jll]] -deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] -git-tree-sha1 = "e3290f2d49e661fbd94046d7e3726ffcb2d41053" -uuid = "5ae413db-bbd1-5e63-b57d-d24a61df00f5" -version = "2.2.4+0" - -[[deps.Einsum]] -deps = ["Compat"] -git-tree-sha1 = "4a6b3eee0161c89700b6c1949feae8b851da5494" -uuid = "b7d42ee7-0b51-5a75-98ca-779d3107e4c0" -version = "0.4.1" - -[[deps.ExprTools]] -git-tree-sha1 = "27415f162e6028e81c72b82ef756bf321213b6ec" -uuid = "e2ba6199-217a-4e67-a87a-7c52f15ade04" -version = "0.1.10" - -[[deps.Extents]] -git-tree-sha1 = "b309b36a9e02fe7be71270dd8c0fd873625332b4" -uuid = "411431e0-e8b7-467b-b5e0-f676ba4f2910" -version = "0.1.6" - -[[deps.FileWatching]] -uuid = "7b1f6079-737a-58dc-b8bc-7a2ca5c1b5ee" -version = "1.11.0" - -[[deps.GPUArraysCore]] -deps = ["Adapt"] -git-tree-sha1 = "83cf05ab16a73219e5f6bd1bdfa9848fa24ac627" -uuid = "46192b85-c4d5-4398-a991-12ede77f4527" -version = "0.2.0" - -[[deps.GeometryBasics]] -deps = ["EarCut_jll", "Extents", "IterTools", "LinearAlgebra", "PrecompileTools", "Random", "StaticArrays"] -git-tree-sha1 = "1f5a80f4ed9f5a4aada88fc2db456e637676414b" -uuid = "5c1252a2-5f33-56bf-86c9-59e7332b4326" -version = "0.5.10" - - [deps.GeometryBasics.extensions] - GeometryBasicsGeoInterfaceExt = "GeoInterface" - - [deps.GeometryBasics.weakdeps] - GeoInterface = "cf35fbd7-0cd7-5166-be24-54bfbe79505f" - -[[deps.Graphs]] -deps = ["ArnoldiMethod", "DataStructures", "Inflate", "LinearAlgebra", "Random", "SimpleTraits", "SparseArrays", "Statistics"] -git-tree-sha1 = "d80e8b7220b884117938b2d219487eea06f9e42e" -uuid = "86223c79-3864-5bf0-83f7-82e725a168b6" -version = "1.13.2" - - [deps.Graphs.extensions] - GraphsSharedArraysExt = "SharedArrays" - - [deps.Graphs.weakdeps] - Distributed = "8ba89e20-285c-5b6f-9357-94700520ee1b" - SharedArrays = "1a1011a3-84de-559e-8e89-a11a2f7dc383" - -[[deps.HostCPUFeatures]] -deps = ["BitTwiddlingConvenienceFunctions", "IfElse", "Libdl", "Preferences", "Static"] -git-tree-sha1 = "af9ab7d1f70739a47f03be78771ebda38c3c71bf" -uuid = "3e5b6fbb-0976-4d2c-9146-d79de83f2fb0" -version = "0.1.18" - -[[deps.IfElse]] -git-tree-sha1 = "debdd00ffef04665ccbb3e150747a77560e8fad1" -uuid = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173" -version = "0.1.1" - -[[deps.Inflate]] -git-tree-sha1 = "d1b1b796e47d94588b3757fe84fbf65a5ec4a80d" -uuid = "d25df0c9-e2be-5dd7-82c8-3ad0b3e990b9" -version = "0.1.5" - -[[deps.InteractiveUtils]] -deps = ["Markdown"] -uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" -version = "1.11.0" - -[[deps.InverseFunctions]] -git-tree-sha1 = "a779299d77cd080bf77b97535acecd73e1c5e5cb" -uuid = "3587e190-3f89-42d0-90ee-14403ec27112" -version = "0.1.17" - - [deps.InverseFunctions.extensions] - InverseFunctionsDatesExt = "Dates" - InverseFunctionsTestExt = "Test" - - [deps.InverseFunctions.weakdeps] - Dates = "ade2ca70-3891-5945-98fb-dc099432e06a" - Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" - -[[deps.IrrationalConstants]] -git-tree-sha1 = "b2d91fe939cae05960e760110b328288867b5758" -uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" -version = "0.2.6" - -[[deps.IterTools]] -git-tree-sha1 = "42d5f897009e7ff2cf88db414a389e5ed1bdd023" -uuid = "c8e1da08-722c-5040-9ed9-7db0dc04731e" -version = "1.10.0" - -[[deps.JLLWrappers]] -deps = ["Artifacts", "Preferences"] -git-tree-sha1 = "0533e564aae234aff59ab625543145446d8b6ec2" -uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" -version = "1.7.1" - -[[deps.JuliaSyntaxHighlighting]] -deps = ["StyledStrings"] -uuid = "ac6e5ff7-fb65-4e79-a425-ec3bc9c03011" -version = "1.12.0" - -[[deps.Kronecker]] -deps = ["LinearAlgebra", "NamedDims", "SparseArrays", "StatsBase"] -git-tree-sha1 = "9253429e28cceae6e823bec9ffde12460d79bb38" -uuid = "2c470bb0-bcc8-11e8-3dad-c9649493f05e" -version = "0.5.5" - -[[deps.LayoutPointers]] -deps = ["ArrayInterface", "LinearAlgebra", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface"] -git-tree-sha1 = "a9eaadb366f5493a5654e843864c13d8b107548c" -uuid = "10f19ff3-798f-405d-979b-55457f8fc047" -version = "0.1.17" - -[[deps.LibCURL]] -deps = ["LibCURL_jll", "MozillaCACerts_jll"] -uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" -version = "0.6.4" - -[[deps.LibCURL_jll]] -deps = ["Artifacts", "LibSSH2_jll", "Libdl", "OpenSSL_jll", "Zlib_jll", "nghttp2_jll"] -uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" -version = "8.15.0+0" - -[[deps.LibGit2]] -deps = ["LibGit2_jll", "NetworkOptions", "Printf", "SHA"] -uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" -version = "1.11.0" - -[[deps.LibGit2_jll]] -deps = ["Artifacts", "LibSSH2_jll", "Libdl", "OpenSSL_jll"] -uuid = "e37daf67-58a4-590a-8e99-b0245dd2ffc5" -version = "1.9.0+0" - -[[deps.LibSSH2_jll]] -deps = ["Artifacts", "Libdl", "OpenSSL_jll"] -uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" -version = "1.11.3+1" - -[[deps.Libdl]] -uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" -version = "1.11.0" - -[[deps.LieGroups]] -deps = ["LinearAlgebra", "Manifolds", "ManifoldsBase", "Quaternions", "Random", "StaticArrays"] -git-tree-sha1 = "7d7b54219f0204d539af303655b24d9a81f61b23" -uuid = "6774de46-80ba-43f8-ba42-e41071ccfc5f" -version = "0.1.9" - - [deps.LieGroups.extensions] - LieGroupsRecursiveArrayToolsExt = "RecursiveArrayTools" - LieGroupsTestExt = "Test" - - [deps.LieGroups.weakdeps] - RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" - Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" - -[[deps.LinearAlgebra]] -deps = ["Libdl", "OpenBLAS_jll", "libblastrampoline_jll"] -uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" -version = "1.12.0" - -[[deps.LinearMaps]] -deps = ["LinearAlgebra"] -git-tree-sha1 = "7f6be2e4cdaaf558623d93113d6ddade7b916209" -uuid = "7a12625a-238d-50fd-b39a-03d52299707e" -version = "3.11.4" - - [deps.LinearMaps.extensions] - LinearMapsChainRulesCoreExt = "ChainRulesCore" - LinearMapsSparseArraysExt = "SparseArrays" - LinearMapsStatisticsExt = "Statistics" - - [deps.LinearMaps.weakdeps] - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" - Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" - -[[deps.LogExpFunctions]] -deps = ["DocStringExtensions", "IrrationalConstants", "LinearAlgebra"] -git-tree-sha1 = "13ca9e2586b89836fd20cccf56e57e2b9ae7f38f" -uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" -version = "0.3.29" - - [deps.LogExpFunctions.extensions] - LogExpFunctionsChainRulesCoreExt = "ChainRulesCore" - LogExpFunctionsChangesOfVariablesExt = "ChangesOfVariables" - LogExpFunctionsInverseFunctionsExt = "InverseFunctions" - - [deps.LogExpFunctions.weakdeps] - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - ChangesOfVariables = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" - InverseFunctions = "3587e190-3f89-42d0-90ee-14403ec27112" - -[[deps.Logging]] -uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" -version = "1.11.0" - -[[deps.LoopVectorization]] -deps = ["ArrayInterface", "CPUSummary", "CloseOpenIntervals", "DocStringExtensions", "HostCPUFeatures", "IfElse", "LayoutPointers", "LinearAlgebra", "OffsetArrays", "PolyesterWeave", "PrecompileTools", "SIMDTypes", "SLEEFPirates", "Static", "StaticArrayInterface", "ThreadingUtilities", "UnPack", "VectorizationBase"] -git-tree-sha1 = "a9fc7883eb9b5f04f46efb9a540833d1fad974b3" -uuid = "bdcacae8-1622-11e9-2a5c-532679323890" -version = "0.12.173" - - [deps.LoopVectorization.extensions] - ForwardDiffExt = ["ChainRulesCore", "ForwardDiff"] - ForwardDiffNNlibExt = ["ForwardDiff", "NNlib"] - SpecialFunctionsExt = "SpecialFunctions" - - [deps.LoopVectorization.weakdeps] - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" - NNlib = "872c559c-99b0-510c-b3b7-b6c96a88d5cd" - SpecialFunctions = "276daf66-3868-5448-9aa4-cd146d93841b" - -[[deps.MacroTools]] -git-tree-sha1 = "1e0228a030642014fe5cfe68c2c0a818f9e3f522" -uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" -version = "0.5.16" - -[[deps.ManifoldDiff]] -deps = ["ADTypes", "DifferentiationInterface", "LinearAlgebra", "ManifoldsBase", "Markdown", "Random", "Requires"] -git-tree-sha1 = "28c69b401c75dd3f3b34a39bb7cb7948a2aeb61d" -uuid = "af67fdf4-a580-4b9f-bbec-742ef357defd" -version = "0.4.5" - - [deps.ManifoldDiff.weakdeps] - FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" - FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000" - ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" - ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" - Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" - -[[deps.Manifolds]] -deps = ["ADTypes", "DifferentiationInterface", "Einsum", "Graphs", "Kronecker", "LinearAlgebra", "ManifoldDiff", "ManifoldsBase", "Markdown", "MatrixEquations", "Quaternions", "Random", "SimpleWeightedGraphs", "SpecialFunctions", "StaticArrays", "Statistics", "StatsBase"] -git-tree-sha1 = "83324dcdfa1d6308b587d82e7eed418e8ec698bc" -uuid = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" -version = "0.11.9" - - [deps.Manifolds.extensions] - ManifoldsBoundaryValueDiffEqExt = "BoundaryValueDiffEq" - ManifoldsDistributionsExt = ["Distributions", "RecursiveArrayTools"] - ManifoldsHybridArraysExt = "HybridArrays" - ManifoldsNLsolveExt = "NLsolve" - ManifoldsOrdinaryDiffEqDiffEqCallbacksExt = ["DiffEqCallbacks", "OrdinaryDiffEq", "RecursiveArrayTools"] - ManifoldsOrdinaryDiffEqExt = "OrdinaryDiffEq" - ManifoldsRecipesBaseExt = ["Colors", "RecipesBase"] - ManifoldsRecursiveArrayToolsExt = "RecursiveArrayTools" - ManifoldsTestExt = "Test" - - [deps.Manifolds.weakdeps] - BoundaryValueDiffEq = "764a87c0-6b3e-53db-9096-fe964310641d" - Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" - DiffEqCallbacks = "459566f4-90b8-5000-8ac3-15dfb0a30def" - Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" - HybridArrays = "1baab800-613f-4b0a-84e4-9cd3431bfbb9" - NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" - OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" - RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" - RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" - Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" - -[[deps.ManifoldsBase]] -deps = ["LinearAlgebra", "Markdown", "Printf", "Random"] -git-tree-sha1 = "b44090cd3c5bf1b1a290110470b95cc0e1f67d11" -uuid = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb" -version = "2.3.0" - - [deps.ManifoldsBase.extensions] - ManifoldsBasePlotsExt = "Plots" - ManifoldsBaseQuaternionsExt = "Quaternions" - ManifoldsBaseRecursiveArrayToolsExt = "RecursiveArrayTools" - ManifoldsBaseStatisticsExt = "Statistics" - - [deps.ManifoldsBase.weakdeps] - Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" - Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" - RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" - Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" - -[[deps.ManualMemory]] -git-tree-sha1 = "bcaef4fc7a0cfe2cba636d84cda54b5e4e4ca3cd" -uuid = "d125e4d3-2237-4719-b19c-fa641b8a4667" -version = "0.1.8" - -[[deps.Markdown]] -deps = ["Base64", "JuliaSyntaxHighlighting", "StyledStrings"] -uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" -version = "1.11.0" - -[[deps.MatrixEquations]] -deps = ["LinearAlgebra", "LinearMaps"] -git-tree-sha1 = "51f3fade0b4ff2cf90b36b3312425460631abb56" -uuid = "99c1a7ee-ab34-5fd5-8076-27c950a045f4" -version = "2.5.6" - -[[deps.Missings]] -deps = ["DataAPI"] -git-tree-sha1 = "ec4f7fbeab05d7747bdf98eb74d130a2a2ed298d" -uuid = "e1d29d7a-bbdc-5cf2-9ac0-f12de2c33e28" -version = "1.2.0" - -[[deps.MozillaCACerts_jll]] -uuid = "14a3606d-f60d-562e-9121-12d972cd8159" -version = "2025.5.20" - -[[deps.NamedDims]] -deps = ["LinearAlgebra", "Statistics"] -git-tree-sha1 = "f9e4a49ecd1ea2eccfb749a506fa882c094152b4" -uuid = "356022a1-0364-5f58-8944-0da4b18d706f" -version = "1.2.3" - - [deps.NamedDims.extensions] - AbstractFFTsExt = "AbstractFFTs" - ChainRulesCoreExt = "ChainRulesCore" - CovarianceEstimationExt = "CovarianceEstimation" - TrackerExt = "Tracker" - - [deps.NamedDims.weakdeps] - AbstractFFTs = "621f4979-c628-5d54-868e-fcf4e3e8185c" - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - CovarianceEstimation = "587fd27a-f159-11e8-2dae-1979310e6154" - Requires = "ae029012-a4dd-5104-9daa-d747884805df" - Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" - -[[deps.NetworkOptions]] -uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" -version = "1.3.0" - -[[deps.OffsetArrays]] -git-tree-sha1 = "117432e406b5c023f665fa73dc26e79ec3630151" -uuid = "6fe1bfb0-de20-5000-8ca7-80f57d26f881" -version = "1.17.0" -weakdeps = ["Adapt"] - - [deps.OffsetArrays.extensions] - OffsetArraysAdaptExt = "Adapt" - -[[deps.OpenBLAS_jll]] -deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"] -uuid = "4536629a-c528-5b80-bd46-f80d51c5b363" -version = "0.3.29+0" - -[[deps.OpenLibm_jll]] -deps = ["Artifacts", "Libdl"] -uuid = "05823500-19ac-5b8b-9628-191a04bc5112" -version = "0.8.7+0" - -[[deps.OpenSSL_jll]] -deps = ["Artifacts", "Libdl"] -uuid = "458c3c95-2e84-50aa-8efc-19380b2a3a95" -version = "3.5.4+0" - -[[deps.OpenSpecFun_jll]] -deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl"] -git-tree-sha1 = "1346c9208249809840c91b26703912dff463d335" -uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" -version = "0.5.6+0" - -[[deps.OrderedCollections]] -git-tree-sha1 = "05868e21324cede2207c6f0f466b4bfef6d5e7ee" -uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" -version = "1.8.1" - -[[deps.Pkg]] -deps = ["Artifacts", "Dates", "Downloads", "FileWatching", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "Random", "SHA", "TOML", "Tar", "UUIDs", "p7zip_jll"] -uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" -version = "1.12.0" - - [deps.Pkg.extensions] - REPLExt = "REPL" - - [deps.Pkg.weakdeps] - REPL = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" - -[[deps.PolyesterWeave]] -deps = ["BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "Static", "ThreadingUtilities"] -git-tree-sha1 = "645bed98cd47f72f67316fd42fc47dee771aefcd" -uuid = "1d0040c9-8b98-4ee7-8388-3f51789ca0ad" -version = "0.2.2" - -[[deps.PrecompileTools]] -deps = ["Preferences"] -git-tree-sha1 = "07a921781cab75691315adc645096ed5e370cb77" -uuid = "aea7be01-6a6a-4083-8856-8a6e6704d82a" -version = "1.3.3" - -[[deps.Preferences]] -deps = ["TOML"] -git-tree-sha1 = "522f093a29b31a93e34eaea17ba055d850edea28" -uuid = "21216c6a-2e73-6563-6e65-726566657250" -version = "1.5.1" - -[[deps.Printf]] -deps = ["Unicode"] -uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" -version = "1.11.0" - -[[deps.PtrArrays]] -git-tree-sha1 = "1d36ef11a9aaf1e8b74dacc6a731dd1de8fd493d" -uuid = "43287f4e-b6f4-7ad1-bb20-aadabca52c3d" -version = "1.3.0" - -[[deps.Quaternions]] -deps = ["LinearAlgebra", "Random", "RealDot"] -git-tree-sha1 = "4d8c1b7c3329c1885b857abb50d08fa3f4d9e3c8" -uuid = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" -version = "0.7.7" - -[[deps.Random]] -deps = ["SHA"] -uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" -version = "1.11.0" - -[[deps.RealDot]] -deps = ["LinearAlgebra"] -git-tree-sha1 = "9f0a1b71baaf7650f4fa8a1d168c7fb6ee41f0c9" -uuid = "c1ae055f-0cd5-4b69-90a6-9a35b1a98df9" -version = "0.1.0" - -[[deps.RecipesBase]] -deps = ["PrecompileTools"] -git-tree-sha1 = "5c3d09cc4f31f5fc6af001c250bf1278733100ff" -uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" -version = "1.3.4" - -[[deps.RecursiveArrayTools]] -deps = ["Adapt", "ArrayInterface", "DocStringExtensions", "GPUArraysCore", "LinearAlgebra", "RecipesBase", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface"] -git-tree-sha1 = "d7c53a81b0ab7f62842ff79d9e74e34562d5834e" -uuid = "731186ca-8d62-57ce-b412-fbd966d074cd" -version = "3.42.0" - - [deps.RecursiveArrayTools.extensions] - RecursiveArrayToolsFastBroadcastExt = "FastBroadcast" - RecursiveArrayToolsForwardDiffExt = "ForwardDiff" - RecursiveArrayToolsKernelAbstractionsExt = "KernelAbstractions" - RecursiveArrayToolsMeasurementsExt = "Measurements" - RecursiveArrayToolsMonteCarloMeasurementsExt = "MonteCarloMeasurements" - RecursiveArrayToolsReverseDiffExt = ["ReverseDiff", "Zygote"] - RecursiveArrayToolsSparseArraysExt = ["SparseArrays"] - RecursiveArrayToolsStructArraysExt = "StructArrays" - RecursiveArrayToolsTablesExt = ["Tables"] - RecursiveArrayToolsTrackerExt = "Tracker" - RecursiveArrayToolsZygoteExt = "Zygote" - - [deps.RecursiveArrayTools.weakdeps] - FastBroadcast = "7034ab61-46d4-4ed7-9d0f-46aef9175898" - ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" - KernelAbstractions = "63c18a36-062a-441e-b654-da1e3ab1ce7c" - Measurements = "eff96d63-e80a-5855-80a2-b1b0885c5ab7" - MonteCarloMeasurements = "0987c9cc-fe09-11e8-30f0-b96dd679fdca" - ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" - SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" - StructArrays = "09ab397b-f2b6-538f-b94a-2f83cf4a842a" - Tables = "bd369af6-aec1-5ad0-b16a-f7cc5008161c" - Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" - Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" - -[[deps.Requires]] -deps = ["UUIDs"] -git-tree-sha1 = "62389eeff14780bfe55195b7204c0d8738436d64" -uuid = "ae029012-a4dd-5104-9daa-d747884805df" -version = "1.3.1" - -[[deps.Rotations]] -deps = ["LinearAlgebra", "Quaternions", "Random", "StaticArrays"] -git-tree-sha1 = "5680a9276685d392c87407df00d57c9924d9f11e" -uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" -version = "1.7.1" -weakdeps = ["RecipesBase"] - - [deps.Rotations.extensions] - RotationsRecipesBaseExt = "RecipesBase" - -[[deps.RuntimeGeneratedFunctions]] -deps = ["ExprTools", "SHA", "Serialization"] -git-tree-sha1 = "2f609ec2295c452685d3142bc4df202686e555d2" -uuid = "7e49a35a-f44a-4d26-94aa-eba1b4ca6b47" -version = "0.5.16" - -[[deps.SHA]] -uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" -version = "0.7.0" - -[[deps.SIMDTypes]] -git-tree-sha1 = "330289636fb8107c5f32088d2741e9fd7a061a5c" -uuid = "94e857df-77ce-4151-89e5-788b33177be4" -version = "0.1.0" - -[[deps.SLEEFPirates]] -deps = ["IfElse", "Static", "VectorizationBase"] -git-tree-sha1 = "456f610ca2fbd1c14f5fcf31c6bfadc55e7d66e0" -uuid = "476501e8-09a2-5ece-8869-fb82de89a1fa" -version = "0.6.43" - -[[deps.SciMLPublic]] -git-tree-sha1 = "ed647f161e8b3f2973f24979ec074e8d084f1bee" -uuid = "431bcebd-1456-4ced-9d72-93c2757fff0b" -version = "1.0.0" - -[[deps.Serialization]] -uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" -version = "1.11.0" - -[[deps.SimpleTraits]] -deps = ["InteractiveUtils", "MacroTools"] -git-tree-sha1 = "be8eeac05ec97d379347584fa9fe2f5f76795bcb" -uuid = "699a6c99-e7fa-54fc-8d76-47d257e15c1d" -version = "0.9.5" - -[[deps.SimpleWeightedGraphs]] -deps = ["Graphs", "LinearAlgebra", "Markdown", "SparseArrays"] -git-tree-sha1 = "749a2b719ec7f34f280c0d97ac3dab5c89818631" -uuid = "47aef6b3-ad0c-573a-a1e2-d07658019622" -version = "1.5.1" - -[[deps.SortingAlgorithms]] -deps = ["DataStructures"] -git-tree-sha1 = "64d974c2e6fdf07f8155b5b2ca2ffa9069b608d9" -uuid = "a2af1166-a08f-5f64-846c-94a0d3cef48c" -version = "1.2.2" - -[[deps.SparseArrays]] -deps = ["Libdl", "LinearAlgebra", "Random", "Serialization", "SuiteSparse_jll"] -uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" -version = "1.12.0" - -[[deps.SpecialFunctions]] -deps = ["IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] -git-tree-sha1 = "f2685b435df2613e25fc10ad8c26dddb8640f547" -uuid = "276daf66-3868-5448-9aa4-cd146d93841b" -version = "2.6.1" - - [deps.SpecialFunctions.extensions] - SpecialFunctionsChainRulesCoreExt = "ChainRulesCore" - - [deps.SpecialFunctions.weakdeps] - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - -[[deps.Static]] -deps = ["CommonWorldInvalidations", "IfElse", "PrecompileTools", "SciMLPublic"] -git-tree-sha1 = "49440414711eddc7227724ae6e570c7d5559a086" -uuid = "aedffcd0-7271-4cad-89d0-dc628f76c6d3" -version = "1.3.1" - -[[deps.StaticArrayInterface]] -deps = ["ArrayInterface", "Compat", "IfElse", "LinearAlgebra", "PrecompileTools", "Static"] -git-tree-sha1 = "96381d50f1ce85f2663584c8e886a6ca97e60554" -uuid = "0d7ed370-da01-4f52-bd93-41d350b8b718" -version = "1.8.0" -weakdeps = ["OffsetArrays", "StaticArrays"] - - [deps.StaticArrayInterface.extensions] - StaticArrayInterfaceOffsetArraysExt = "OffsetArrays" - StaticArrayInterfaceStaticArraysExt = "StaticArrays" - -[[deps.StaticArrays]] -deps = ["LinearAlgebra", "PrecompileTools", "Random", "StaticArraysCore"] -git-tree-sha1 = "b8693004b385c842357406e3af647701fe783f98" -uuid = "90137ffa-7385-5640-81b9-e52037218182" -version = "1.9.15" - - [deps.StaticArrays.extensions] - StaticArraysChainRulesCoreExt = "ChainRulesCore" - StaticArraysStatisticsExt = "Statistics" - - [deps.StaticArrays.weakdeps] - ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" - -[[deps.StaticArraysCore]] -git-tree-sha1 = "6ab403037779dae8c514bad259f32a447262455a" -uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" -version = "1.4.4" - -[[deps.Statistics]] -deps = ["LinearAlgebra"] -git-tree-sha1 = "ae3bb1eb3bba077cd276bc5cfc337cc65c3075c0" -uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" -version = "1.11.1" -weakdeps = ["SparseArrays"] - - [deps.Statistics.extensions] - SparseArraysExt = ["SparseArrays"] - -[[deps.StatsAPI]] -deps = ["LinearAlgebra"] -git-tree-sha1 = "178ed29fd5b2a2cfc3bd31c13375ae925623ff36" -uuid = "82ae8749-77ed-4fe6-ae5f-f523153014b0" -version = "1.8.0" - -[[deps.StatsBase]] -deps = ["AliasTables", "DataAPI", "DataStructures", "LinearAlgebra", "LogExpFunctions", "Missings", "Printf", "Random", "SortingAlgorithms", "SparseArrays", "Statistics", "StatsAPI"] -git-tree-sha1 = "be5733d4a2b03341bdcab91cea6caa7e31ced14b" -uuid = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91" -version = "0.34.9" - -[[deps.StyledStrings]] -uuid = "f489334b-da3d-4c2e-b8f0-e476e12c162b" -version = "1.11.0" - -[[deps.SuiteSparse_jll]] -deps = ["Artifacts", "Libdl", "libblastrampoline_jll"] -uuid = "bea87d4a-7f5b-5778-9afe-8cc45184846c" -version = "7.8.3+2" - -[[deps.SymbolicIndexingInterface]] -deps = ["Accessors", "ArrayInterface", "RuntimeGeneratedFunctions", "StaticArraysCore"] -git-tree-sha1 = "94c58884e013efff548002e8dc2fdd1cb74dfce5" -uuid = "2efcf032-c050-4f8e-a9bb-153293bab1f5" -version = "0.3.46" - - [deps.SymbolicIndexingInterface.extensions] - SymbolicIndexingInterfacePrettyTablesExt = "PrettyTables" - - [deps.SymbolicIndexingInterface.weakdeps] - PrettyTables = "08abe8d2-0d0c-5749-adfa-8a2ac140af0d" - -[[deps.TOML]] -deps = ["Dates"] -uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" -version = "1.0.3" - -[[deps.Tar]] -deps = ["ArgTools", "SHA"] -uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" -version = "1.10.0" - -[[deps.ThreadingUtilities]] -deps = ["ManualMemory"] -git-tree-sha1 = "d969183d3d244b6c33796b5ed01ab97328f2db85" -uuid = "8290d209-cae3-49c0-8002-c8c24d57dab5" -version = "0.5.5" - -[[deps.UUIDs]] -deps = ["Random", "SHA"] -uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" -version = "1.11.0" - -[[deps.UnPack]] -git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b" -uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" -version = "1.0.2" - -[[deps.Unicode]] -uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" -version = "1.11.0" - -[[deps.VectorizationBase]] -deps = ["ArrayInterface", "CPUSummary", "HostCPUFeatures", "IfElse", "LayoutPointers", "Libdl", "LinearAlgebra", "SIMDTypes", "Static", "StaticArrayInterface"] -git-tree-sha1 = "d1d9a935a26c475ebffd54e9c7ad11627c43ea85" -uuid = "3d5dd08c-fd9d-11e8-17fa-ed2836048c2f" -version = "0.21.72" - -[[deps.Zlib_jll]] -deps = ["Libdl"] -uuid = "83775a58-1f1d-513f-b197-d71354ab007a" -version = "1.3.1+2" - -[[deps.libblastrampoline_jll]] -deps = ["Artifacts", "Libdl"] -uuid = "8e850b90-86db-534c-a0d3-1478176c7d93" -version = "5.15.0+0" - -[[deps.nghttp2_jll]] -deps = ["Artifacts", "Libdl"] -uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" -version = "1.64.0+1" - -[[deps.p7zip_jll]] -deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"] -uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" -version = "17.7.0+0" 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 0000000000000000000000000000000000000000..1a4cff5777f373289e9b317bf489041efb4ecaff GIT binary patch literal 28426 zcmc(oO^+j4R)%W^hJls_Au%gfmL*WP91 z&aSF1)e;Gb4FVyt012^U$r{0LV8enfzW|9p!1Lbty5Wehotds~m$E9$5stX`oO924 z&wI`dfBD(3{OSjf^!k^-{kx|>`j7wmo$v9xetyW$*VFZI`+DnrSv%{$|M}N1zv8HV zeuJOwdO98c*=D(T+n-JP=Zo9~oJ-yihnvxOOXestjs z{UmfbAqbt+^HVS2q!;t$_@$ZOpugF^nNR!YZ~LS9>}t_J?~f7MblrdVZctq#2x2b` zV?T9bC*oK88rE5E=!S{s#c>=YaT4jY?ez7Qi#~fcnT@X2qZ_^TX2lKrZfiTCP~yjS|$rEymZ&HJ_eO zFSlpgiBaS1m`x>W8bewapHs?$Q(kp<@J-7shjtF(X*`?f(PPT-_zg7|$m^5g0>XW-ON z45H8rJ>QE%4A4uuZ}3t7fT)v9(rz5rWWAnUU2o6O^>%r~t&!#zqwV$2W}DS~^kzq> z31*eJao`86Hx_~=)W#uOsO5rtTP%o;%!HfCtXZUK;77jiCywuhtR9a;rH$P%oG00u zdG<{I*>3Br6S+iA9C;zy_V~A3Mjzhkx{1poiW1hm>j!D*>blu-p50vC=}I^b+(O`D zOO78KzmQF6*GfLR+hr4sC-qqEjLr>WV}98PKdxC(lmw|8rDiQeo+FQNl$rXG>!o3U z2!c3roV1d_r-NVA?vMr;w3~#9>o}|jb7t2RL1_WBDijEuj) z`f65OdW-`W>;_>PVrwzpA(XjUCfqz`+i)>re6p9gX_|Vm!%J3SnRjtk8aaL#V?lo4 zra1IUa@l&j9xW!l^{w#`rKQ{6^pNAs#Ly)LYx!)p>*dSoC(F4!>(e+G;0_0#7Y$t0 zAbV~-B;mIDRmm?-y@*BXdQlWu@@pJw7av}gaO%0JJVXf{^26Y;izq~RiRUY7=WFkB zIUdb>tL1F5<;{D^&P$991)jIZhFW4u>f`0pBuZ1)bKKOPd*hHjcgsHVNvzhh#dx-| zCQ=wq%xqKeic8qR(6m%r=QsdBWGa~WpsT%0%S|(} zc>cbn)gl&;V3uI$N$9zW?|Jr`Y98A!m`uC#>Edd8&D0C2U~fPg;)Cb0@T7T&p&ms;A$c#Nw^}QH^m+A7mMp;9w+u#6YJ%6WI)il!~a?VO93dQ>SArVemWrj z2A&y9$I@~L^S+xRMHlcEJ6>!pu9k!QC7H#_voV-#1Sno3u>;6cvGtdu`K9 zw^WA*;ECNh0blw-=$U|$1)kSNn#GAkr_f`a1$c`HBwjlh#$YtF&Jj-(;`tHj#0DOh zqxlA0SuOyLP>`gA1Nqt634FS^Z$hxbOw!beBA3{0VpYEO&PK0in=bpYu0c?Z0VOdw z#|cxQm%X$SfJR6d@tI-dN7=z{llxA4oi+yHT`6M6-|KIDCPHbL2 zQ96uRA}okBaoorxXkW3sRpLbC>rR4WxsQDTr9ljU?}T(CCE=*ej0FZA0KgU`ARRbhV9}6R#Ob(= z0M^9I{8tzZfj&Wk%L~Z`gPQgEZZLH8*0*VZ252W}1XzjJcP;yC9J0-?h!jLJI1m!Z zG>U)&CX}^AJQ6n&8>Q=z3FL5=0ev#MnXX4YQ0eT&+JJ)|acVt#-8=7$G6zz10*aA$ zd~#X@%@cLWL0x=M6xtBBai~pPDS(L&+}%oyZFVeovj)&0R&HtYWzKV_)Vb(BxCz{1NYPAkz@F;395 z@8Oe(3j{o9rq;Gtd5C6sAE*;HQP2>4_2P1dgSyjntt z6AC0q#G`TfIFN#x!o;08(4*WK$BGY<$XkpeCGX=(AgLsZ!MW8CHrWsceYdX5(8rxQ zv8W3NQnmHw3HB2vfy)4}YC}=9+*=3^qz|0~<%S=u?hW0h>FeILiN{=+sYCX<+#A@` zHggxTD8&L@k*>zTz}A_2%7S75spmV4vX;%(3@MJ2fQh0{rGA0MI6csg`ib{a9As4}(<@wq~K+JQn%{T@u$AK8m zR)vyNkj(f#F#+7;TY4(*?mVy3VAc`Y9O8;y^a~s}Qfn#zato&fS4YOe)XZ{gnLeiJ zfKikodR}7IMXrHy&6;q*aDH-gID1evb17ZvgS-+e(sf0Q1`s%yRI4aaBV?cnG;V<0 zJG_!l?2XCkBdA+eX}4o?>jF)YK2lacCLL0#8a?o6hv@EO0he13cV6x>Xa6VT_`vU>{+-G?o_x@ z+VZM@?i(KK_J+zEzk;8}FQ+P)JvFD17IzhnKo%J!-Am$q0b$Ty%7|PjVC)An7(WB~ zP!-u4hiucb)?*PsXWMH7^~#_f5{D^B;A9|3RR^@01C)n7k~H}Ul7;To)+~bc;6+0) zw#d&80b`_GL<^e|ArwnxiM_L4?Rs29JU2WDd42V2Z7<=2Zt=D9(`2m^e9Fa5}t2=|Y(w`h#I`NW*N^7c*w@ z!O&6apcH`1=zu#cWUhlx0c|1nXGN{(bQZjwSa|6<|2b@Q6yoQpafz>ogn6RMT&d}iNF@Bm}yg5 zc|LVQ6@r}MyIc?_UYU%zk_$TAyqv!3jhEBQ%h`BFk4W#s#d2}BTyL+JSIY&o$o7qC zPOutk$h={Y0unZyE%>W_h6)v&K|TWgLP^ZJ>rR-X#Yc27fL*-+HXuKd-q0E4E!MsR z;HA>VP?~xJ6QT%$E$lAm$r#<%1Yo zLlD)LAQikEFfUZ~qbgC{Gyb&Ry9oBI128@fB4`7$!tx0UB$aGJSJ6h+B<3M6l@%9( zUt!=R#Xf8^6uqI;i-XY5q`0hXvNHMh+Ew`&nH_q9k45yggCwRAL<7vCi48#P2l~Ql zHY^yG1qA?=tl2`w6?Pb3BcfH;Q}c_mPtsqA5mwnQ2t$&cNe~_+OM=plx+2qVbaC(= zMd1Z)o?yPb3zBAv>3UPXRrDLq01K4#g?V5&;f!T(+v&|t@1&$_#;Es;&#vIHkUtpA zL%A^-7OOOC2~vycC~uD|8v+gcMn|C4rZ(uWOpeOCa3^M__${c)B9cr-Kpmv!z}EoZ zFa_hLvTHIDKA2a^RC(o7yg?w#WX3zl_If%}585ASbfei^?PyDWXF8Y4L(lmA=9g5@ z&t2=TD4*O7APk5SzaS5^_on$MeCPi66zwkf?F0Z?dI1mJvgoXI-Fx5j3aXZ-F4bkh z(x7Ddb~S-+v-aRp)#PEUajjIP9@G~&ui(39>#^x_YA9L_D{B3W6U?VnH1!zsZju;XXBT&_F`d_qUuFVQo#X39Qc0LT~Q2cL{>@xi?m4KomuRN3>cPKg2M$EEt|j?3TpHU@fOc@&T}Ysiq#L_GP~Ql_iI zGe+J}U6^n`uy933iQ^2ZYV(5VWvYSdujOKb&&(_wd&cXVrt67(AY9vorFVu?<89+) z_5PXXQ1(?Fi|QagJ3q_whj=+&3{XffqHnGe(B%=nvvLQwFJ?k5y!A z94a1A<)!{eX^~4DbTEE4d73V4ndC|byVM-a8mEI)tFX>TsKjdi-0NXk)HxnWbX zSUc|Yza(14I7y;-0WsS zu?EXgMMp`^iq8)oD-Ed-Tq-&uNkYc1f?Eb#$m-i4ECz_bAk;zwsHnMPj4fj=ME(9^ zTbB_~sv%Vi)GrN2%gO;YE6CR*?OXZMVSQzG!8?_wIcdzO5B8#3=PNY|k$U53Dd_`% zCEJ6}1{2RljB~U+F!W>gHr=E4YO>r7fOBCsitNJ;^&a2KCsAfll!Ixemz~aoT_oCc zN>0|w1MzT_WZM^5-D}rcZIP8V5zBwBaAL5RaKzw5(1Ycq*cW!TUf$9vF*#FmP#QBE zg^K9Mr<&o9b!Q11CfNv^Mg-m-FW0oaJ|0bGx0^4gYwhb|BUAsJrUvSaroK*YBPK)| zEB&0M8M|AaPuY%brD_N9)+Z}cp(d;8XD(M&EGg99x@2zsyQUFtZB4z_iHHh(h)(oDf?|uw|$XnxS_ZT z>K?#2D!3U!$x5Dg+Wbcy;(-VTD2#594Ffp#G(%YBuTuEtA3 zryXec4-v_!MMJW?>8T6$kqH+`jkezX35{-~#N(UW#hLu!t2JDJc1lvv< z?k&_lK~y`N%^;P4K6EP~bbN7Dt)5I9hio}mbIoZoO^6Lk7rl>r`gOXP4EQ;Eeid26 zLJ_9~=>^iJm$%+w$w(o%WUG*XSX)NrbcuyRXy&XEUy-5NGs&9`?gqp;CDBH?F^&{w z+!Wy}3uzuZD4E{*dG9ALSn8wkmU1}_c5k1&J5u!;VYLBN1J3vJ(aiy0#NV)f?0%~E z&coK=l9Dim23Kd*Uh9FlF=C3{b}3_%1C=B7Rg- zCJ{1q>HG>0WS5Jn;BZ`eTU9&(MUXc##)|zd&Z2(L{v>&f#FP&@%5Az9g)3Ybfl9TG zYQnG4cGmuJs;W1QP&jBd%Q(~mXw|FkWj1;o+!k8^H#n|#3+q~kcxhN0_&-cpFUx!y zhiqXs{Zmd4TV_CSjBKD41K#53_eux27f&sYD`f0kiQ3F|qEtuQ_s*K%c#@=V{&-9 zC+JzV+mafG_8V0e#R+;?(NST%l9H)k0wJ((LJS}z+c{XsiPkSxwwjbM33P2FUL2&$ z4yp}stj0s)6nD3*DmU0H4!cDZE4mS=R*375iD+`Vs54I{T0go68pqgqkJbhZTWkFD|6IBt>J}) z+p_ja8tYAiy9otl!#?8g>8b#W*t>rkhdP8(DxV-stf2Ek?RacnDT8Fb!6JJO#fRD% zz%GjybM-ILzGNO&VUBHTyXR}jL>5xqh#Z8KCGn`!7p`?b{>V2AdmRr<#*gA?bfVU9Kn7^`9>0_(bJ)+;qA6oX2|5jY;i~{P>)!8P8D9 zicJQ|ZA+I0GG+hzlWfRhBP@DBOSq`^tduL_u2gGe|DVMW?=3fWOg6|jLz{Y|H5%*Y zg6-wa>DK`XP-&v+SqrX`^5)NPM{Cgm8RKIxzo?CL{#z(cVYWo`Bz1qMqqR5D^b3LOF9{Gwexv!-s}clzrSX?8 z!EsPpb9(SRL((!@uqYQH4==Qg$vdK2Wsi>xkCcZ#DN-J~8Ft6#hC2l_{2kD3NROqc zI=a?q3PK}hwDjjTM~i6=*<{IDQNsjq``&|HL0i*i4b&z-Q>&vzT<fs9zc{M)-9?=6)Xl|qfkj47MmL;ub7D%(7Zb(Mg$59eokTMtmR`7(P{A?C#f~G72_OFlBoY)Uth%idLq; zgV*CnUAuU93&-Dy2PYBNv!~Rz!Il`hn=6vDamdarN|evzB;W>U%!IdR-F1u>DnL)i zVW1E!_5-d?#H*=~o5&{oM1dgDFe?aNKRtx`*v)67hbrumV z*i#1uX-&U*tYiMUjvwg1Dko(vtU|O1Rr?fiuI%6-WvN8F!wJ)lMlP-rxr$y`m`(5YJqWFX<7ffDdFV+DoHxNUX90Ge)XLW5g4aRLPoW&z8N7^Vu`}hk6-v7;ita#C|}@*To~%iusv`^k^fco)tpUuAw)jK(@v$LhD7q#Fi)~C0vKzGFvYNxce~Fi<%;+ z70Wro4r4i(Pj!?3`OCk5Tt8L&QS@A0(3ke(RHxo^A50-L$|104P}Jff0GF*YLnBN~ z+p6H;dB(QRvKC`;EmC#!7y-(;WW*R{HOd0c*;2#*P+P(csVPmno^<+A223f1cnrA! zhln?+ZgXz~mct;gfls93N|O$kY}zkaLEDjeEEp6k9ZOwepv+}92VoSLVl}$G#!J0} zP3_qY zZr1|Vi|Oxy$i<^%tJg}v_hXadvXsH-AFhMNamS+cLEDkr2^@y7VB_>GcFeZEl}`_2l>rlY3f7)k7b^~hxnzEGNIJ95)2N?>KiH9YBPqXp)R-PpLKc#-aUle;?UV?lj(5CIj=t&R4J_ zE=|p$b%W6cRZgF$uJA2{I~qqi?D1@v>$ldlKvd z*XAVZFL)e1OV_e`HirV`cp|t0qv|936`v=@>?ypP&x1>(iB3rvO}5GvE0@yp zzG(#}B?PhJR1k&z}-Tu?tH96jOg&=a(=e`D9_K9AlW(1`#?Hw54! zX+2}}hk3Au$$XCO2N>x=h3X$8M^v{{t@>i>ZCOkrP|GUxPMW<#3NOLjYrCvzU7*B? zQ!?C%KKfJM>HxwRsAo?!RX-3OvkxZ@MBV6Q^Kq*ndG5+LETkmXVvG`2zMqkN~*(=p|GUBE2J$%v#*9Yipvcy^w5p#;U& z6xcSqiRN!^YXPU~u6HZzQWK_W$=nkEYj}tQ_sqMsv6A~*$4GPvK9cr6F$n;~gEKAW zP7Ub3cU`e?)xt*}PZkf#wi3rz<2ZR$&9kr8zHkqvkxoI0gs2EaI3B^ z8l)mlzz_sKeaQl;1%IQAGBeW63r@IEoa5k%jYMDVw4K|#BR13Z?2-%K@c6kutaeU3 z(Z;Ch`tutOnM;`ON_yMRkmfn%QoWHUA#K) z`Sa&|KiEbO?$Vxk`}gAXJzwAT+kDs7db(NOvN39NHd|aSTff2UufKjZ`3p|f&u?a5 z&t<+=Y?Gb+L66s4^S6F8Yr1#bmyHdoPc6*Gl3qV+^wTzw`KI^y=Rf(`7eDV?r&adQ z>%V>1H=lp^0be=xz3>0{QTLaqF>33Tf2G>)hjzbHt@_08@oV`FYr7xY{X(?rWBf?T*Brhb*IdJ)gJQ m$?gSq?WDTE-*5q@Vp+|XpZ(z1bo__>`B%Q`?voq+@BaaE#S8ra literal 0 HcmV?d00001 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 21557ca..0cc7ea6 100644 --- a/src/CameraModels.jl +++ b/src/CameraModels.jl @@ -1,6 +1,6 @@ module CameraModels -using +using LinearAlgebra, LieGroups, DocStringExtensions, @@ -8,18 +8,23 @@ using GeometryBasics - using RecursiveArrayTools: ArrayPartition + using LoopVectorization: @tturbo +# will be deprecated soon, when radialDistortion! is implemented on the GPU with KA.jl + -import Rotations as Rot_ -import Base: getindex, getproperty, show +import Base: + getindex, + getproperty, + show include("ExportAPI.jl") include("entities/GeneralTypes.jl") include("entities/CameraCalibration.jl") -include("services/CameraCalibration.jl") +include("services/Helper.jl") +include("services/Display.jl") include("services/CameraServices.jl") include("services/Utils.jl") diff --git a/src/ExportAPI.jl b/src/ExportAPI.jl index fb227d2..cd7210b 100644 --- a/src/ExportAPI.jl +++ b/src/ExportAPI.jl @@ -1,21 +1,45 @@ +# Abstract type export AbstractCameraModel -export CameraCalibration, CameraCalibrationMutable +# Camera structures +export + CameraCalibration, + CameraCalibrationMutable -export toNonhomogeneous -export CameraSkewDistortion -export undistortPoint -export Ray, PixelIndex -export canreproject, sensorsize #, origin, direction, -export project, projectHomogeneous -export backproject, backprojectHomogeneous -export pp_w, pp_h, f_w, f_h +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 -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 +# Elemnents from JuliaGeometry/GeometryBasics +export + Vector2, + Vector3, + Point3, + origin3d # Origin Point3 diff --git a/src/entities/CameraCalibration.jl b/src/entities/CameraCalibration.jl index 72d4bb0..91a1766 100644 --- a/src/entities/CameraCalibration.jl +++ b/src/entities/CameraCalibration.jl @@ -1,17 +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. @@ -20,17 +21,20 @@ 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 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 """ @@ -38,18 +42,27 @@ Base.@kwdef struct CameraCalibration{R <: Real, N} <: AbstractCameraModel """ number of pixels from left to right """ width::Int = 640 """ distortion coefficients up to fifth order """ - kc::SVector{N, R} = SVector(zeros(5)...) + 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.0]']) + + 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 + $(TYPEDEF) See [`CameraCalibration`](@ref). + +kc, K and Ki are stored as Mutable Vectors and Mutable Matrices (`MVector`, `MMatrix`), +compared to `CameraCalibration` + """ Base.@kwdef mutable struct CameraCalibrationMutable{R <: Real, N} <: AbstractCameraModel """ number of pixels from top to bottom """ @@ -57,9 +70,13 @@ Base.@kwdef mutable struct CameraCalibrationMutable{R <: Real, N} <: AbstractCam """ number of pixels from left to right """ width::Int = 640 """ distortion coefficients up to fifth order """ - kc::MVector{N, R} = MVector(zeros(5)...) + 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.0]']) + 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 diff --git a/src/entities/GeneralTypes.jl b/src/entities/GeneralTypes.jl index 542f324..a672b45 100644 --- a/src/entities/GeneralTypes.jl +++ b/src/entities/GeneralTypes.jl @@ -1,9 +1,19 @@ +# Abstract type +abstract type AbstractCameraModel end + + struct PixelIndex{VALID, T <: Real} row::T 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) + +PixelIndex( + u::T, + v::T; + valid::Bool = true, + depth = zero(T) +) where {T <: Real} = PixelIndex{valid, T}(u, v, depth) Base.getindex(p::PixelIndex, i::Int) = i == 1 ? p.row : @@ -16,8 +26,6 @@ const Vector2 = Vec{2, Float64} const Point3 = Point{3, Float64} const Vector3 = Vec{3, Float64} -# Abstract type -abstract type AbstractCameraModel end origin3d = Point3(0, 0, 0) diff --git a/src/services/CameraCalibration.jl b/src/services/CameraCalibration.jl deleted file mode 100644 index c286f95..0000000 --- a/src/services/CameraCalibration.jl +++ /dev/null @@ -1,34 +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, "}") - 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) - - -""" - $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.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/CameraServices.jl b/src/services/CameraServices.jl index 357663c..b2f9f60 100644 --- a/src/services/CameraServices.jl +++ b/src/services/CameraServices.jl @@ -45,7 +45,8 @@ direction(ray::Ray) = ray.direction """ 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} +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))` """ @@ -137,19 +138,31 @@ canreproject(camera::AbstractCameraModel) = true ## 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) +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 - x0 = x y = (y - cy) / fy - y0 = y + x0, y0 = x, y for _ in 1:iter_num r2 = x^2 + y^2 - k_inv = 1 / (1 + k1 * r2 + k2 * r2^2 + k3 * r2^3) + 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 @@ -163,6 +176,18 @@ 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). + + +Project a 3D point in homogeneous coordinates `r_P` (camera frame) to a `PixelIndex`. +""" function project!( ret::AbstractVector{<:Real}, ci::CameraCalibration, @@ -175,7 +200,7 @@ function project!( end """ - $SIGNATURES + $(SIGNATURES) Project a world scene onto an image. @@ -191,7 +216,7 @@ Also see: [`backproject`](@ref) function project( model::AbstractCameraModel, r_P::Union{<:AbstractVector{<:Real}, <:Point3}; - c_T_r = ArrayPartition(Vector3(0.0, 0.0, 0.0), Mat{3,3}(1.0*I(3))) + 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) @@ -199,25 +224,25 @@ end """ - $SIGNATURES + $(SIGNATURES) Project a 3D point in homogeneous coordinates `c_Ph` (camera frame) to a `PixelIndex`. """ function projectHomogeneous( - cam::AbstractCameraModel, - c_Ph::AbstractVector{<:Real} -) + cam::AbstractCameraModel, + c_Ph::AbstractVector{<:Real} + ) x, y, z, w = c_Ph - + # Project to image plane inv_z = 1 / z col = x * f_w(cam) * inv_z + pp_w(cam) row = y * f_h(cam) * inv_z + pp_h(cam) - + # Depth and validity (point must be in front of camera) depth = z / w valid = (w == 0 && z > 0) || depth > 0 - + return PixelIndex(col, row; depth, valid) end @@ -228,7 +253,7 @@ end """ - $SIGNATURES + $(SIGNATURES) Backproject from an image into a world scene. @@ -241,30 +266,28 @@ 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) 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}, @@ -278,4 +301,3 @@ function cameraResidual!( res[1:2] += z[1:2] return nothing end - 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/Utils.jl b/src/services/Utils.jl index eeaa6b0..b2f82fa 100644 --- a/src/services/Utils.jl +++ b/src/services/Utils.jl @@ -1,30 +1,18 @@ """ - $SIGNATURES + $(SIGNATURES) Converts a homogeneous point to a non-homogeneous point, i.e. divides by the last element. """ function toNonhomogeneous(_Ph::AbstractVector) if length(_Ph) == 4 - return Point3(_Ph[1]/_Ph[4], _Ph[2]/_Ph[4], _Ph[3]/_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]) + return Point2(_Ph[1] / _Ph[3], _Ph[2] / _Ph[3]) end -""" - $SIGNATURES - -Constructor helper for creating a camera model. -""" -function CameraSkewDistortion(width, height, fc, cc, skew, kc) - K = Mat{3, 3}( - fc[1], 0.0, 0.0, - skew, fc[2], 0.0, - cc[1], cc[2], 1.0) - return CameraCalibration(;width, height, K, kc=SVector(float.(kc)...)) -end """ - $SIGNATURES + $(SIGNATURES) Slightly general Radial Distortion type, currently limited to StaticArrays.jl on CPU, @@ -153,7 +141,7 @@ function intersectLineToPlane3D( end """ - $SIGNATURES + $(SIGNATURES) 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). @@ -207,8 +195,21 @@ function intersectRayToPlane( 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 1; -1 0 0; 0 -1 0.0]), + 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) diff --git a/test/CameraTestBench.jl b/test/CameraTestBench.jl index 1b6930a..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 <: AbstractCameraModel +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,11 +26,12 @@ 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. - # backproject returns a direction vector, and we assume origin is (0,0,0) for now if not specified + # 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) - + ray = Ray(Point3(0, 0, 0), dir) + # Generate a 3D point along that ray. point = CameraModels.origin(ray) + 4.2 .* CameraModels.direction(ray) @@ -35,13 +39,13 @@ function run_test_bench(model::C, pixel_accuracy::Float64 = 1e-5, ray_accuracy:: # Some models might not implement project. if canreproject(model) - reprojection = project(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 + else @info "project not implemented for $(C)." end end -end \ No newline at end of file +end diff --git a/test/SkewDistortion.jl b/test/SkewDistortion.jl deleted file mode 100644 index bf4a28b..0000000 --- a/test/SkewDistortion.jl +++ /dev/null @@ -1,17 +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 diff --git a/test/multiview_manifolds.jl b/test/multiview_manifolds.jl index 0b4b202..4cbee4c 100644 --- a/test/multiview_manifolds.jl +++ b/test/multiview_manifolds.jl @@ -8,53 +8,54 @@ M = SpecialEuclideanGroup(3; variant = :right) @testset "Multiview optimization of point in front of 2 cameras" begin -cam = CameraModels.CameraCalibration() + cam = CameraModels.CameraCalibration() -obs1 = CameraModels.PixelIndex(320, 240) -obs2 = CameraModels.PixelIndex(315, 240) + obs1 = CameraModels.PixelIndex(320, 240) + obs2 = CameraModels.PixelIndex(315, 240) -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_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]) -function projectPointFrom(cam, c_H_w, w_Ph) - c_Ph = c_H_w*w_Ph |> SVector{4} - CameraModels.projectHomogeneous(cam,c_Ph) -end + 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 + 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 + 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_Ph = SVector(10.,0.,0.,1.) + w_Ph = SVector(10.0, 0.0, 0.0, 1.0) -cost(w_Ph) + cost(w_Ph) -cost(SVector(0.1,0.,0.,1.)) -cost(SVector(0.5,0.,0.,1.)) + cost(SVector(0.1, 0.0, 0.0, 1.0)) + cost(SVector(0.5, 0.0, 0.0, 1.0)) -w_Res = Optim.optimize( - cost, - [1.0;0.0;0.0], - LBFGS(), -) + w_Res = Optim.optimize( + cost, + [1.0;0.0;0.0], + LBFGS(), + ) -@show w_Res.minimizer -@show w_P3 = w_Res.minimizer -@test isapprox([10.56;0;0], w_P3; atol=1e-3) + # @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 7a55bc3..e3dc222 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -11,7 +11,6 @@ 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) end @@ -21,4 +20,3 @@ 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 390688c..16bf28d 100644 --- a/test/testutils.jl +++ b/test/testutils.jl @@ -13,14 +13,14 @@ using LieGroups: @testset "Test intersect of line and plane" begin # Define plane - floornorm = [0;0;1.0] - floorcenter = [0;0;5.0] + floornorm = [0, 0, 1.0] + floorcenter = [0, 0, 5.0] # Define ray - raydir = [0;-1;-1.0] - raypnt = [0; 0;10.0] + raydir = [0, -1, -1.0] + raypnt = [0, 0, 10.0] pt = intersectLineToPlane3D(floornorm, floorcenter, raydir, raypnt) - @test isapprox([0;-5;5.0], pt) + @test isapprox([0, -5, 5.0], pt) end @@ -29,21 +29,32 @@ end M = SpecialEuclideanGroup(3; variant = :right) Mr = SpecialOrthogonalGroup(3) - R0 = [1 0 0; 0 1 0; 0 0 1.0] - + 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] + 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_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 @@ -57,10 +68,14 @@ end ## 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]))) + 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 @@ -69,14 +84,12 @@ end test_camera = CameraCalibration( height = 720, width = 1280, - kc = SVector(-0.12, 0.03, 0.0008, -0.0006, -0.004), - K = SMatrix{3, 3}( - [ - 800.0 0.0 360.0; - 0.0 800.0 640.0; - 0.0 0.0 1.0 - ] - ) + 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 + ] )