Merge pull request #17 from JuliaRobotics/23Q1/enh/cleanup

refactor, consolidate types

Author: dehann

NEWS.md

@@ -4,6 +4,7 @@
 
 - Remove method redefinition warnings by dropping additional types and keeping just construction helpers.  Only types are `CameraCalibration[Mutable]`.
 - Consolidate type `CameraIntrinsic`
+
 ## v0.1
 
 - Created consolidated `CameraCalibration` type to replace various previous camera models (all doing about the same thing),

src/CameraModels.jl

@@ -5,7 +5,7 @@ using Manifolds
 using DocStringExtensions
 using StaticArrays
 import Rotations as Rot_
-
+import Base: getindex, getproperty
 
 
 # exports

src/Deprecated.jl

@@ -5,7 +5,16 @@
 ## ================================================================================================
 ## consolidated types below
 
+@deprecate CameraExtrinsic(R::AbstractMatrix=[1 0 0; 0 1 0; 0 0 1.], t::AbstractVector=[0,0,0.]) ArrayPartition(SVector(t...),SMatrix(R))
 
+# Camera extrinsic must be world in camera frame (cRw)
+# Base.@kwdef struct CameraExtrinsic{T <: Real}
+#   R::SMatrix{3,3,T,9} = one(Rot_.RotMatrix{3, Float64}).mat
+#   t::SVector{3,T} = SVector(0,0,0.)
+# end
+
+
+@deprecate PixelCoordinate(row,col) PixelIndex(row,col)
 
 # CameraCalibration(
 #   height::Int= 480,
@@ -79,7 +88,7 @@ CameraModel = (@warn("CameraModels.CameraModel is deprecated, use CameraModels.A
 
 
 export CameraModelandParameters
-const CameraModelandParameters = (@warn("CameraModels.CameraModelandParameters is deprecated, use CamereModels.CameraCalibration instead.");CameraCalibration)
+# const CameraModelandParameters = (@warn("CameraModels.CameraModelandParameters is deprecated, use CamereModels.CameraCalibration instead.");CameraCalibration)
 
 function CameraModelandParameters(
     width::Int,
@@ -189,7 +198,7 @@ export Pinhole
 
 
 ## From yakir12/CameraModels.jl
-const Pinhole = (@warn("CameraModels.Pinhole is deprecated, use CamereModels.CameraCalibration instead."); CameraCalibration)
+# const Pinhole = (@warn("CameraModels.Pinhole is deprecated, use CamereModels.CameraCalibration instead."); CameraCalibration)
 function Pinhole(columns::Int,rows::Int,prinicipalpoint,focallength::Vector2 )
   @warn "CameraModels.Pinhole is deprecated, use CamereModels.CameraCalibration instead."
   f_w,f_h = focallength[1], focallength[2]
@@ -225,7 +234,7 @@ export CameraIntrinsic
 
 
 function Base.getproperty(
-  x::Union{<:Pinhole, CameraModelandParameters},
+  x::CameraCalibration, # Union{<:Pinhole, CameraModelandParameters},
   f::Symbol,
 )
   if f == :skew

src/ExportAPI.jl

@@ -7,9 +7,7 @@ export CameraCalibrationT, CameraCalibration, CameraCalibrationMutable
 
 export CameraSkewDistortion
 
-export RadialDistortion
-
-export Ray, PixelCoordinate
+export Ray, PixelIndex
 export pixel2ray, point2pixel, canreproject, sensorsize #, origin, direction, 
 
 export lookdirection, updirection #, columns, rows

src/entities/CameraCalibration.jl

@@ -80,15 +80,9 @@ CameraCalibrationT = Union{<:CameraCalibration, <:CameraCalibrationMutable}
 ## ===========================================================================
 
 
-# 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
-
 Base.@kwdef struct CameraModelFull
   ci::CameraCalibration = CameraCalibration()
-  ce::CameraExtrinsic   = CameraExtrinsic()
+  ce::ArrayPartition    = CameraExtrinsic()
 end
 
 

src/entities/GeneralTypes.jl

@@ -1,8 +1,21 @@
 
 
-const PixelCoordinate = SVector{2, Float64}
-const Vector2 = SVector{2, Float64}
+struct PixelIndex{T <: Real}
+    row::T
+    col::T
+end
+
+function Base.getindex(p::PixelIndex,i::Int)
+    if i === 1
+        p.row
+    elseif i === 2  
+        p.col
+    else
+        DomainError("Camera only has rows and columns, cannot index to $i")
+    end
+end
 
+const Vector2 = SVector{2, Float64}
 const Point3 = SVector{3, Float64}
 const Vector3 = SVector{3, Float64}
 

src/services/CameraCalibration.jl

@@ -22,67 +22,3 @@ function CameraCalibrationMutable(img::AbstractMatrix{T}) where T
   CameraCalibrationMutable(;width, height, K)
 end
 
-
-
-"""
-    $SIGNATURES
-
-Slightly general Radial Distortion type, currently limited to StaticArrays.jl on CPU, but can later be extended to utilize GPUs -- see notes.
-
-Notes
-- Make sure `dest` image is large enough to encapsulate the resulting image after un-distortion
-
-Example
-```julia
-using Images, FileIO, CameraModels
-
-# load the image
-img = load("myimg.jpg")
-
-# genereate a radial distortion object
-radialdistortion = RadialDistortion()
-```
-
-Reference:
-From Wikipedia: https://en.wikipedia.org/wiki/Distortion_(optics)
-  ( xd ,   yd ) = distorted image point as projected on image plane using specified lens,
-  ( xu ,   yu ) = undistorted image point as projected by an ideal pinhole camera,
-  ( xc ,   yc ) = distortion center,
-              r = sqrt( (xd - xc)^2 + (yd - yc)^2 )
-
-```math
-xu = xc + (xd + xc) / (1 + K1*(r^2) + K2*(r^4) + ...)
-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
-"""
-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)
-    end
-    # calculate the new 'undistorted' coordinates and set equal to incoming image
-    @inbounds @fastmath h_u = cc.center[1] + h_/_denomin
-    @inbounds @fastmath w_u = cc.center[2] + h_/_denomin
-    dest[h_u,w_u] = src[h_d,w_d]
-  end
-  nothing
-end

src/services/CameraServices.jl

@@ -96,7 +96,7 @@ Also see: [`project`](@ref)
 """
 function backproject(
     model::CameraCalibrationT, 
-    px_coord::Union{<:AbstractVector{<:Real}, <:PixelCoordinate}
+    px_coord::Union{<:AbstractVector{<:Real}, <:PixelIndex}
   )
   #
   x =  (px_coord[1] - c_w(model)) / f_w(model)
@@ -124,7 +124,7 @@ end
 function project!(
   ret::AbstractVector{<:Real}, 
   ci::CameraCalibration, #CameraIntrinsic, 
-  ce::CameraExtrinsic, 
+  ce::ArrayPartition, 
   pt::Vector{Float64}
 )
   res = ci.K*(ce.R*pt + ce.t)
@@ -152,7 +152,7 @@ function cameraResidual!(
     res::AbstractVector{<:Real},
     z::AbstractVector{<:Real},
     ci::CameraCalibration, #CameraIntrinsic,
-    ce::CameraExtrinsic,
+    ce::ArrayPartition,
     pt::AbstractVector{<:Real}  
   )
   # in place memory operations

src/services/Utils.jl

@@ -16,6 +16,69 @@ function CameraSkewDistortion(width,height,fc,cc,skew,kc)
     CameraModelandParameters(width,height,fc,cc,skew,kc,KK,Ki)
 end
 
+"""
+    $SIGNATURES
+
+Slightly general Radial Distortion type, currently limited to StaticArrays.jl on CPU, but can later be extended to utilize GPUs -- see notes.
+
+Notes
+- Make sure `dest` image is large enough to encapsulate the resulting image after un-distortion
+
+Example
+```julia
+using Images, FileIO, CameraModels
+
+# load the image
+img = load("myimg.jpg")
+
+# TODO, store radialDistortion in CameraCalibration
+```
+
+Reference:
+From Wikipedia: https://en.wikipedia.org/wiki/Distortion_(optics)
+  ( xd ,   yd ) = distorted image point as projected on image plane using specified lens,
+  ( xu ,   yu ) = undistorted image point as projected by an ideal pinhole camera,
+  ( xc ,   yc ) = distortion center,
+              r = sqrt( (xd - xc)^2 + (yd - yc)^2 )
+
+```math
+xu = xc + (xd + xc) / (1 + K1*(r^2) + K2*(r^4) + ...)
+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
+"""
+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)
+        end
+        # calculate the new 'undistorted' coordinates and set equal to incoming image
+        @inbounds @fastmath h_u = cc.center[1] + h_/_denomin
+        @inbounds @fastmath w_u = cc.center[2] + h_/_denomin
+        dest[h_u,w_u] = src[h_d,w_d]
+    end
+    nothing
+end
+
+
 
 
 function intersectLineToPlane3D(

test/CameraTestBench.jl

@@ -31,7 +31,8 @@ function run_test_bench(model::C, pixel_accuracy::Float64 = 1e-5, ray_accuracy::
         # Some models might not implement point2pixel.
         if canreproject(model)
             reprojection = point2pixel(model, point)
-            @test some_pixel_location ≈ reprojection atol = pixel_accuracy
+            @test some_pixel_location[1] ≈ reprojection[1] atol = pixel_accuracy
+            @test some_pixel_location[2] ≈ reprojection[2] atol = pixel_accuracy
         else 
             @info "point2pixel not implemented for $(C)."
         end