Adds ML fundamental matrix estimation (Bundle Adjustment)

Implemented bundle adjustment for gold-standard fundamental matrix estimation (i.e. reprojection error). Incorporated analytic Jacobian. At the moment the memory for the Jacobian gets allocated at each iteration. This is something that ought to be addressed in the future.

demo/example_fundamental_matrix.jl

@@ -15,7 +15,7 @@ close(file)
 img1g = Gray.(img1)
 img2g = Gray.(img2)
 
-plotlyjs()
+#plotlyjs()
 
 _, npts1 = size(inlier_pts1)
 _, npts2 = size(inlier_pts2)

src/MultipleViewGeometry.jl

@@ -11,6 +11,7 @@ export HomogeneousPoint, ProjectiveEntity, FundamentalMatrix, ProjectionMatrix
 export EssentialMatrix
 export CameraModel, Pinhole, CanonicalLens
 export EstimationAlgorithm, DirectLinearTransform, Taubin, FundamentalNumericalScheme
+export BundleAdjustment
 export CostFunction, ApproximateMaximumLikelihood, AML
 export HomogeneousCoordinates
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
@@ -23,7 +24,7 @@ export NoiseModel, GaussianNoise
 export ⊗, ∑, √
 
 # Functions exported from `operators.jl`.
-export 𝑛, ∂𝑛, smallest_eigenpair,vec2antisym
+export 𝑛, ∂𝑛, smallest_eigenpair,vec2antisym, hom⁻¹, hom, ∂hom⁻¹
 
 # Functions exported from `hartley_transformation.jl`.
 export hartley_normalization, hartley_normalization!, hartley_transformation

src/draw/ModuleDraw.jl

@@ -1,7 +1,7 @@
 module ModuleDraw
 using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleOperators, MultipleViewGeometry.ModuleMathAliases
 using StaticArrays
-using Plots, Plotly
+using Plots #, Plotly
 using Juno
 
 export draw!, EpipolarLineGraphic, LineSegment3D, PlaneSegment3D, Camera3D

src/estimate/ModuleEstimation.jl

@@ -1,9 +1,10 @@
 module ModuleEstimation
 using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleDataNormalization, MultipleViewGeometry.ModuleOperators, MultipleViewGeometry.ModuleMathAliases
 using MultipleViewGeometry.ModuleMoments, MultipleViewGeometry.ModuleCarriers, MultipleViewGeometry.ModuleCostFunction
-using MultipleViewGeometry.ModuleTransform
+using MultipleViewGeometry.ModuleTransform, MultipleViewGeometry
 using StaticArrays
 using Juno
+using LsqFit
 export estimate
 include("estimate_twoview.jl")
 end

src/estimate/estimate_twoview.jl

@@ -62,6 +62,87 @@ function estimate(entity::FundamentalMatrix, method::FundamentalNumericalScheme,
     𝐅 = 𝐓ʹ'*𝐅*𝐓
 end
 
+function estimate(entity::FundamentalMatrix, method::BundleAdjustment,  𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
+    ℳ, ℳʹ = 𝒟
+    N = length(ℳ)
+    if (N != length(ℳʹ))
+          throw(ArgumentError("There should be an equal number of points for each view."))
+    end
+    𝐅 = reshape(method.𝛉₀,(3,3))
+    𝒳 = triangulate(DirectLinearTransform(),𝐅,(ℳ,ℳʹ))
+
+    𝐏₁, 𝐏₂ = construct(ProjectionMatrix(),𝐅)
+
+    # Construct a length-(12+3*N) vector consisting of the projection matrix associated
+    # with the second view (the first twelve dimensions), as well as N three-dimensional points
+    # (the remaining dimensions).
+    𝛉 = pack(FundamentalMatrix(), 𝐏₂, 𝒳)
+
+    index₁ = SVector(1,2)
+    index₂ = SVector(3,4)
+    pts = Matrix{Float64}(4,N)
+    for n = 1:N
+        pts[index₁,n] = ℳ[n][index₁]
+        pts[index₂,n] = ℳʹ[n][index₁]
+    end
+
+    fit = curve_fit(model_fundamental, jacobian_model, 𝐏₁, reinterpret(Float64,pts,(4*N,)), 𝛉; show_trace = false)
+    𝐏₂ = reshape(fit.param[1:12],(3,4))
+    𝐅 = construct(FundamentalMatrix(), 𝐏₁, 𝐏₂)
+    𝐅, fit
+end
+
+function model_fundamental(𝐏₁,𝛉)
+    # Twelve parameters for the projection matrix, and 3 parameters per 3D point.
+    N = Int((length(𝛉) - 12) / 3)
+    index₁ = SVector(1,2)
+    index₂ = SVector(3,4)
+    reprojections = Matrix{Float64}(4,N)
+    𝛉v = @view 𝛉[1:12]
+    𝐏₂ = SMatrix{3,4,Float64,12}(reshape(𝛉v,(3,4)))
+    i = 13
+    for n = 1:N
+        # Extract 3D point and convert to homogeneous coordinates
+        v = @view 𝛉[i:i+2]
+        M = hom(SVector{3,Float64}(𝛉[i:i+2]))
+        reprojections[index₁,n] = hom⁻¹(𝐏₁ * M)
+        reprojections[index₂,n] = hom⁻¹(𝐏₂ * M)
+        i = i + 3
+    end
+    reinterpret(Float64,reprojections,(4*N,))
+end
+
+function jacobian_model(𝐏₁,𝛉)
+    # Twelve parameters for the projection matrix, and 3 parameters per 3D point.
+    N = Int((length(𝛉) - 12) / 3)
+    index₁ = SVector(1,2)
+    index₂ = SVector(3,4)
+    reprojections = Matrix{Float64}(4,N)
+    𝛉v = @view 𝛉[1:12]
+    𝐏₂ = SMatrix{3,4,Float64,12}(reshape(𝛉v,(3,4)))
+    𝐉 = zeros(4, N, 12+3*N)
+    𝐀 = SMatrix{2,3,Float64,6}(1,0,0,1,0,0)
+    𝐈₃ = @SMatrix eye(3)
+    i = 13
+    for n = 1:N
+        # Extract 3D point and convert to homogeneous coordinates
+        𝐌 = hom(SVector{3,Float64}(𝛉[i:i+2]))
+
+        # Derivative of residual in first and second image w.r.t 3D point.
+        ∂𝐫₁_d𝐌 = -𝐀 * ∂hom⁻¹(𝐏₁ * 𝐌) * 𝐏₁
+        ∂𝐫₂_d𝐌 = -𝐀 * ∂hom⁻¹(𝐏₂ * 𝐌) * 𝐏₂
+
+        # Derivative of residual in second image w.r.t projection martix
+        # ∂𝐫₁_d𝐏₁ is the zero vector.
+        ∂𝐫₂_d𝐏₂ = 𝐀 * ∂hom⁻¹(𝐏₂ * 𝐌) * (𝐌' ⊗ 𝐈₃)
+
+        𝐉[index₂,n,1:12] = ∂𝐫₂_d𝐏₂
+        𝐉[index₁,n,i:i+2] = ∂𝐫₁_d𝐌[:,1:3]
+        𝐉[index₂,n,i:i+2] = ∂𝐫₂_d𝐌[:,1:3]
+        i = i + 3
+    end
+    reinterpret(Float64,𝐉,(4*N,12+3*N))
+end
 
 #𝛉 = reshape(𝛉₀,length(𝛉₀),1)
 # function z(entity::FundamentalMatrix, 𝐦::Vector{Float64}, 𝐦ʹ::Vector{Float64})
@@ -94,3 +175,16 @@ function enforce_ranktwo!(𝐅::AbstractArray)
     S[end] = 0.0
     𝐅 = U*diagm(S)*V'
 end
+
+# Construct a parameter vector consisting of a projection matrix and 3D points
+function pack(entity::FundamentalMatrix, 𝐏₂::AbstractArray, 𝒳::AbstractArray, )
+    N = length(𝒳)
+    𝛉 = Vector{Float64}(12+N*3)
+    𝛉[1:12] = Array(𝐏₂[:])
+    i = 13
+    for n = 1:N
+        𝛉[i:i+2] = 𝒳[n][1:3]
+        i = i + 3
+    end
+    𝛉
+end

src/operators/ModuleOperators.jl

@@ -2,5 +2,6 @@ module ModuleOperators
 using StaticArrays
 using MultipleViewGeometry.ModuleMathAliases, MultipleViewGeometry.ModuleTypes
 export 𝑛, ∂𝑛, smallest_eigenpair,vec2antisym
+export hom, hom⁻¹, ∂hom⁻¹
 include("operators.jl")
 end

src/operators/operators.jl

@@ -36,6 +36,28 @@ function 𝑛(v::AbstractArray)
     end
 end
 
+function 𝑛(v::SVector)
+    if v[end] != 0 && v[end] != 1
+        v / v[end]
+    else
+        v
+    end
+end
+
+function hom⁻¹(v::SVector)
+    pop(v / v[end])
+end
+
+function hom(v::SVector)
+    push(v,1)
+end
+
+function ∂hom⁻¹(𝐧::SVector)
+    k = length(𝐧)
+    𝐞ₖ = push(zeros(SVector{k-1}),1.0)
+    𝐈 = @SMatrix eye(k)
+    1/𝐧[k]*𝐈 - 1/𝐧[k]^2 * 𝐧 * 𝐞ₖ'
+end
 
 function ∂𝑛(𝐧::AbstractArray)
     k = length(𝐧)

src/types/ModuleTypes.jl

@@ -5,6 +5,7 @@ export HomogeneousPoint, ProjectiveEntity, FundamentalMatrix, ProjectionMatrix
 export HomogeneousCoordinates, EssentialMatrix
 export CameraModel, Pinhole, CanonicalLens
 export EstimationAlgorithm, DirectLinearTransform, Taubin, FundamentalNumericalScheme
+export BundleAdjustment
 export CostFunction, ApproximateMaximumLikelihood, AML
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices

src/types/types.jl

@@ -71,6 +71,12 @@ end
 type DirectLinearTransform <: EstimationAlgorithm
 end
 
+type BundleAdjustment <: EstimationAlgorithm
+    𝛉₀::Matrix{Float64}
+    max_iter::Int8
+    toleranceθ::Float64
+end
+
 type FundamentalNumericalScheme <: EstimationAlgorithm
     𝛉₀::Matrix{Float64}
     max_iter::Int8

test/estimate_twoview_tests.jl

@@ -1,11 +1,12 @@
 using MultipleViewGeometry, Base.Test
-using StaticArrays
+using MultipleViewGeometry.ModuleTypes
+using StaticArrays, Calculus
 # Tests for fundamental matrix estimation
 
 
 𝒳 = [Point3DH(x,y,z,1.0)
                         for x=-100:5:100 for y=-100:5:100 for z=1:-50:-100]
-
+𝒳 = 𝒳[1:50:end]
 # Intrinsic and extrinsic parameters of camera one.
 𝐊₁ = eye(3)
 𝐑₁ = eye(3)
@@ -43,7 +44,7 @@ for correspondence in zip(1:length(ℳ),ℳ, ℳʹ)
     i, m , mʹ = correspondence
     𝐦  = 𝑛(m)
     𝐦ʹ = 𝑛(mʹ)
-    residual[i] = (𝐦ʹ'*𝐅*𝐦) 
+    residual[i] = (𝐦ʹ'*𝐅*𝐦)
 end
 
 @test isapprox(sum(residual), 0.0; atol = 1e-7)
@@ -79,3 +80,11 @@ end
 # 𝐅ₜ = 𝐅ₜ / sign(𝐅ₜ[1,2])
 #
 # @test 𝐅 ≈ 𝐅ₜ
+
+# Test the Bundle Adjustment estimator on the Fundamental matrix problem.
+𝐅, lsqFit = estimate(FundamentalMatrix(),
+                        BundleAdjustment(reshape(𝐅₀,9,1), 5, 1e-10),
+                                                           (ℳ, ℳʹ))
+𝐅 = 𝐅 / norm(𝐅)
+𝐅 = 𝐅 / sign(𝐅[1,2])
+@test 𝐅 ≈ 𝐅ₜ