Adds basic fundamental matrix estimation infrastructure

Adds the requisite functions to estimate the Fundamental matrix using the direct linear transform and to validate the correctness of the estimate. 

Various requisite support functions are also included in this commit.

src/MultipleViewGeometry.jl

@@ -8,10 +8,11 @@ include("types.jl")
 include("math_aliases.jl")
 
 # Types exported from `types.jl`
-export HomogeneousPoint, ProjectiveEntity, FundamentalMatrix
+export HomogeneousPoint, ProjectiveEntity, FundamentalMatrix, ProjectionMatrix
+export CameraModel, Pinhole, CanonicalLens
 
 # Functions exported from `operators.jl`.
-export 𝑛, smallest_eigenpair
+export 𝑛, smallest_eigenpair,vec2antisym
 
 # Functions exported from `hartley_transformation.jl`.
 export hartley_normalization, hartley_transformation
@@ -22,10 +23,26 @@ export moments
 # Functions exported from `estimate_twoview.jl`
 export estimate
 
+# Functions exported from `construct_fundamentalmatrix.jl`
+export construct
+
+# Functions exported from `construct_projectionmatrix.jl`
+export construct
+
+# Functions exported from `project.jl`
+export project
+
+# Functions exported from `rotations.jl`
+export rotx, roty, rotz, rotxyz, rodrigues2matrix
+
 include("operators.jl")
+include("rotation/rotations.jl")
 include("data_normalization/hartley_transformation.jl")
+include("camera/construct_projectionmatrix.jl")
+include("projection/project.jl")
 include("twoview_estimation/moments_fundamentalmatrix.jl")
 include("twoview_estimation/estimate_twoview.jl")
+include("twoview_estimation/construct_fundamentalmatrix.jl")
 
 # package code goes here
 

src/camera/construct_projectionmatrix.jl

@@ -0,0 +1,13 @@
+function construct( e::ProjectionMatrix,
+                   𝐊::AbstractArray{T,2},
+                   𝐑::AbstractArray{T,2},
+                    𝐭::AbstractArray{T,1} ) where T<:Real
+
+    if size(𝐊) != (3,3) || size(𝐑) != (3,3)
+        throw(ArgumentError("Expect 3 x 3 calibration and rotation matrices."))
+    end
+    if length(𝐭) != 3
+        throw(ArgumentError("Expect length-3 translation vectors."))
+    end
+    𝐏 = 𝐊*[𝐑 -𝐑*𝐭]
+end

src/operators.jl

@@ -36,7 +36,16 @@ function 𝑛(v::Vector{T}) where T<:Real
     end
 end
 
-function smallest_eigenpair(A::AbstractArray{T,2} where T <:Real)
+function smallest_eigenpair(A::AbstractArray{T,2}) where T <:Real
     F = eigfact(A)::Base.LinAlg.Eigen{Float64,Float64,Array{Float64,2},Array{Float64,1}};
-    (F[:values][end], F[:vectors][:,end])
+    (F[:values][1], F[:vectors][:,1])
+end
+
+function vec2antisym(v::AbstractVector{T})  where T<:Real
+    if length(v) != 3
+         throw(ArgumentError("The operation is only defined for a length-3 vector."))
+    end
+    𝐒::Matrix{T} = [  0  -v[3]    v[2] ;
+                    v[3]    0    -v[1] ;
+                   -v[2]  v[1]      0]
 end

src/projection/project.jl

@@ -0,0 +1,11 @@
+function project(e::Pinhole, 𝐏::AbstractArray{T1,2}, 𝒳::AbstractArray{T2}) where {T1<:Real,T2<:HomogeneousPoint}
+
+    if size(𝐏) != (3,4)
+        throw(ArgumentError("Expect 3 x 4 projection matrix."))
+    end
+    ℳ = map(𝒳) do X
+        𝐗 = collect(X.coords)
+        𝐦 = 𝑛(𝐏 * 𝐗)
+        HomogeneousPoint(tuple(𝐦...))
+    end
+end

src/rotation/rotations.jl

@@ -0,0 +1,57 @@
+"""julia
+      rotx(θ::Real)
+
+A matrix representing the rotation about the X-axis by an angle θ.
+"""
+function rotx(θ::Real)
+cosθ = cos(θ);
+sinθ = sin(θ);
+𝐑ˣ = [1.0  0.0      0.0  ;
+      0.   cosθ    -sinθ ;
+      0    sinθ     cosθ ]
+end
+
+"""juliaa
+      roty(θ::Real)
+
+A matrix representing rotation about the Y-axis by an angle θ.
+"""
+function roty(θ::Real)
+cosθ = cos(θ);
+sinθ = sin(θ);
+𝐑ʸ = [cosθ     0.0    sinθ ;
+      0.0      1.0    0.0  ;
+      -sinθ    0.0    cosθ]
+end
+
+"""julia
+      rotz(θ::Real)
+
+A matrix representing the rotation about the Z-axis by an angle θ.
+"""
+function rotz(θ::Real)
+cosθ = cos(θ);
+sinθ = sin(θ);
+𝐑ᶻ = [cosθ     -sinθ    0.0 ;
+      sinθ      cosθ    0.0 ;
+      0.0       0.0     1.0 ]
+end
+
+"""julia
+      rotz(θ::Real)
+
+A matrix representing the rotation about the X-axis, Y-axis and Z-axis by the angles θˣ, θʸ, and θᶻ respectively.
+"""
+function rotxyz(θˣ::Real,θʸ::Real,θᶻ::Real)
+      rotx(θˣ)*roty(θʸ)*rotz(θᶻ)
+end
+
+
+function rodrigues2matrix(vˣ::Real,vʸ::Real,vᶻ::Real)
+      𝐯 = [vˣ, vʸ, vᶻ]
+      θ = norm(𝐯)
+      𝐯 = θ == 0 ? 𝐯 : 𝐯/θ
+      𝐈 = eye(3)
+      𝐖 = vec2antisym(𝐯)
+      𝐑 = 𝐈 + 𝐖 * sin(θ) + 𝐖^2 * (1-cos(θ))
+end

src/twoview_estimation/construct_fundamentalmatrix.jl

@@ -0,0 +1,17 @@
+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
+
+    if size(𝐊₁) != (3,3) || size(𝐊₂) != (3,3) ||
+       size(𝐑₁) != (3,3) || size(𝐑₂) != (3,3)
+        throw(ArgumentError("Expect 3 x 3 calibration and rotation matrices."))
+    end
+    if length(𝐭₁) != 3 || length(𝐭₂) != 3
+        throw(ArgumentError("Expect length-3 translation vectors."))
+    end
+    𝐅 = vec2antisym(𝐊₂*𝐑₂*(𝐭₁ .- 𝐭₂))*𝐊₂*𝐑₂/𝐑₁/𝐊₁
+end

src/twoview_estimation/estimate_twoview.jl

@@ -5,12 +5,14 @@ function estimate(entity::FundamentalMatrix, matches...)
           throw(ArgumentError("There should be an equal number of points for each view."))
     end
     (ℳ,𝐓) = hartley_normalization(ℳ)
-    (ℳʹ,𝐓ʹ) = hartley_normalization!(ℳʹ)
+    (ℳʹ,𝐓ʹ) = hartley_normalization(ℳʹ)
     𝐀::Matrix{Float64} = moments(FundamentalMatrix(), ℳ, ℳʹ)
     (λ::Float64, f::Vector{Float64}) = smallest_eigenpair(𝐀)
     𝐅::Matrix{Float64} = reshape(f,(3,3))
     # Enforce the rank-2 constraint.
     U,S,V = svd(𝐅)
     S[end] = 0.0
     𝐅 = U*diagm(S)*V'
+    # Transform estimate back to the original (unnormalised) coordinate system.
+    𝐅 = 𝐓ʹ'*𝐅*𝐓
 end

src/types.jl

@@ -4,5 +4,16 @@ end
 
 abstract type ProjectiveEntity end
 
+abstract type CameraModel end
+
 type FundamentalMatrix <: ProjectiveEntity
 end
+
+type ProjectionMatrix <: ProjectiveEntity
+end
+
+type Pinhole <: CameraModel
+end
+
+type CanonicalLens <: CameraModel
+end

test/construct_fundamentalmatrix_tests.jl

@@ -0,0 +1,10 @@
+using MultipleViewGeometry, Base.Test
+
+𝐊₁ = eye(3)
+𝐑₁ = eye(3)
+𝐭₁ = [1.0, 1.0, 1.0]
+𝐊₂ = eye(3)
+𝐑₂ = eye(3)
+𝐭₂ = [2.0, 2.0, 2.0]
+
+@test construct(FundamentalMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂) == vec2antisym([-1,-1,-1])

test/construct_projectionmatrix_tests.jl

@@ -0,0 +1,7 @@
+using MultipleViewGeometry, Base.Test
+
+𝐊 = eye(3)
+𝐑 = eye(3)
+𝐭 = [1.0, 1.0, 1.0]
+
+@test construct(ProjectionMatrix(),𝐊,𝐑,𝐭) == [eye(3) -ones(3)]

test/estimate_twoview_tests.jl

@@ -1,14 +1,49 @@
 using MultipleViewGeometry, Base.Test
 
 # Tests for fundamental matrix estimation
-ℳ = map(HomogeneousPoint, [(-10.0, -10.0, 1.0),
-                               (-10.0,  10.0, 1.0),
-                               ( 10.0,-10.0, 1.0),
-                               ( 10.0,  10.0, 1.0)])
 
-ℳʹ = map(HomogeneousPoint, [(10.0, -10.0, 1.0),
-                               (10.0,  10.0, 1.0),
-                               ( 20.0,-10.0, 1.0),
-                               ( 20.0,  10.0, 1.0)])
+# A rectangular array of 3D points represented in homogeneous coordinates
+𝒳 = [HomogeneousPoint(Float64.((x,y,z,1.0),RoundDown))
+                        for x=-100:10:100 for y=-100:10:100 for z=1:-100:-1000]
 
-F = estimate(FundamentalMatrix(), ℳ, ℳʹ)
+# Intrinsic and extrinsic parameters of camera one.
+𝐊₁ = eye(3)
+𝐑₁ = eye(3)
+𝐭₁ = [0.0, 0.0, 0.0]
+
+# Intrinsic and extrinsic parameters of camera two.
+𝐊₂ = eye(3)
+𝐑₂ = eye(3)
+𝐭₂ = [100.0, 2.0, -100.0]
+
+# Camera projection matrices.
+𝐏₁ = construct(ProjectionMatrix(),𝐊₁,𝐑₁,𝐭₁)
+𝐏₂ = construct(ProjectionMatrix(),𝐊₂,𝐑₂,𝐭₂)
+
+# Set of corresponding points.
+ℳ = project(Pinhole(),𝐏₁,𝒳)
+ℳʹ = project(Pinhole(),𝐏₂,𝒳)
+
+# Estimate of the fundamental matrix and the true fundamental matrix.
+𝐅 = estimate(FundamentalMatrix(), ℳ, ℳʹ)
+𝐅ₜ = construct(FundamentalMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂)
+
+# Ensure the estimated and true matrix have the same scale and sign.
+𝐅 = 𝐅 / norm(𝐅)
+𝐅 = 𝐅 / sign(𝐅[1,2])
+𝐅ₜ = 𝐅ₜ / norm(𝐅ₜ)
+𝐅ₜ = 𝐅ₜ / sign(𝐅ₜ[1,2])
+
+@test 𝐅 ≈ 𝐅ₜ
+
+# Check that the fundamental matrix satisfies the corresponding point equation.
+npts = length(ℳ)
+residual = zeros(Float64,npts,1)
+for correspondence in zip(1:length(ℳ),ℳ, ℳʹ)
+    i, m , mʹ = correspondence
+    𝐦  = 𝑛(collect(Float64,m.coords))
+    𝐦ʹ = 𝑛(collect(Float64,mʹ.coords))
+    residual[i] = 𝐦ʹ'*𝐅*𝐦
+end
+
+@test isapprox(sum(residual), 0.0; atol = 1e-9)

test/moments_fundamentalmatrix_tests.jl

@@ -13,3 +13,5 @@ pts2 = map(HomogeneousPoint,
            ( 20.0,  20.0, 1.0)])
 
 moments(FundamentalMatrix(), (pts1,pts2)...)
+
+# TODO

test/operators_tests.jl

@@ -6,3 +6,8 @@ v = [2,2,2]
 # Vectors which represent points at infinity are unchanged.
 v = [2,2,0]
 @test 𝑛(v) == [2,2,0]
+
+v = [1, 2, 3]
+@test vec2antisym(v) == [ 0 -3  2;
+                          3  0 -1;
+                         -2  1  0]

test/rotations_tests.jl

@@ -0,0 +1,15 @@
+using MultipleViewGeometry, Base.Test
+
+@test rotx(0.0) == eye(3)
+@test rotx(2.0*pi) ≈  eye(3)
+
+@test roty(0.0) == eye(3)
+@test roty(2.0*pi) ≈  eye(3)
+
+@test rotz(0.0) == eye(3)
+@test rotz(2.0*pi) ≈  eye(3)
+
+@test rotxyz(0.0, 0.0, 0.0) == eye(3)
+@test rotxyz(2.0*pi, 2.0*pi, 2.0*pi) ≈  eye(3)
+
+@test rodrigues2matrix(0.0, 0.0, 0.0) == eye(3)

test/runtests.jl

@@ -4,3 +4,7 @@ using Base.Test
 
 @testset "Operators Tests" begin include("operators_tests.jl") end
 @testset "Data Normalization Tests" begin include("data_normalization_tests.jl") end
+@testset "Fundamental Matrix Construction Tests" begin include("construct_fundamentalmatrix_tests.jl") end
+@testset "Estimate Two View Tests" begin include("estimate_twoview_tests.jl") end
+@testset "Projection Matrix Construction Tests" begin include("construct_projectionmatrix_tests.jl") end
+@testset "Rotations Tests" begin include("rotations_tests.jl") end