Merge pull request #6 from zygmuntszpak/triangulate_fundamental

 Adds two-view unrectified triangulation capability

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_uncalibrated_triangulate.jl

@@ -0,0 +1,113 @@
+using MultipleViewGeometry, Base.Test
+using MultipleViewGeometry.ModuleCostFunction
+using MultipleViewGeometry.ModuleTypes
+using MultipleViewGeometry.ModuleConstraints
+using MultipleViewGeometry.ModuleConstruct
+using MultipleViewGeometry.ModuleDraw
+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(),𝐏₂,𝒳)
+
+# Estimate of the fundamental matrix and the true fundamental matrix.
+𝐅 = estimate(FundamentalMatrix(), DirectLinearTransform(), (ℳ, ℳʹ))
+
+# 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)
+
+
+# 𝐅 = construct(FundamentalMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂)
+#
+# # To triangulate the corresponding points using the Fundamental matrix, we first
+# # have to factorise the Fundamental matrix into a pair of Camera matrices. Due
+# # to projective ambiguity, the camera matrices are not unique, and so the
+# # triangulated 3D points will most probably not match the original 3D points.
+# # However, when working with noiseless data, the projections of the triangulated
+# # points should satisfy the epipolar constraint. We can use this fact to
+# # validate that the triangulation is correctly implemented.
+# 𝒴 = triangulate(DirectLinearTransform(),𝐅,(ℳ,ℳʹ))
+# Y = reinterpret(Float64,map(SVector{4,Float64},𝒴),(4,length(𝒴)))
+# p6 = Plots.plot(Y[1,:],Y[2,:],Y[3,:],seriestype = :scatter, ms=1,grid = false, box = :none, legend = false)
+#
+# 𝐐₁, 𝐐₂ = construct(ProjectionMatrix(),𝐅)

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
@@ -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