Merge pull request #4 from zygmuntszpak/covariance_matrix

Adds measure of uncertainty for AML-based  fundamental matrix estimate

src/MultipleViewGeometry.jl

@@ -2,6 +2,7 @@ __precompile__()
 module MultipleViewGeometry
 
 using Compat
+using StaticArrays
 
 
 
@@ -13,15 +14,17 @@ export CostFunction, ApproximateMaximumLikelihood, AML
 export HomogeneousCoordinates
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices
+export Point2DH, Point3DH
+export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme
 
 # Aliases exported from math_aliases.jl
 export ⊗, ∑, √
 
 # Functions exported from `operators.jl`.
-export 𝑛, smallest_eigenpair,vec2antisym
+export 𝑛, ∂𝑛, smallest_eigenpair,vec2antisym
 
 # Functions exported from `hartley_transformation.jl`.
-export hartley_normalization, hartley_transformation
+export hartley_normalization, hartley_normalization!, hartley_transformation
 
 # Functions exported from `transform.jl`.
 export transform
@@ -45,7 +48,7 @@ export project
 export rotx, roty, rotz, rotxyz, rodrigues2matrix
 
 # Functions exported from `cost_functions.jl`
-export cost, X
+export cost, X, covariance_matrix, covariance_matrix_debug
 
 include("math_aliases/ModuleMathAliases.jl")
 include("types/ModuleTypes.jl")

src/carriers/ModuleCarriers.jl

@@ -1,5 +1,6 @@
 module ModuleCarriers
 using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleMathAliases, MultipleViewGeometry.ModuleOperators
+using StaticArrays
 export ∂ₓu, uₓ
 include("carriers.jl")
 end

src/carriers/carriers.jl

@@ -1,25 +1,29 @@
 # Carrier vector for fundamental matrix estimation.
-function ∂ₓu(entity::FundamentalMatrix, 𝒟)
-    m, mʹ = 𝒟
-    𝐦  = 𝑛(collect(Float64,m.coords))
-    𝐦ʹ = 𝑛(collect(Float64,mʹ.coords))
+@inline function ∂ₓu(entity::FundamentalMatrix, 𝒟)
+    m, mʹ = collect(𝒟)
+    # 𝐦  = 𝑛(collect(Float64,m.coords))
+    # 𝐦ʹ = 𝑛(collect(Float64,mʹ.coords))
+    𝐦  = 𝑛(m)
+    𝐦ʹ = 𝑛(mʹ)
     ∂ₓu(entity, 𝐦 , 𝐦ʹ)
 end
 
-function ∂ₓu(entity::FundamentalMatrix, 𝐦::Vector{Float64}, 𝐦ʹ::Vector{Float64})
+@inline function ∂ₓu(entity::FundamentalMatrix, 𝐦::AbstractVector, 𝐦ʹ::AbstractVector)
     𝐞₁ = [1.0 0.0 0.0]'
     𝐞₂ = [0.0 1.0 0.0]'
     [(𝐞₁ ⊗ 𝐦ʹ) (𝐞₂ ⊗ 𝐦ʹ) (𝐦 ⊗ 𝐞₁) (𝐦 ⊗ 𝐞₂)]
 end
 
 
-function uₓ(entity::FundamentalMatrix, 𝒟)
-    m, mʹ = 𝒟
-    𝐦  = 𝑛(collect(Float64,m.coords))
-    𝐦ʹ = 𝑛(collect(Float64,mʹ.coords))
+@inline function uₓ(entity::FundamentalMatrix, 𝒟)
+    m, mʹ = collect(𝒟)
+    # 𝐦  = 𝑛(collect(Float64,m.coords))
+    # 𝐦ʹ = 𝑛(collect(Float64,mʹ.coords))
+    𝐦  = 𝑛(m)
+    𝐦ʹ = 𝑛(mʹ)
     uₓ(entity, 𝐦 , 𝐦ʹ)
 end
 
-function uₓ(entity::FundamentalMatrix, 𝐦::Vector{Float64}, 𝐦ʹ::Vector{Float64})
+@inline function uₓ(entity::FundamentalMatrix, 𝐦::AbstractVector, 𝐦ʹ::AbstractVector)
     𝐦 ⊗ 𝐦ʹ
 end

src/construct/ModuleConstruct.jl

@@ -1,7 +1,9 @@
 module ModuleConstruct
 using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleMathAliases, MultipleViewGeometry.ModuleOperators
 using MultipleViewGeometry.ModuleProjection
+using StaticArrays
 export construct
+
 include("construct_projectionmatrix.jl")
 include("construct_fundamentalmatrix.jl")
 end

src/construct/construct_fundamentalmatrix.jl

@@ -1,10 +1,10 @@
 function construct( e::FundamentalMatrix,
-                   𝐊₁::AbstractArray{T,2},
-                   𝐑₁::AbstractArray{T,2},
-                    𝐭₁::AbstractArray{T,1},
-                   𝐊₂::AbstractArray{T,2},
-                   𝐑₂::AbstractArray{T,2},
-                     𝐭₂::AbstractArray{T,1} ) where T<:Real
+                   𝐊₁::AbstractArray,
+                   𝐑₁::AbstractArray,
+                   𝐭₁::AbstractArray,
+                   𝐊₂::AbstractArray,
+                   𝐑₂::AbstractArray,
+                   𝐭₂::AbstractArray)
 
     if size(𝐊₁) != (3,3) || size(𝐊₂) != (3,3) ||
        size(𝐑₁) != (3,3) || size(𝐑₂) != (3,3)
@@ -14,16 +14,15 @@ function construct( e::FundamentalMatrix,
         throw(ArgumentError("Expect length-3 translation vectors."))
     end
     𝐅 = vec2antisym(𝐊₂*𝐑₂*(𝐭₁ .- 𝐭₂))*𝐊₂*𝐑₂/𝐑₁/𝐊₁
+    MMatrix{3,3,Float64,3*3}(𝐅)
 end
 
-function construct( e::FundamentalMatrix,
-                   𝐏₁::AbstractArray{T,2},
-                   𝐏₂::AbstractArray{T,2}) where T<:Real
-
+function construct( e::FundamentalMatrix, 𝐏₁::AbstractArray, 𝐏₂::AbstractArray)
     if (size(𝐏₁) != (3,4)) || (size(𝐏₂) != (3,4))
         throw(ArgumentError("Expect 3 x 4 projection matrices."))
     end
-    𝐜₁ = nullspace(𝐏₁)
+    𝐜₁ = SVector{4,Float64}(nullspace(Array(𝐏₁)))
     𝐞₂ = 𝐏₂*𝐜₁
-    return 𝐅 = vec2antisym(𝐞₂)*𝐏₂*pinv(𝐏₁)
+    𝐅 = vec2antisym(𝐞₂)*𝐏₂*pinv(Array(𝐏₁))
+    MMatrix{3,3,Float64,3*3}(𝐅)
 end

src/construct/construct_projectionmatrix.jl

@@ -1,7 +1,7 @@
 function construct( e::ProjectionMatrix,
-                   𝐊::AbstractArray{T,2},
-                   𝐑::AbstractArray{T,2},
-                    𝐭::AbstractArray{T,1} ) where T<:Real
+                   𝐊::AbstractArray,
+                   𝐑::AbstractArray,
+                    𝐭::AbstractArray )
 
     if size(𝐊) != (3,3) || size(𝐑) != (3,3)
         throw(ArgumentError("Expect 3 x 3 calibration and rotation matrices."))
@@ -10,9 +10,14 @@ function construct( e::ProjectionMatrix,
         throw(ArgumentError("Expect length-3 translation vectors."))
     end
     𝐏 = 𝐊*[𝐑 -𝐑*𝐭]
+    SMatrix{3,4,Float64,3*4}(𝐏)
 end
 
-function construct( e::ProjectionMatrix, 𝐅::AbstractArray{T,2}) where T<:Real
-    𝐞 = collect(epipole(𝐅').coords)
-    eye(3,4), [vec2antisym(𝐞) * 𝐅  𝐞]
+function construct( e::ProjectionMatrix, 𝐅::AbstractArray)
+    𝐞 = epipole(𝐅')
+    𝐏₁ = eye(3,4)
+    𝐏₂ = [vec2antisym(𝐞) * 𝐅  𝐞]
+
+    SMatrix{3,4,Float64,3*4}(𝐏₁), SMatrix{3,4,Float64,3*4}(𝐏₂)
+
 end

src/cost_function/ModuleCostFunction.jl

@@ -1,6 +1,8 @@
 module ModuleCostFunction
 using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleOperators, MultipleViewGeometry.ModuleMathAliases
 using MultipleViewGeometry.ModuleCarriers
-export cost, X, H
+using MultipleViewGeometry.ModuleTransform
+using StaticArrays
+export cost, X, H, covariance_matrix, covariance_matrix_debug, covariance_matrix_normalised
 include("cost_functions.jl")
 end