Adds two-view unrectified triangulation capability

Adds two-view triangulation capability. Triangulation is based on direct linear transform (an algebraic method). One can triangulate by giving a pair of projection (camera) matrices. Alternatively, one can supply the fundamental matrix which will result in a projective reconstruction. If calibration matrices are known, then one can triangulate using the Essential Matrix. This will result in a Euclidean reconstruction up to an unknown scale factor. 

I've implemented Kanatani's optimal correction so that a set of noisy corresponding points satisfy the epipolar constraint. However, the triangulation method currently does not use this correction.

demo/example_unrectified_triangulate.jl

@@ -0,0 +1,106 @@
+using MultipleViewGeometry, Base.Test
+using MultipleViewGeometry.ModuleCostFunction
+using MultipleViewGeometry.ModuleTypes
+using MultipleViewGeometry.ModuleConstraints
+using MultipleViewGeometry.ModuleConstruct
+using MultipleViewGeometry.ModuleDraw
+using MultipleViewGeometry.ModuleTriangulation
+using BenchmarkTools, Compat
+using StaticArrays
+using  MAT, Plots
+
+# Load MATLAB matrices that represent a pair of images and that contain
+# a set of manually matched corresponding points.
+file = matopen("./data/teapot.mat")
+X = read(file,"pts3D")
+# X is a 4 x N matrix, where N is the number of points expressed in homogeneous
+# coordinates.
+X = X[:,1:10:end]
+close(file)
+
+# Fix random seed.
+srand(1234)
+plotlyjs()
+
+# Transform the point cloud so that it will be oriented upright and
+# in front of two cameras which we will define next.
+T1 = eye(4)
+T1[:,4] = -mean(X,2)
+T2 = [1 0 0 0;
+      0 1 0 0;
+      0 0 1 -2500;
+      0 0 0 1]
+R = eye(4)
+R[1:3,1:3] = rotx(deg2rad(90))
+X = T2*R*T1*X;
+
+# Convert the 4 x N matrix into a list of 3D points expressed in homogeneous coordinates.
+𝒳 = [Point3DH(X[1,i],X[2,i],X[3,i],1.0) for i = 1:size(X,2)]
+
+# Intrinsic and extrinsic parameters of camera one.
+𝐊₁ = eye(3)
+𝐑₁ =  eye(3)
+𝐭₁ = [0.0, 0.0, 0.0]
+𝐜₁ = [0.0, 0.0, 0.0]
+𝐄₁ = [𝐑₁ 𝐭₁]
+
+# Intrinsic and extrinsic parameters of camera two.
+𝐊₂ = eye(3)
+𝐑₂ = eye(3)
+𝐭₂ = [450.0,   0.0, 0.0]
+𝐜₂ = [0.0, 0.0, 0.0]
+𝐄₂ = [𝐑₂ 𝐭₂]
+
+# Camera projection matrices.
+𝐏₁ = construct(ProjectionMatrix(),𝐊₁,𝐑₁,𝐭₁)
+𝐏₂ = construct(ProjectionMatrix(),𝐊₂,𝐑₂,𝐭₂)
+
+# Set of corresponding points.
+ℳ = project(Pinhole(),𝐏₁,𝒳)
+ℳʹ = project(Pinhole(),𝐏₂,𝒳)
+
+# Visualise the data points
+p1 = Plots.plot(X[1,:],X[2,:],X[3,:],seriestype = :scatter, ms=1,grid = false, box = :none, legend = false)
+draw!(WorldCoordinateSystem3D(), 1, p1)
+draw!(Camera3D(), 𝐊₁, 𝐑₁, 𝐭₁, 1, p1)
+draw!(Camera3D(), 𝐊₂, 𝐑₂, 𝐭₂, 1, p1)
+
+
+# Plot the projections of the point cloud in the image pair.
+p2 =  Plots.plot();
+for n = 1:length(ℳ)
+    m = ℳ[n]
+    Plots.plot!([m[1]],[m[2]], grid = false, box = :none, legend = false,
+                    seriestype = :scatter, ms = 2, markercolor=:Black,
+                    aspect_ratio = :equal, yflip = true)
+end
+
+p3 =  Plots.plot();
+for n = 1:length(ℳʹ)
+    mʹ = ℳʹ[n]
+    Plots.plot!([mʹ[1]],[mʹ[2]], grid = false, box = :none, legend = false,
+                    seriestype = :scatter, ms = 2, markercolor=:Black,
+                    aspect_ratio = :equal, yflip = true)
+end
+
+# Visualise the 3D point cloud, as well as the projected images.
+l = @layout [ a; [b c] ]
+p4 = Plots.plot(p1,p2, p3, layout = l)
+
+# Estimate of the fundamental matrix and the true fundamental matrix.
+𝐅 = estimate(FundamentalMatrix(), DirectLinearTransform(), (ℳ, ℳʹ))
+𝐄 = construct(EssentialMatrix(), 𝐅,  𝐊₁, 𝐊₂)
+
+𝐏₁, 𝐏₂ = construct(ProjectionMatrix(), 𝐄, (ℳ, ℳʹ))
+
+𝒴 = triangulate(DirectLinearTransform(),𝐏₁,𝐏₂,(ℳ,ℳʹ))
+Y = reinterpret(Float64,map(SVector{4,Float64},𝒴),(4,length(𝒴)))
+
+# The point cloud can be reconsructed only up to an unknown scale factor.
+p5 = Plots.plot(Y[1,:],Y[2,:],Y[3,:],seriestype = :scatter, ms=1,grid = false, box = :none, legend = false)
+draw!(WorldCoordinateSystem3D(), 0.002, p5)
+draw!(Camera3D(), 𝐊₁, 𝐏₁[1:3,1:3], 𝐏₁[:,4], 0.002, p5)
+draw!(Camera3D(), 𝐊₂, 𝐏₂[1:3,1:3], 𝐏₂[:,4], 0.002, p5)
+
+display(p4)
+#display(p5)

src/MultipleViewGeometry.jl

@@ -8,6 +8,7 @@ using StaticArrays
 
 # Types exported from `types.jl`
 export HomogeneousPoint, ProjectiveEntity, FundamentalMatrix, ProjectionMatrix
+export EssentialMatrix
 export CameraModel, Pinhole, CanonicalLens
 export EstimationAlgorithm, DirectLinearTransform, Taubin, FundamentalNumericalScheme
 export CostFunction, ApproximateMaximumLikelihood, AML
@@ -36,6 +37,9 @@ export moments
 # Functions exported from `estimate_twoview.jl`
 export estimate
 
+# Functions exported from `construct_essentialmatrix.jl`
+export construct
+
 # Functions exported from `construct_fundamentalmatrix.jl`
 export construct
 

src/carriers/carriers.jl

@@ -1,8 +1,6 @@
 # Carrier vector for fundamental matrix estimation.
 @inline function ∂ₓu(entity::FundamentalMatrix, 𝒟)
     m, mʹ = collect(𝒟)
-    # 𝐦  = 𝑛(collect(Float64,m.coords))
-    # 𝐦ʹ = 𝑛(collect(Float64,mʹ.coords))
     𝐦  = 𝑛(m)
     𝐦ʹ = 𝑛(mʹ)
     ∂ₓu(entity, 𝐦 , 𝐦ʹ)
@@ -17,8 +15,6 @@ end
 
 @inline function uₓ(entity::FundamentalMatrix, 𝒟)
     m, mʹ = collect(𝒟)
-    # 𝐦  = 𝑛(collect(Float64,m.coords))
-    # 𝐦ʹ = 𝑛(collect(Float64,mʹ.coords))
     𝐦  = 𝑛(m)
     𝐦ʹ = 𝑛(mʹ)
     uₓ(entity, 𝐦 , 𝐦ʹ)

src/construct/ModuleConstruct.jl

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

src/construct/construct_essentialmatrix.jl

@@ -0,0 +1,11 @@
+function construct( e::EssentialMatrix, 𝐅::AbstractArray,  𝐊₁::AbstractArray, 𝐊₂::AbstractArray)
+    if (size(𝐊₁) != (3,3)) || (size(𝐊₂) != (3,3))
+        throw(ArgumentError("Expect 3 x 3 calibration matrices."))
+    end
+    if (size(𝐅) != (3,3))
+        throw(ArgumentError("Expect 3 x 3 fundamental matrix."))
+    end
+    # Equation 9.12 Chapter 9 from Hartley & Zisserman
+    𝐄 = 𝐊₂'*𝐅*𝐊₁
+    MMatrix{3,3,Float64,3*3}(𝐄)
+end

src/construct/construct_projectionmatrix.jl

@@ -21,3 +21,54 @@ function construct( e::ProjectionMatrix, 𝐅::AbstractArray)
     SMatrix{3,4,Float64,3*4}(𝐏₁), SMatrix{3,4,Float64,3*4}(𝐏₂)
 
 end
+
+function construct( e::ProjectionMatrix, 𝐄::AbstractArray, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
+    𝐖 = SMatrix{3,3,Float64,3*3}([0 -1 0; 1 0 0; 0 0 1])
+    𝐙 = SMatrix{3,3,Float64,3*3}([0 1 0; -1 0 0; 0 0 0])
+    𝐔,𝐒,𝐕 = svd(𝐄)
+    𝐭 = 𝐔[:,3]
+    𝐏₁ = SMatrix{3,4,Float64,3*4}(eye(3,4))
+    𝐏₂₁ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖*𝐕'  𝐭])
+    𝐏₂₂ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖'*𝐕' 𝐭])
+    𝐏₂₃ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖*𝐕' -𝐭])
+    𝐏₂₄ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖'*𝐕' -𝐭])
+
+    𝒳₁ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₁, 𝒟)
+    𝒳₂ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₂, 𝒟)
+    𝒳₃ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₃, 𝒟)
+    𝒳₄ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₄, 𝒟)
+
+    # Determine which projection matrix in the second view triangulated
+    # the majority of points in front of the cameras.
+    ℳ₁ = map(𝒳₁) do 𝐗
+        𝐦 = 𝐏₂₁ * 𝐗
+        𝐦[3] > 0
+    end
+
+    ℳ₂ = map(𝒳₂) do 𝐗
+        𝐦 = 𝐏₂₂ * 𝐗
+        𝐦[3] > 0
+    end
+
+    ℳ₃ = map(𝒳₃) do 𝐗
+        𝐦 = 𝐏₂₃ * 𝐗
+        𝐦[3] > 0
+    end
+
+    ℳ₄ = map(𝒳₄) do 𝐗
+        𝐦 = 𝐏₂₄ * 𝐗
+        𝐦[3] > 0
+    end
+
+    total, index = findmax((sum(ℳ₁), sum(ℳ₂), sum(ℳ₃), sum(ℳ₄)))
+
+    if index == 1
+        return 𝐏₁,  𝐏₂₁
+    elseif index == 2
+        return 𝐏₁,  𝐏₂₂
+    elseif index == 3
+        return 𝐏₁,  𝐏₂₃
+    else
+        return 𝐏₁,  𝐏₂₄
+    end
+end

src/draw/draw.jl

@@ -84,10 +84,11 @@ function draw!(g::WorldCoordinateSystem3D, scale,  p::RecipesBase.AbstractPlot{<
     𝐞₃ = [0,  0,   1]
     𝐨  = [0,  0,  0]
 
+    s = scale*300
     # Draw the world coordinate axes.
-    draw!(LineSegment3D(), 𝐨, 𝐨 + scale*𝐞₁, :red, p)
-    draw!(LineSegment3D(), 𝐨, 𝐨 + scale*𝐞₂, :green, p)
-    draw!(LineSegment3D(), 𝐨, 𝐨 + scale*𝐞₃, :blue, p)
+    draw!(LineSegment3D(), 𝐨, 𝐨 + s*𝐞₁, :red, p)
+    draw!(LineSegment3D(), 𝐨, 𝐨 + s*𝐞₂, :green, p)
+    draw!(LineSegment3D(), 𝐨, 𝐨 + s*𝐞₃, :blue, p)
 end
 
 function draw!(g::Camera3D, 𝐊::AbstractArray,  𝐑::AbstractArray, 𝐭::AbstractArray, scale,  p::RecipesBase.AbstractPlot{<:RecipesBase.AbstractBackend})
@@ -99,10 +100,10 @@ function draw!(g::Camera3D, 𝐊::AbstractArray,  𝐑::AbstractArray, 𝐭::Abs
     𝐨  = [0,  0,  0]
 
     # Initial camera imaging plane.
-    𝐩₁ =  [-125,  125,  -50]
-    𝐩₂ =  [125,  125,  -50]
-    𝐩₃ =  [125, -125, -50]
-    𝐩₄ =  [-125,  -125, -50]
+    𝐩₁ =  scale*[-125,  125,  50]
+    𝐩₂ =  scale*[125,  125,  50]
+    𝐩₃ =  scale*[125, -125, 50]
+    𝐩₄ =  scale*[-125,  -125, 50]
 
     # Initial camera center.
     𝐜 = [0.0, 0.0, 0.0]
@@ -117,10 +118,11 @@ function draw!(g::Camera3D, 𝐊::AbstractArray,  𝐑::AbstractArray, 𝐭::Abs
     draw!(LineSegment3D(), 𝐜, 𝐑*𝐩₃ + 𝐭, :black, p)
     draw!(LineSegment3D(), 𝐜, 𝐑*𝐩₄ + 𝐭, :black, p)
 
-    # Draw camera coordinate axes for the first camera.
-    draw!(LineSegment3D(), 𝐜, (𝐑*scale*𝐞₁ + 𝐭), :red, p)
-    draw!(LineSegment3D(), 𝐜, (𝐑*scale*𝐞₂ + 𝐭), :green, p)
-    draw!(LineSegment3D(), 𝐜, (𝐑*scale*𝐞₃ + 𝐭), :blue, p)
+    s = scale*200
+    # Draw camera coordinate axes for the camera.
+    draw!(LineSegment3D(), 𝐜, (𝐑*s*𝐞₁ + 𝐭), :red, p)
+    draw!(LineSegment3D(), 𝐜, (𝐑*s*𝐞₂ + 𝐭), :green, p)
+    draw!(LineSegment3D(), 𝐜, (𝐑*s*𝐞₃ + 𝐭), :blue, p)
 
 
 end

src/types/ModuleTypes.jl

@@ -2,14 +2,14 @@ module ModuleTypes
 using StaticArrays
 
 export HomogeneousPoint, ProjectiveEntity, FundamentalMatrix, ProjectionMatrix
-export HomogeneousCoordinates
+export HomogeneousCoordinates, EssentialMatrix
 export CameraModel, Pinhole, CanonicalLens
-export EstimationAlgorithm, DirectLinearTransform, Taubin,FundamentalNumericalScheme
+export EstimationAlgorithm, DirectLinearTransform, Taubin, FundamentalNumericalScheme
 export CostFunction, ApproximateMaximumLikelihood, AML
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices
 export Point2DH, Point3DH
 export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme
-export NoiseModel,GaussianNoise
+export NoiseModel, GaussianNoise
 include("types.jl")
 end

src/types/types.jl

@@ -53,6 +53,9 @@ abstract type NoiseModel end
 type FundamentalMatrix <: ProjectiveEntity
 end
 
+type EssentialMatrix <: ProjectiveEntity
+end
+
 type ProjectionMatrix <: ProjectiveEntity
 end
 

test/construct_essentialmatrix_tests.jl

@@ -0,0 +1,28 @@
+using MultipleViewGeometry, MultipleViewGeometry.ModuleRotation, Base.Test
+
+# Intrinsic and extrinsic parameters for the first camera.
+𝐊₁ = zeros(3,3)
+𝐊₁[1,1] = 10
+𝐊₁[2,2] = 10
+𝐊₁[3,3] = 1
+𝐑₁ = rotxyz(deg2rad(10), deg2rad(15), deg2rad(45))
+𝐭₁ = [-250.0, 0.0, 2500.0]
+
+# Intrinsic and extrinsic parameters for the second camera.
+𝐊₂ = zeros(3,3)
+𝐊₂[1,1] = 5
+𝐊₂[2,2] = 5
+𝐊₂[3,3] = 1
+𝐑₂ =  rotxyz(deg2rad(10), deg2rad(15), deg2rad(45))
+𝐭₂ = [250.0,   0.0, 2500.0]
+
+𝐅 = construct(FundamentalMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂)
+𝐄 = construct(EssentialMatrix(),𝐅, 𝐊₁, 𝐊₂)
+
+# Result 9.17 of R. Hartley and A. Zisserman, “Two-View Geometry,” Multiple View Geometry in Computer Vision
+# A 3 by 3 matrix is an essential matrix if and only if two of its singular values
+# are equal, and the third is zero.
+U, S , V = svd(𝐄)
+
+@test isapprox.(S[1], S[2]; atol = 1e-14)
+@test isapprox.(S[3], 0.0; atol = 1e-10)

test/runtests.jl

@@ -6,6 +6,7 @@ using Base.Test
 @testset "Data Normalization Tests" begin include("data_normalization_tests.jl") end
 @testset "Transform Tests" begin include("transform_tests.jl") end
 @testset "Fundamental Matrix Construction Tests" begin include("construct_fundamentalmatrix_tests.jl") end
+@testset "Essential Matrix Construction Tests" begin include("construct_essentialmatrix_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