CPU imlementation for radialDistortion! with @tturbo

Author: Thimoté Dupuch

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"

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")

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 """

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