Improves performance through consistent use of StaticArrays

REQUIRE

@@ -1,7 +1,6 @@
 julia 0.7
-StaticArrays
+StaticArrays 0.8.3
 Juno
-Plots
 LsqFit
 LinearAlgebra
 BenchmarkTools

src/MultipleViewGeometry.jl

@@ -16,7 +16,7 @@ export CostFunction, ApproximateMaximumLikelihood, AML
 export HomogeneousCoordinates
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices
-export Point2DH, Point3DH
+export Point2D, Point2DH, Point3D, Point3DH
 export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme
 export NoiseModel, GaussianNoise
 
@@ -81,7 +81,7 @@ include("moments/ModuleMoments.jl")
 include("cost_function/ModuleCostFunction.jl")
 include("estimate/ModuleEstimation.jl")
 include("construct/ModuleConstruct.jl")
-include("draw/ModuleDraw.jl")
+#include("draw/ModuleDraw.jl")
 include("constraints/ModuleConstraints.jl")
 include("triangulation/ModuleTriangulation.jl")
 include("noise/ModuleNoise.jl")
@@ -98,7 +98,7 @@ using .ModuleEstimation
 using .ModuleMoments
 using .ModuleCostFunction
 using .ModuleConstruct
-using .ModuleDraw
+#using .ModuleDraw
 using .ModuleConstraints
 using .ModuleTriangulation
 using .ModuleNoise

src/constraints/constraints.jl

@@ -14,8 +14,8 @@ function satisfy(entity::FundamentalMatrix, constraint::EpipolarConstraint, 𝐅
 
     I = 10
     for n = 1:N
-        𝐦 = ℳ[n]
-        𝐦ʹ = ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ = hom(ℳʹ[n])
         𝐦ₕ = init_correction_view_1(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
         𝐦ₕʹ= init_correction_view_2(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
         for i = 1:I
@@ -24,8 +24,8 @@ function satisfy(entity::FundamentalMatrix, constraint::EpipolarConstraint, 𝐅
             𝐦ₕ = update_correction_view_1(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
             𝐦ₕʹ = update_correction_view_2(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
         end
-        𝒪[n] = 𝐦ₕ
-        𝒪ʹ[n] = 𝐦ₕʹ
+        𝒪[n] = hom⁻¹(𝐦ₕ)
+        𝒪ʹ[n] = hom⁻¹(𝐦ₕʹ)
     end
     𝒪 ,𝒪ʹ
 end

src/construct/construct_essentialmatrix.jl

@@ -7,5 +7,5 @@ function construct( e::EssentialMatrix, 𝐅::AbstractArray,  𝐊₁::AbstractA
     end
     # Equation 9.12 Chapter 9 from Hartley & Zisserman
     𝐄 = 𝐊₂'*𝐅*𝐊₁
-    MMatrix{3,3,Float64,3*3}(𝐄)
+    SMatrix{3,3,Float64,3*3}(𝐄)
 end

src/construct/construct_fundamentalmatrix.jl

@@ -14,7 +14,7 @@ function construct( e::FundamentalMatrix,
         throw(ArgumentError("Expect length-3 translation vectors."))
     end
     𝐅 = vec2antisym(𝐊₂*𝐑₂*(𝐭₁ .- 𝐭₂))*𝐊₂*𝐑₂/𝐑₁/𝐊₁
-    MMatrix{3,3,Float64,3*3}(𝐅)
+    SMatrix{3,3,Float64,3*3}(𝐅)
 end
 
 function construct( e::FundamentalMatrix, 𝐏₁::AbstractArray, 𝐏₂::AbstractArray)
@@ -24,5 +24,5 @@ function construct( e::FundamentalMatrix, 𝐏₁::AbstractArray, 𝐏₂::Abstr
     𝐜₁ = SVector{4,Float64}(nullspace(Array(𝐏₁)))
     𝐞₂ = 𝐏₂*𝐜₁
     𝐅 = vec2antisym(𝐞₂)*𝐏₂*pinv(Array(𝐏₁))
-    MMatrix{3,3,Float64,3*3}(𝐅)
+    SMatrix{3,3,Float64,3*3}(𝐅)
 end

src/cost_function/cost_functions.jl

@@ -12,8 +12,8 @@ function cost(c::CostFunction, entity::FundamentalMatrix, 𝛉::AbstractArray, 
     @inbounds for n = 1:N
         𝚲ₙ[1:2,1:2] .=  Λ₁[n][index,index]
         𝚲ₙ[3:4,3:4] .=  Λ₂[n][index,index]
-        𝐦 = ℳ[n]
-        𝐦ʹ= ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ= hom(ℳʹ[n])
         𝐔ₙ = (𝐦 ⊗ 𝐦ʹ)
         ∂ₓ𝐮ₙ =  [(𝐞₁ ⊗ 𝐦ʹ) (𝐞₂ ⊗ 𝐦ʹ) (𝐦 ⊗ 𝐞₁) (𝐦 ⊗ 𝐞₂)]
         𝐁ₙ =  ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
@@ -229,9 +229,9 @@ end
 
 function _X(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray,𝒞::Tuple{AbstractArray, Vararg{AbstractArray}}, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
     l = length(𝛉)
-    𝐈ₗ = Matrix{Float64}(I,l,l)
-    𝐍 = fill(0.0,(l,l))
-    𝐌 = fill(0.0,(l,l))
+    𝐈ₗ = SMatrix{l,l}(1.0I)
+    𝐍 = @SMatrix zeros(l,l)
+    𝐌 = @SMatrix zeros(l,l)
     N = length(𝒟[1])
     ℳ, ℳʹ = 𝒟
     Λ₁, Λ₂ = 𝒞
@@ -242,8 +242,8 @@ function _X(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray,𝒞:
         index = SVector(1,2)
         𝚲ₙ[1:2,1:2] .=  Λ₁[n][index,index]
         𝚲ₙ[3:4,3:4] .=  Λ₂[n][index,index]
-        𝐦 = ℳ[n]
-        𝐦ʹ= ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ= hom(ℳʹ[n])
         𝐔ₙ = (𝐦 ⊗ 𝐦ʹ)
         ∂ₓ𝐮ₙ =  [(𝐞₁ ⊗ 𝐦ʹ) (𝐞₂ ⊗ 𝐦ʹ) (𝐦 ⊗ 𝐞₁) (𝐦 ⊗ 𝐞₂)]
         𝐁ₙ =  ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
@@ -285,50 +285,50 @@ end
 
 function T(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray, 𝒞::Tuple{AbstractArray, Vararg{AbstractArray}}, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
     l = length(𝛉)
-    𝐈ₗ = Matrix{Float64}(I, l, l)
-    𝐈ₘ = Iₘ(entity)
-    𝐍 = fill(0.0,(l,l))
-    𝐌 = fill(0.0,(l,l))
-    𝐓 = fill(0.0,(l,l))
+    𝐈ₗ = SMatrix{l,l}(1.0I)
+    𝐈ₘ =  Iₘ(entity)
+    𝐍 = @SMatrix zeros(l,l)
+    𝐌 = @SMatrix zeros(l,l)
+    𝐓 = @SMatrix zeros(l,l)
     N = length(𝒟[1])
     ℳ, ℳʹ = 𝒟
     Λ₁, Λ₂ = 𝒞
     𝚲ₙ = @MMatrix zeros(4,4)
     𝐞₁ = @SMatrix [1.0; 0.0; 0.0]
     𝐞₂ = @SMatrix [0.0; 1.0; 0.0]
-    for n = 1:N
+    for n = 1: N
         index = SVector(1,2)
         𝚲ₙ[1:2,1:2] .=  Λ₁[n][index,index]
         𝚲ₙ[3:4,3:4] .=  Λ₂[n][index,index]
-        𝐦 = ℳ[n]
-        𝐦ʹ= ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ= hom(ℳʹ[n])
         𝐔ₙ = (𝐦 ⊗ 𝐦ʹ)
         ∂ₓ𝐮ₙ =  [(𝐞₁ ⊗ 𝐦ʹ) (𝐞₂ ⊗ 𝐦ʹ) (𝐦 ⊗ 𝐞₁) (𝐦 ⊗ 𝐞₂)]
-        𝐁ₙ =  ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
+        𝐁ₙ = ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
         𝚺ₙ = 𝛉' * 𝐁ₙ * 𝛉
         𝚺ₙ⁻¹ = inv(𝚺ₙ)
-        𝐓₁ = fill(0.0,(l,l))
-        𝐓₂ = fill(0.0,(l,l))
-        𝐓₃ = fill(0.0,(l,l))
-        𝐓₄ = fill(0.0,(l,l))
-        𝐓₅ = fill(0.0,(l,l))
+        𝐓₁ = @SMatrix zeros(Float64,l,l)
+        𝐓₂ = @SMatrix zeros(Float64,l,l)
+        𝐓₃ = @SMatrix zeros(Float64,l,l)
+        𝐓₄ = @SMatrix zeros(Float64,l,l)
+        𝐓₅ = @SMatrix zeros(Float64,l,l)
+        # The additional parentheses around some of the terms are needed as
+        # a workaround to a bug where Base.afoldl allocates memory unnecessarily.
+        # https://github.com/JuliaArrays/StaticArrays.jl/issues/537
         for k = 1:l
-            𝐞ₖ = fill(0.0,(l,1))
-            𝐞ₖ[k] = 1
+            𝐞ₖ = 𝐈ₗ[:,k]
             ∂𝐞ₖ𝚺ₙ = (𝐈ₘ ⊗ 𝐞ₖ') * 𝐁ₙ * (𝐈ₘ ⊗ 𝛉) + (𝐈ₘ ⊗ 𝛉') * 𝐁ₙ * (𝐈ₘ ⊗ 𝐞ₖ)
-            𝐓₁ = 𝐓₁ + 𝐔ₙ * 𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 * 𝐞ₖ'
+            𝐓₁ = 𝐓₁ + (((𝐔ₙ * 𝚺ₙ⁻¹) * (∂𝐞ₖ𝚺ₙ)) * 𝚺ₙ⁻¹) * 𝐔ₙ' * 𝛉 * 𝐞ₖ'
             𝐓₂ = 𝐓₂ + (𝐞ₖ' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
             𝐓₄ = 𝐓₄ + (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
             𝐓₅ = 𝐓₅ + (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
         end
         𝐓₃ =  (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝐈ₘ ⊗ 𝛉) * 𝚺ₙ⁻¹ * 𝐔ₙ'
         𝐓 = 𝐓 + 𝐓₁ + 𝐓₂ + 𝐓₃ - 𝐓₄ - 𝐓₅
-
     end
     𝐓
 end
 
-
 @inline function Iₘ(entity::FundamentalMatrix)
-     Matrix{Float64}(I, 1, 1)
+     SMatrix{1,1}(1.0I)
 end

src/data_normalization/hartley_transformation.jl

@@ -48,26 +48,25 @@ function hartley_transformation(ℳ::Vector{T})::SMatrix where T <:AbstractArray
         throw(ArgumentError("Array cannot be empty."))
     end
     npts = length(ℳ)
-    ndim = length(ℳ[1])-1
+    ndim = length(ℳ[1])
     𝐜 = centroid(ℳ)
     σ = root_mean_square(ℳ, 𝐜)
-    σ⁻¹ = 1 ./ σ
-    𝐓 = SMatrix{ndim+1,ndim+1,Float64, (ndim+1)^2}([σ⁻¹*Matrix{Float64}(I,ndim,ndim) -σ⁻¹*𝐜[1:end-1] ; zeros(1,ndim) 1.0])
+    σ⁻¹ = 1 / σ
+    𝐓 = SMatrix{ndim+1,ndim+1,Float64, (ndim+1)^2}([σ⁻¹*Matrix{Float64}(I,ndim,ndim) -σ⁻¹*𝐜 ; zeros(1,ndim) 1.0])
 end
 
 function centroid(positions::Vector{T}) where T <: AbstractArray
     x = zeros(T)
     for pos ∈ positions
-        x .= (+).(x, pos)
+        x = x + pos
     end
-    x .= (/).(x,length(positions))
-    return x
+    return x / length(positions)
 end
 
 function root_mean_square(ℳ::Vector{T}, 𝐜::T ) where  T <: AbstractArray
     total = 0.0
     npts = length(ℳ)
-    ndim = length(ℳ[1])-1
+    ndim = length(ℳ[1])
     for 𝐦 ∈ ℳ
          total  = total + ∑((𝐦-𝐜).^2)
     end
@@ -126,7 +125,7 @@ end
 function hartley_normalization!(ℳ::Vector{<:AbstractArray})
     𝐓 = hartley_transformation(ℳ)
     map!(ℳ , ℳ) do 𝐦
-         𝐓 * 𝐦
+         hom⁻¹(𝐓 * hom(𝐦))
     end
      ℳ, 𝐓
 end

src/estimate/estimate_twoview.jl

@@ -10,7 +10,7 @@ function estimate(entity::FundamentalMatrix, method::DirectLinearTransform, 𝒟
     λ, f = smallest_eigenpair(Symmetric(𝐀))
     𝐅 = reshape(f,(3,3))
     𝐅 = enforce_ranktwo!(Array(𝐅))
-    𝐅 = 𝐅 / norm(𝐅)
+    𝐅 = SMatrix{3,3,Float64,9}(𝐅 / norm(𝐅))
     # Transform estimate back to the original (unnormalised) coordinate system.
     𝐓ʹ'*𝐅*𝐓
 end
@@ -58,6 +58,7 @@ function estimate(entity::FundamentalMatrix, method::FundamentalNumericalScheme,
     end
     𝐅 = reshape(𝛉,(3,3))
     𝐅 = enforce_ranktwo!(Array(𝐅))
+    𝐅 = SMatrix{3,3,Float64,9}(𝐅)
     # Transform estimate back to the original (unnormalised) coordinate system.
     𝐅 = 𝐓ʹ'*𝐅*𝐓
 end
@@ -68,7 +69,7 @@ function estimate(entity::FundamentalMatrix, method::BundleAdjustment,  𝒟::Tu
     if (N != length(ℳʹ))
           throw(ArgumentError("There should be an equal number of points for each view."))
     end
-    𝐅 = reshape(method.𝛉₀,(3,3))
+    𝐅 = SMatrix{3,3,Float64,9}(reshape(method.𝛉₀,(3,3)))
     𝒳 = triangulate(DirectLinearTransform(),𝐅,(ℳ,ℳʹ))
 
     𝐏₁, 𝐏₂ = construct(ProjectionMatrix(),𝐅)

src/moments/moments_fundamentalmatrix.jl

@@ -7,8 +7,8 @@ function moments(entity::FundamentalMatrix, 𝒟::Tuple{AbstractArray, Vararg{Ab
     end
     𝐀 =  @SMatrix zeros(9,9)
     for n = 1:N
-        𝐦  = 𝑛(ℳ[n])
-        𝐦ʹ = 𝑛(ℳʹ[n])
+        𝐦  = hom(ℳ[n])
+        𝐦ʹ = hom(ℳʹ[n])
         𝐀 = 𝐀 + (𝐦*𝐦') ⊗ (𝐦ʹ*𝐦ʹ')
     end
     𝐀/N

src/operators/operators.jl

@@ -45,7 +45,11 @@ function 𝑛(v::SVector)
 end
 
 function hom⁻¹(v::SVector)
-    pop(v / v[end])
+    if isapprox(v[end], 0.0; atol = 1e-14)
+        pop(v)
+    else
+        pop(v / v[end])
+    end
 end
 
 function hom(v::SVector)

src/projection/project.jl

@@ -5,6 +5,6 @@ function project(e::Pinhole, 𝐏::AbstractArray, 𝒳::Vector{<:AbstractArray})
         throw(ArgumentError("Expect 3 x 4 projection matrix."))
     end
     ℳ = map(𝒳) do 𝐗
-        𝐦 = 𝑛(Point2DH(𝐏 * 𝐗))
+        𝐦 = hom⁻¹(𝐏 * hom(𝐗))
     end
 end

src/triangulation/triangulate.jl

@@ -2,14 +2,19 @@ function triangulate(method::DirectLinearTransform, 𝐅::AbstractArray, 𝒟::T
     ℳ, ℳʹ =  𝒟
     𝐏₁, 𝐏₂ = construct(ProjectionMatrix(),𝐅)
     N = length(ℳ)
-    𝒴 = Array{Point3DH}(undef,N)
+    𝒴 = Array{Point3D}(undef,N)
     for n = 1:N
         𝐦 = ℳ[n]
         𝐦ʹ = ℳʹ[n]
-        𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
-              (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
-              (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
-              (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
+        eq1 = 𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:]
+        eq2 = 𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:]
+        eq3 = 𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:]
+        eq4 = 𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:]
+        𝐀 = SMatrix{4,4}(transpose(hcat(eq1,eq2,eq3,eq4)))
+        # 𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
+        #       (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
+        #       (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
+        #       (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
         # 𝐀₁ = vec2antisym(𝐦)*𝐏₁
         # 𝐀₂ = vec2antisym(𝐦ʹ)*𝐏₂
         # @show typeof(𝐀₁)
@@ -20,24 +25,29 @@ function triangulate(method::DirectLinearTransform, 𝐅::AbstractArray, 𝒟::T
         # λ, f = smallest_eigenpair(Symmetric(Array(𝐀'*𝐀)))
         # @show λ
         # 𝒴[n] = Point3DH(𝑛(f))
-
-        U,S,V = svd(Array(𝐀))
-        𝒴[n] = Point3DH(𝑛(V[:,4]))
+        U,S,V = svd(𝐀)
+        𝒴[n] = hom⁻¹(V[:,4])
     end
     𝒴
 end
 
 function triangulate(method::DirectLinearTransform, 𝐏₁::AbstractArray, 𝐏₂::AbstractArray, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
     ℳ, ℳʹ =  𝒟
     N = length(ℳ)
-    𝒴 = Array{Point3DH}(undef,N)
+    𝒴 = Array{Point3D}(undef,N)
     for n = 1:N
         𝐦 = ℳ[n]
         𝐦ʹ = ℳʹ[n]
-        𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
-              (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
-              (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
-              (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
+        eq1 = 𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:]
+        eq2 = 𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:]
+        eq3 = 𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:]
+        eq4 = 𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:]
+        𝐀 = SMatrix{4,4}(transpose(hcat(eq1,eq2,eq3,eq4)))
+        # 𝐀 = [  (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
+        #         (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
+        #         (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
+        #         (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
+
         # 𝐀₁ = vec2antisym(𝐦)*𝐏₁
         # 𝐀₂ = vec2antisym(𝐦ʹ)*𝐏₂
         # @show typeof(𝐀₁)
@@ -48,9 +58,8 @@ function triangulate(method::DirectLinearTransform, 𝐏₁::AbstractArray, 𝐏
         # λ, f = smallest_eigenpair(Symmetric(Array(𝐀'*𝐀)))
         # @show λ
         # 𝒴[n] = Point3DH(𝑛(f))
-
-        U,S,V = svd(Array(𝐀))
-        𝒴[n] = Point3DH(𝑛(V[:,4]))
+        U,S,V = svd(𝐀)
+        𝒴[n] = hom⁻¹(V[:,4])
     end
     𝒴
 end

src/types/ModuleTypes.jl

@@ -9,7 +9,7 @@ export BundleAdjustment
 export CostFunction, ApproximateMaximumLikelihood, AML
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices
-export Point2DH, Point3DH
+export Point2D, Point2DH, Point3D, Point3DH
 export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme
 export NoiseModel, GaussianNoise
 include("types.jl")