WIP Triangulate

demo/example_fundamental_matrix.jl

@@ -11,7 +11,7 @@ inlier_pts1 = read(file,"inlierPts1")
 inlier_pts2 = read(file,"inlierPts2")
 close(file)
 
-# Conver the images to Grayscale. 
+# Conver the images to Grayscale.
 img1g = Gray.(img1)
 img2g = Gray.(img2)
 
@@ -63,3 +63,5 @@ Plots.plot!([eʹ[1]],[eʹ[2]], grid = false, box = :none, legend = false,
 # Display both views simultaneously.
 p3 = Plots.plot(p1,p2,layout=(1,2), legend = false)
 display(p3)
+
+𝐅₀*e

demo/example_unrectified_triangulate.jl

@@ -0,0 +1,111 @@
+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 = X[:,1:10:end]
+close(file)
+
+# Fix random seed.
+srand(1234)
+plotlyjs()
+
+#𝒳 = [Point3DH(x,y,z,1.0) for x=-1:0.5:10 for y=-1:0.5:10 for z=2:-0.1:1]
+
+𝒳 = [Point3DH(X[1,i],X[2,i],X[3,i],1.0) for i = 1:size(X,2)]
+
+#X = reinterpret(Float64,map(SVector{4,Float64},𝒳),(4,length(𝒳)))
+#Z = reinterpret(SVector{4,Float64},(3000,4))
+# reinterpret(SVector{4,Float64}, X, (size(X,2),))#
+# reinterpret(SVector{4,Float64}, Z, (size(Z,2),))#
+# reinterpret(SVector{4,Float64}, X, (3000,1))
+
+# Intrinsic and extrinsic parameters of camera one.
+𝐊₁ = eye(3)
+𝐑₁ = eye(3)
+𝐭₁ = [-250.0, 0.0, 2500.0]
+𝐜₁ = [0.0, 0.0, 0.0]
+𝐄₁ = [𝐑₁ 𝐭₁]
+
+# Intrinsic and extrinsic parameters of camera two.
+𝐊₂ = eye(3)
+𝐑₂ =  eye(3)
+𝐭₂ = [250.0,   0.0, 2500.0]
+𝐜₂ = [0.0, 0.0, 0.0]
+𝐄₂ = [𝐑₂ 𝐭₂]
+
+# Camera projection matrices.
+𝐏₁ = construct(ProjectionMatrix(),𝐊₁,𝐑₁,𝐭₁)
+𝐏₂ = construct(ProjectionMatrix(),𝐊₂,𝐑₂,𝐭₂)
+
+# Set of corresponding points.
+ℳ = project(Pinhole(),𝐏₁,𝒳)
+ℳʹ = project(Pinhole(),𝐏₂,𝒳)
+
+# Convert the array of Point3DH into a 4 x N matrix to simplify
+# the plotting of the data points.
+#X = reinterpret(Float64,map(SVector{4,Float64},𝒳),(4,length(𝒳)))
+
+# 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(), 450, p1)
+draw!(Camera3D(), 𝐊₁, 𝐑₁, 𝐭₁, 250, p1)
+draw!(Camera3D(), 𝐊₂, 𝐑₂, 𝐭₂, 250, 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)
+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)
+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)
+
+
+# 𝒴 = triangulate(DirectLinearTransform(),𝐏₁,𝐏₂,(ℳ,ℳʹ))
+#
+# Y = reinterpret(Float64,map(SVector{4,Float64},𝒴),(4,length(𝒴)))
+# # Visualise the data points
+# p5 = Plots.plot(Y[1,:],Y[2,:],Y[3,:],seriestype = :scatter, ms=1,grid = false, box = :none, legend = false)
+# draw!(WorldCoordinateSystem3D(), 450, p1)
+# draw!(Camera3D(), 𝐊₁, 𝐑₁, 𝐭₁, 250, p1)
+# draw!(Camera3D(), 𝐊₂, 𝐑₂, 𝐭₂, 250, p1)
+
+# 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(𝒴)))
+# Visualise the data points
+p5 = Plots.plot(Y[1,:],Y[2,:],Y[3,:],seriestype = :scatter, ms=1,grid = false, box = :none, legend = false)
+draw!(WorldCoordinateSystem3D(), 450/1000, p1)
+# draw!(Camera3D(), 𝐊₁, 𝐏₁[1:3,1:3], 𝐏₁[:,4], 250/1000, p1)
+# draw!(Camera3D(), 𝐊₂, 𝐏₂[1:3,1:3], 𝐏₂[:,4], 250/1000, p1)
+# draw!(Camera3D(), 𝐊₁, 𝐑₁, 𝐭₁, 250, p1)
+# draw!(Camera3D(), 𝐊₂, 𝐑₂, 𝐭₂, 250, p1)

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
@@ -16,6 +17,7 @@ export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices
 export Point2DH, Point3DH
 export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme
+export NoiseModel, GaussianNoise
 
 # Aliases exported from math_aliases.jl
 export ⊗, ∑, √
@@ -35,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
 
@@ -51,7 +56,17 @@ export rotx, roty, rotz, rotxyz, rodrigues2matrix
 export cost, X, covariance_matrix, covariance_matrix_debug
 
 # Functions exported from `draw.jl`
-export draw!, EpipolarLineGraphic
+export draw!, EpipolarLineGraphic, LineSegment3D, PlaneSegment3D, Camera3D
+export WorldCoordinateSystem3D
+
+# Functions exported from `constraints.jl`
+export satisfy, EpipolarConstraint, Constraint
+
+# Functions exported from `triangulation.jl`
+export triangulate
+
+# Functions exported from `noise.jl`
+export perturb
 
 include("math_aliases/ModuleMathAliases.jl")
 include("types/ModuleTypes.jl")
@@ -66,7 +81,9 @@ include("cost_function/ModuleCostFunction.jl")
 include("estimate/ModuleEstimation.jl")
 include("construct/ModuleConstruct.jl")
 include("draw/ModuleDraw.jl")
-
+include("constraints/ModuleConstraints.jl")
+include("triangulation/ModuleTriangulation.jl")
+include("noise/ModuleNoise.jl")
 
 using .ModuleMathAliases
 using .ModuleTypes
@@ -81,6 +98,9 @@ using .ModuleMoments
 using .ModuleCostFunction
 using .ModuleConstruct
 using .ModuleDraw
+using .ModuleConstraints
+using .ModuleTriangulation
+using .ModuleNoise
 
 
 # package code goes here

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/constraints/ModuleConstraints.jl

@@ -0,0 +1,7 @@
+module ModuleConstraints
+using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleMathAliases, MultipleViewGeometry.ModuleOperators
+using StaticArrays
+export satisfy, EpipolarConstraint, Constraint
+
+include("constraints.jl")
+end

src/constraints/constraints.jl

@@ -0,0 +1,47 @@
+abstract type Constraint end
+
+type EpipolarConstraint <: Constraint
+end
+
+# Triangulation from Two Views Revisited: Hartley-Sturm vs. Optimal Correction
+function satisfy(entity::FundamentalMatrix, constraint::EpipolarConstraint, 𝐅::AbstractArray, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
+    ℳ, ℳʹ = 𝒟
+    𝒪 = similar(ℳ)
+    𝒪ʹ = similar(ℳʹ)
+
+    N = length(ℳ)
+    𝐏₂ = SMatrix{3,3,Float64,3^2}([1 0 0; 0 1 0; 0 0 0])
+
+    I = 10
+    for n = 1:N
+        𝐦 = ℳ[n]
+        𝐦ʹ = ℳʹ[n]
+        𝐦ₕ = init_correction_view_1(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
+        𝐦ₕʹ= init_correction_view_2(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
+        for i = 1:I
+            𝐦ₜ =  𝐦 - 𝐦ₕ
+            𝐦ₜʹ =  𝐦ʹ - 𝐦ₕʹ
+            𝐦ₕ = update_correction_view_1(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
+            𝐦ₕʹ = update_correction_view_2(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
+        end
+        𝒪[n] = 𝐦ₕ
+        𝒪ʹ[n] = 𝐦ₕʹ
+    end
+    𝒪 ,𝒪ʹ
+end
+
+function init_correction_view_1(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ʹ::AbstractVector, 𝐏₂::AbstractArray)
+    𝐦 -  dot(𝐦,𝐅*𝐦ʹ)*𝐏₂*𝐅*𝐦ʹ / ( dot(𝐅*𝐦ʹ,𝐏₂*𝐅*𝐦ʹ) + dot(𝐅'*𝐦, 𝐏₂*𝐅'*𝐦) )
+end
+
+function init_correction_view_2(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ʹ::AbstractVector, 𝐏₂::AbstractArray)
+    𝐦ʹ -  dot(𝐦,𝐅*𝐦ʹ)*𝐏₂*𝐅'*𝐦 / ( dot(𝐅*𝐦ʹ,𝐏₂*𝐅*𝐦ʹ) + dot(𝐅'*𝐦, 𝐏₂*𝐅'*𝐦) )
+end
+
+function update_correction_view_1(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ₕ::AbstractVector, 𝐦ₜ::AbstractVector,  𝐦ʹ::AbstractVector, 𝐦ₕʹ::AbstractVector, 𝐦ₜʹ::AbstractVector,  𝐏₂::AbstractArray)
+    𝐦 -  ( ( dot(𝐦ₕ,𝐅*𝐦ₕʹ) + dot(𝐅*𝐦ₕʹ, 𝐦ₜ) + dot(𝐅'*𝐦ₕ, 𝐦ₜʹ)  ) * 𝐏₂*𝐅*𝐦ₕʹ)   / (dot(𝐅*𝐦ₕʹ, 𝐏₂*𝐅*𝐦ₕʹ) + dot(𝐅'*𝐦ₕ, 𝐏₂*𝐅'*𝐦ₕ) )
+end
+
+function update_correction_view_2(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ₕ::AbstractVector, 𝐦ₜ::AbstractVector,  𝐦ʹ::AbstractVector, 𝐦ₕʹ::AbstractVector, 𝐦ₜʹ::AbstractVector,  𝐏₂::AbstractArray)
+    𝐦ʹ -  ( ( dot(𝐦ₕ,𝐅*𝐦ₕʹ) + dot(𝐅*𝐦ₕʹ, 𝐦ₜ) + dot(𝐅'*𝐦ₕ, 𝐦ₜʹ)  ) * 𝐏₂*𝐅'*𝐦ₕ)   / (dot(𝐅*𝐦ₕʹ, 𝐏₂*𝐅*𝐦ₕʹ) + dot(𝐅'*𝐦ₕ, 𝐏₂*𝐅'*𝐦ₕ) )
+end

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/cost_function/cost_functions.jl

@@ -1,8 +1,8 @@
 
 
 function cost(c::CostFunction, entity::FundamentalMatrix, 𝛉::AbstractArray, 𝒞::Tuple{AbstractArray, Vararg{AbstractArray}}, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
-    ℳ, ℳʹ = collect(𝒟)
-    Λ₁, Λ₂ = collect(𝒞)
+    ℳ, ℳʹ = 𝒟
+    Λ₁, Λ₂ = 𝒞
     Jₐₘₗ = 0.0
     N = length(𝒟[1])
     𝚲ₙ = @MMatrix zeros(4,4)
@@ -130,7 +130,7 @@ function covariance_matrix(c::CostFunction, s::CanonicalApproximation, entity::F
     𝚲  = _covariance_matrix(AML(),FundamentalMatrix(), 𝛉₁, (Λ₁,Λ₁ʹ), (𝒪 , 𝒪ʹ))
 
     𝛉₁ =  𝛉₁ / norm(𝛉₁)
-    
+
     # Derivative of the determinant of 𝚯 = reshape(𝛉₁,(3,3)).
     φ₁ = 𝛉₁[5]*𝛉₁[9] - 𝛉₁[8]*𝛉₁[6]
     φ₂ = -(𝛉₁[4]*𝛉₁[5] - 𝛉₁[7]*𝛉₁[6])
@@ -158,8 +158,8 @@ end
 
 function _covariance_matrix(c::CostFunction, entity::FundamentalMatrix, 𝛉::AbstractArray, 𝒞::Tuple{AbstractArray, Vararg{AbstractArray}}, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
     𝛉 = 𝛉 / norm(𝛉)
-    ℳ, ℳʹ = collect(𝒟)
-    Λ₁, Λ₂ = collect(𝒞)
+    ℳ, ℳʹ = 𝒟
+    Λ₁, Λ₂ = 𝒞
     N = length(𝒟[1])
     𝚲ₙ = @MMatrix zeros(4,4)
     𝐞₁ = @SMatrix [1.0; 0.0; 0.0]
@@ -233,8 +233,8 @@ function _X(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray,𝒞:
     𝐍 = fill(0.0,(l,l))
     𝐌 = fill(0.0,(l,l))
     N = length(𝒟[1])
-    ℳ, ℳʹ = collect(𝒟)
-    Λ₁, Λ₂ = collect(𝒞)
+    ℳ, ℳʹ = 𝒟
+    Λ₁, Λ₂ = 𝒞
     𝚲ₙ = @MMatrix zeros(4,4)
     𝐞₁ = @SMatrix [1.0; 0.0; 0.0]
     𝐞₂ = @SMatrix [0.0; 1.0; 0.0]
@@ -291,8 +291,8 @@ function T(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray, 𝒞:
     𝐌 = fill(0.0,(l,l))
     𝐓 = fill(0.0,(l,l))
     N = length(𝒟[1])
-    ℳ, ℳʹ = collect(𝒟)
-    Λ₁, Λ₂ = collect(𝒞)
+    ℳ, ℳʹ = 𝒟
+    Λ₁, Λ₂ = 𝒞
     𝚲ₙ = @MMatrix zeros(4,4)
     𝐞₁ = @SMatrix [1.0; 0.0; 0.0]
     𝐞₂ = @SMatrix [0.0; 1.0; 0.0]