Refactors code so that tests pass

demo/explore_fundamental_matrix.jl

@@ -0,0 +1,123 @@
+using Makie
+using MultipleViewGeometry, Test, Random
+using MultipleViewGeometry.ModuleCostFunction
+using MultipleViewGeometry.ModuleTypes
+using MultipleViewGeometry.ModuleConstraints
+using MultipleViewGeometry.ModuleConstruct
+using MultipleViewGeometry.ModuleDraw
+using MultipleViewGeometry.ModuleMove
+using LinearAlgebra
+using StaticArrays
+using GeometryTypes
+using Test
+
+𝒳 = [Point3D(x,y,rand(50:100)) for x = -100:5:100 for y = -100:5:100]
+𝒳 = 𝒳[1:50:end]
+X = reshape(reinterpret(Float64,𝒳),(3,length(𝒳)))
+
+# Specify the coordinate systems of the world, the camera frame and the picture
+# plane.
+world_basis = (Vec(1.0, 0.0, 0.0), Vec(0.0, 1.0, 0.0), Vec(0.0, 0.0, 1.0))
+camera_basis = (Point(0.0, 0.0, 0.0), Vec(-1.0, 0.0, 0.0), Vec(0.0, -1.0, 0.0), Vec(0.0, 0.0, 1.0))
+picture_basis = (Point(0.0, 0.0), Vec(-1.0, 0.0), Vec(0.0, -1.0))
+
+# The focal length for both cameras is one.
+f = 1
+image_width = 640 / 10
+image_height = 480 / 10
+
+camera₁ = Pinhole(image_width, image_height, f, camera_basis..., picture_basis...)
+camera₂ = Pinhole(image_width, image_height, f, camera_basis..., picture_basis...)
+
+# Rotate and translate camera one.
+𝐑₁ = Matrix{Float64}(I,3,3)
+𝐭₁ = [-50.0, -2.0, 0.0]
+relocate!(camera₁, 𝐑₁, 𝐭₁)
+
+# Rotate and translate camera two.
+𝐑₂ = Matrix{Float64}(I,3,3)
+𝐭₂ = [50.0, 2.0, 0.0]
+relocate!(camera₂, 𝐑₂, 𝐭₂)
+
+
+scale = 20.0f0
+x = Vec3f0(0); baselen = 0.2f0 * scale ; dirlen = 1f0 * scale
+# create an array of differently colored boxes in the direction of the 3 axes
+rectangles = [
+    (HyperRectangle(Vec3f0(x), Vec3f0(dirlen, baselen, baselen)), RGBAf0(1,0,0,1)),
+    (HyperRectangle(Vec3f0(x), Vec3f0(baselen, dirlen, baselen)), RGBAf0(0,1,0,1)),
+    (HyperRectangle(Vec3f0(x), Vec3f0(baselen, baselen, dirlen)), RGBAf0(0,0,1,1))
+]
+meshes = map(GLNormalMesh, rectangles)
+
+
+scene = mesh(merge(meshes))
+scatter!(scene, X[1,:],X[2,:], X[3,:], markersize = 3, color = :red)
+draw!(camera₁, scene)
+draw!(camera₂, scene)
+
+
+
+𝐑₁′, 𝐭₁′ = ascertain_pose(camera₁, world_basis... )
+𝐊₁′ = obtain_intrinsics(camera₁, CartesianSystem())
+𝐑₂′, 𝐭₂′ = ascertain_pose(camera₂, world_basis... )
+𝐊₂′ = obtain_intrinsics(camera₂, CartesianSystem())
+
+# Camera projection matrices.
+𝐏₁ = construct(ProjectionMatrix(),𝐊₁′,𝐑₁′,𝐭₁′)
+𝐏₂ = construct(ProjectionMatrix(),𝐊₂′,𝐑₂′,𝐭₂′)
+
+
+# Set of corresponding points.
+ℳ = project(camera₁,𝐏₁,𝒳)
+ℳʹ = project(camera₂,𝐏₂,𝒳)
+
+# Estimate of the fundamental matrix and the true fundamental matrix.
+𝐅 = estimate(FundamentalMatrix(), DirectLinearTransform(), (ℳ, ℳʹ))
+
+#𝐅ₜ = construct(FundamentalMatrix(), 𝐏₁, 𝐏₂)
+𝐅ₜ = construct(FundamentalMatrix(), 𝐊₁′,𝐑₁′,-𝐭₁′,𝐊₂′,𝐑₂′,-𝐭₂′)
+
+# Ensure the estimated and true matrix have the same scale and sign.
+𝐅 = 𝐅 / norm(𝐅)
+𝐅 = 𝐅 / sign(𝐅[3,1])
+𝐅ₜ = 𝐅ₜ / norm(𝐅ₜ)
+𝐅ₜ = 𝐅ₜ / sign(𝐅ₜ[3,1])
+
+@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
+    𝐦  = hom(m)
+    𝐦ʹ = hom(mʹ)
+    residual[i] = (𝐦ʹ'*𝐅*𝐦)
+end
+
+@test isapprox(sum(residual), 0.0; atol = 1e-7)
+
+
+# Test the Fundamental Numerical Scheme on the Fundamental matrix problem.
+Λ₁ =  [SMatrix{3,3}(Matrix(Diagonal([1.0,1.0,0.0]))) for i = 1:length(ℳ)]
+Λ₂ =  [SMatrix{3,3}(Matrix(Diagonal([1.0,1.0,0.0]))) for i = 1:length(ℳ)]
+𝐅₀ = estimate(FundamentalMatrix(),DirectLinearTransform(),  (ℳ, ℳʹ))
+𝐅 = estimate(FundamentalMatrix(),
+              FundamentalNumericalScheme(vec(𝐅₀), 5, 1e-10),
+               (Λ₁,Λ₂), (ℳ, ℳʹ))
+
+
+# Ensure the estimated and true matrix have the same scale and sign.
+𝐅 = 𝐅 / norm(𝐅)
+𝐅 = 𝐅 / sign(𝐅[3,1])
+
+@test 𝐅 ≈ 𝐅ₜ
+
+# Test the Bundle Adjustment estimator on the Fundamental matrix problem.
+𝐅, lsqFit = estimate(FundamentalMatrix(),
+                      BundleAdjustment(vec(𝐅₀), 5, 1e-10),
+                        (ℳ, ℳʹ))
+𝐅 = 𝐅 / norm(𝐅)
+𝐅 = 𝐅 / sign(𝐅[3,1])
+@test 𝐅 ≈ 𝐅ₜ

src/MultipleViewGeometry.jl

@@ -61,6 +61,9 @@ export rotate!, translate!, relocate!
 # Functions exported from `cost_functions.jl`
 export cost, X, covariance_matrix, covariance_matrix_debug
 
+# Functions exported from "synthetic_data.jl"
+export generate_planar_points
+
 # Functions exported from `draw.jl`
 #export draw!, EpipolarLineGraphic, LineSegment3D, PlaneSegment3D, Camera3D
 #export WorldCoordinateSystem3D
@@ -80,6 +83,7 @@ include("math_aliases/ModuleMathAliases.jl")
 include("types/ModuleTypes.jl")
 include("operators/ModuleOperators.jl")
 #include("rotation/ModuleRotation.jl")
+include("synthetic_data/ModuleSyntheticData.jl")
 include("camera/ModuleCamera.jl")
 include("move/ModuleMove.jl")
 include("data_normalization/ModuleDataNormalization.jl")
@@ -99,6 +103,7 @@ using .ModuleMathAliases
 using .ModuleTypes
 using .ModuleOperators
 #using .ModuleRotation
+using .ModuleSyntheticData
 using .ModuleCamera
 using .ModuleMove
 using .ModuleDataNormalization

src/construct/construct_projectionmatrix.jl

@@ -10,7 +10,6 @@ function construct( e::ProjectionMatrix,
         throw(ArgumentError("Expect length-3 translation vectors."))
     end
     # TODO: Reconcile this change in convention with the rest of the code.
-    #𝐏 = 𝐊*[𝐑 -𝐑*𝐭]
     𝐏 = 𝐊*[𝐑' -𝐑'*𝐭]
     SMatrix{3,4,Float64,3*4}(𝐏)
 end

src/estimate/homography_matrix.jl

@@ -0,0 +1,15 @@
+function estimate(entity::HomographyMatrix, method::DirectLinearTransform, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
+    ℳ, ℳʹ =  𝒟
+    N = length(ℳ)
+    if (N != length(ℳʹ))
+          throw(ArgumentError("There should be an equal number of points for each view."))
+    end
+    𝒪, 𝐓 = transform(HomogeneousCoordinates(),CanonicalToHartley(),ℳ)
+    𝒪ʹ, 𝐓ʹ  = transform(HomogeneousCoordinates(),CanonicalToHartley(),ℳʹ)
+    𝐀 = moments(HomographyMatrix(), (𝒪, 𝒪ʹ))
+    λ, h = smallest_eigenpair(Symmetric(𝐀))
+    𝐇 = reshape(h,(3,3))
+    𝐇 = SMatrix{3,3,Float64,9}(𝐇 / norm(𝐇))
+    # Transform estimate back to the original (unnormalised) coordinate system.
+    inv(𝐓ʹ)*𝐇*𝐓
+end

src/synthetic_data/ModuleSyntheticData.jl

@@ -0,0 +1,6 @@
+module ModuleSyntheticData
+using MultipleViewGeometry.ModuleOperators, MultipleViewGeometry.ModuleMathAliases
+using LinearAlgebra, StaticArrays
+export generate_planar_points
+include("synthetic_data.jl")
+end

src/synthetic_data/synthetic_data.jl

@@ -0,0 +1,19 @@
+# Generates a random point on the plane centered around a point on the plane
+# that is closest to the origin.
+function generate_planar_points(𝐧::AbstractArray, d::Real, extent::Real, N::Int)
+    # Generate vector 𝐰 on a plane through the origin with normal vector 𝐧.
+    first(𝐧) == 0 ? 𝐰 = cross(𝐧,SVector(1.0,0.0,0.0)) : 𝐰 = cross(𝐧,SVector(0.0,0.0,1.0))
+    points = Array{SVector{3,Float64},1}(undef,N)
+    for n = 1:N
+        # Rotate 𝐰 randomly around the axis 𝐧.
+        θ = rand() * 2*pi
+        𝐤 = 𝐧 / norm(𝐧)
+        𝐯 = 𝐰*cos(θ) + cross(𝐤,𝐰)*sin(θ) + 𝐤*dot(𝐤,𝐰)*(1-cos(θ))
+        # Scale the vector so that it lies in the interval [0, extent)
+        𝐯 = (rand() * extent) * 𝐯
+        # Translate the vector so that it lies on the plane parametrised by 𝐧 and d.
+        𝐯 = 𝐯 + d*(𝐧/norm(𝐧)^2)
+        points[n] = 𝐯
+    end
+    points
+end

test/cost_functions_tests.jl

@@ -3,36 +3,60 @@ using MultipleViewGeometry.ModuleCostFunction
 using MultipleViewGeometry.ModuleTypes
 using LinearAlgebra
 using StaticArrays
+using GeometryTypes
 
 # Test for cost functions.
 
 # Test cost function on Fundamental matrix estimation.
 
-𝒳 = [Point3D(x,y,z) for x=-1:0.5:10 for y=-1:0.5:10 for z=2:-0.1:1]
+𝒳 = [Point3D(x,y,rand(50:100)) for x = -100:5:100 for y = -100:5:100]
+𝒳 = 𝒳[1:50:end]
 
-# Intrinsic and extrinsic parameters of camera one.
-𝐊₁ = SMatrix{3,3}(Matrix{Float64}(I, 3, 3))
-𝐑₁ = SMatrix{3,3}(Matrix{Float64}(I, 3, 3))
-𝐭₁ =  @SVector [0.0, 0.0, -10]
 
-# Intrinsic and extrinsic parameters of camera two.
-𝐊₂ = SMatrix{3,3}(Matrix{Float64}(I, 3, 3))
-𝐑₂ = SMatrix{3,3}(Matrix{Float64}(I, 3, 3)) #SMatrix{3,3,Float64,9}(rotxyz(pi/10,pi/10,pi/10))
-𝐭₂ = @SVector [10.0, 10.0, -10.0]
+# Specify the coordinate systems of the world, the camera frame and the picture
+# plane.
+world_basis = (Vec(1.0, 0.0, 0.0), Vec(0.0, 1.0, 0.0), Vec(0.0, 0.0, 1.0))
+camera_basis = (Point(0.0, 0.0, 0.0), Vec(-1.0, 0.0, 0.0), Vec(0.0, -1.0, 0.0), Vec(0.0, 0.0, 1.0))
+picture_basis = (Point(0.0, 0.0), Vec(-1.0, 0.0), Vec(0.0, -1.0))
+
+# The focal length for both cameras is one.
+f = 1
+image_width = 640 / 10
+image_height = 480 / 10
+
+camera₁ = Pinhole(image_width, image_height, f, camera_basis..., picture_basis...)
+camera₂ = Pinhole(image_width, image_height, f, camera_basis..., picture_basis...)
+
+# Rotate and translate camera one.
+𝐑₁ = Matrix{Float64}(I,3,3)
+𝐭₁ = [-50.0, -2.0, 0.0]
+relocate!(camera₁, 𝐑₁, 𝐭₁)
+
+# Rotate and translate camera two.
+𝐑₂ = Matrix{Float64}(I,3,3)
+𝐭₂ = [50.0, 2.0, 0.0]
+relocate!(camera₂, 𝐑₂, 𝐭₂)
+
+
+𝐑₁′, 𝐭₁′ = ascertain_pose(camera₁, world_basis... )
+𝐊₁′ = obtain_intrinsics(camera₁, CartesianSystem())
+𝐑₂′, 𝐭₂′ = ascertain_pose(camera₂, world_basis... )
+𝐊₂′ = obtain_intrinsics(camera₂, CartesianSystem())
 
 # Camera projection matrices.
-𝐏₁ = construct(ProjectionMatrix(),𝐊₁,𝐑₁,𝐭₁)
-𝐏₂ = construct(ProjectionMatrix(),𝐊₂,𝐑₂,𝐭₂)
+𝐏₁ = construct(ProjectionMatrix(),𝐊₁′,𝐑₁′,𝐭₁′)
+𝐏₂ = construct(ProjectionMatrix(),𝐊₂′,𝐑₂′,𝐭₂′)
 
 # Set of corresponding points.
-ℳ = project(Pinhole(),𝐏₁,𝒳)
-ℳʹ = project(Pinhole(),𝐏₂,𝒳)
+ℳ = project(camera₁,𝐏₁,𝒳)
+ℳʹ = project(camera₂,𝐏₂,𝒳)
 
-𝐅 = construct(FundamentalMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂)
+# Construct fundamental matrix from projection matrices.
+𝐅 = construct(FundamentalMatrix(), 𝐏₁, 𝐏₂)
 
 # Ensure the estimated and true matrix have the same scale and sign.
 𝐅 = 𝐅 / norm(𝐅)
-𝐅 = 𝐅 / sign(𝐅[1,3])
+𝐅 = 𝐅 / sign(𝐅[3,1])
 𝐟 = vec(𝐅)
 
 Λ₁ =  [SMatrix{3,3}(Matrix(Diagonal([1.0,1.0,0.0]))) for i = 1:length(ℳ)]
@@ -45,11 +69,11 @@ Jₐₘₗ =  cost(AML(),FundamentalMatrix(), SVector{9}(𝐟), (Λ₁,Λ₂), (
 𝐗 = X(AML(),FundamentalMatrix(), vec(𝐅), (Λ₁,Λ₂), (ℳ, ℳʹ))
 
 # The true parameters should lie in the null space of the matrix X.
-@test all(isapprox.(𝐗 * 𝐟, 0.0; atol = 1e-10))
+@test all(isapprox.(𝐗 * 𝐟, 0.0; atol = 1e-9))
 
 # Verify that the the vectorised fundamental matrix is in the null space of H.
 # H represents the Hessian matrix of the AML cost function.
 𝐇 = H(AML(),FundamentalMatrix(), vec(𝐅), (Λ₁,Λ₂), (ℳ, ℳʹ))
 
 # The true parameters should lie in the null space of the matrix H.
-@test all(isapprox.(𝐇 * 𝐟, 0.0; atol = 1e-10))
+@test all(isapprox.(𝐇 * 𝐟, 0.0; atol = 1e-9))