From b4af7c60cd58b6edd89d2e415184ffa491dbdb4b Mon Sep 17 00:00:00 2001 From: "Dr. Zygmunt L. Szpak" Date: Tue, 21 Aug 2018 22:55:35 +0930 Subject: [PATCH] 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 | 2 +- src/MultipleViewGeometry.jl | 3 +- src/draw/ModuleDraw.jl | 2 +- src/estimate/ModuleEstimation.jl | 3 +- src/estimate/estimate_twoview.jl | 94 ++++++++++++++++++++++++++++++ src/operators/ModuleOperators.jl | 1 + src/operators/operators.jl | 22 +++++++ src/types/ModuleTypes.jl | 1 + src/types/types.jl | 6 ++ test/estimate_twoview_tests.jl | 15 ++++- 10 files changed, 142 insertions(+), 7 deletions(-) diff --git a/demo/example_fundamental_matrix.jl b/demo/example_fundamental_matrix.jl index 64fd92c..95e2953 100644 --- a/demo/example_fundamental_matrix.jl +++ b/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) diff --git a/src/MultipleViewGeometry.jl b/src/MultipleViewGeometry.jl index 3248a32..9748a96 100644 --- a/src/MultipleViewGeometry.jl +++ b/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 diff --git a/src/draw/ModuleDraw.jl b/src/draw/ModuleDraw.jl index ba3ac43..3f8390d 100644 --- a/src/draw/ModuleDraw.jl +++ b/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 diff --git a/src/estimate/ModuleEstimation.jl b/src/estimate/ModuleEstimation.jl index 21bce31..460b0a8 100644 --- a/src/estimate/ModuleEstimation.jl +++ b/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 diff --git a/src/estimate/estimate_twoview.jl b/src/estimate/estimate_twoview.jl index 820bc8f..fb85c52 100644 --- a/src/estimate/estimate_twoview.jl +++ b/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 diff --git a/src/operators/ModuleOperators.jl b/src/operators/ModuleOperators.jl index abf5803..0911f6a 100644 --- a/src/operators/ModuleOperators.jl +++ b/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 diff --git a/src/operators/operators.jl b/src/operators/operators.jl index fbf05ed..8ddbd32 100644 --- a/src/operators/operators.jl +++ b/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(𝐧) diff --git a/src/types/ModuleTypes.jl b/src/types/ModuleTypes.jl index 1bf3158..6bcc530 100644 --- a/src/types/ModuleTypes.jl +++ b/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 diff --git a/src/types/types.jl b/src/types/types.jl index 4506044..4dd7002 100644 --- a/src/types/types.jl +++ b/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 diff --git a/test/estimate_twoview_tests.jl b/test/estimate_twoview_tests.jl index 9f12ada..6f6d802 100644 --- a/test/estimate_twoview_tests.jl +++ b/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 𝐅 β‰ˆ π…β‚œ