Merge pull request #3 from zygmuntszpak/aml_hessian

Adds function to compute the Hessian matrix of the AML cost

src/cost_function/ModuleCostFunction.jl

@@ -1,6 +1,6 @@
 module ModuleCostFunction
 using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleOperators, MultipleViewGeometry.ModuleMathAliases
 using MultipleViewGeometry.ModuleCarriers
-export cost, X
+export cost, X, H
 include("cost_functions.jl")
 end

src/cost_function/cost_functions.jl

@@ -64,3 +64,64 @@ end
 function Σₙ(entity::FundamentalMatrix, 𝛉::Matrix, 𝐁ₙ::Matrix)
 𝛉' * 𝐁ₙ * 𝛉
 end
+
+function H(c::CostFunction, entity::FundamentalMatrix, 𝛉::Matrix, Λ::Vector{T1},  matches...) where T1 <: Matrix
+    ℳ, ℳʹ = matches
+    N = length(ℳ)
+    if (N != length(ℳʹ))
+          throw(ArgumentError("There should be an equal number of points for each view."))
+    end
+    if (N != length(Λ))
+          throw(ArgumentError("There should be a covariance matrix for each point correspondence."))
+    end
+    _H(c, entity, 𝛉, Λ, ℳ, ℳʹ)
+end
+
+
+function _H(c::CostFunction, entity::ProjectiveEntity, 𝛉::Matrix, Λ::Vector{T1}, 𝒟...) where T1 <: Matrix
+    𝐗 = X(c, entity, 𝛉, Λ, 𝒟...)
+    𝐓 = T(c, entity, 𝛉, Λ, 𝒟...)
+    𝐇 = 2*(𝐗-𝐓)
+end
+
+
+function T(c::CostFunction, entity::ProjectiveEntity, 𝛉::Matrix, Λ::Vector{T1}, 𝒟...) where T1 <: Matrix
+    l = length(𝛉)
+    𝐈ₗ = eye(l)
+    𝐈ₘ = Iₘ(entity)
+    𝐍 = fill(0.0,(l,l))
+    𝐌 = fill(0.0,(l,l))
+    𝐓 = fill(0.0,(l,l))
+    n = 1
+    for dataₙ in zip(𝒟...)
+        𝒟ₙ = dataₙ
+        𝚲ₙ = Λ[n]
+        𝐔ₙ = uₓ(entity,𝒟ₙ)
+        ∂ₓ𝐮ₙ = ∂ₓu(entity, 𝒟ₙ)
+        𝐁ₙ =  ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
+        𝚺ₙ = Σₙ(entity,𝛉, 𝐁ₙ)
+        𝚺ₙ⁻¹ = inv(𝚺ₙ)
+        𝐓₁ = fill(0.0,(l,l))
+        𝐓₂ = fill(0.0,(l,l))
+        𝐓₃ = fill(0.0,(l,l))
+        𝐓₄ = fill(0.0,(l,l))
+        𝐓₅ = fill(0.0,(l,l))
+        for k = 1:l
+            𝐞ₖ = fill(0.0,(l,1))
+            𝐞ₖ[k] = 1
+            ∂𝐞ₖ𝚺ₙ = (𝐈ₘ ⊗ 𝐞ₖ') * 𝐁ₙ * (𝐈ₘ ⊗ 𝛉) + (𝐈ₘ ⊗ 𝛉') * 𝐁ₙ * (𝐈ₘ ⊗ 𝐞ₖ)
+            𝐓₁ = 𝐓₁ + 𝐔ₙ * 𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 * 𝐞ₖ'
+            𝐓₂ = 𝐓₂ + (𝐞ₖ' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
+            𝐓₃ = 𝐓₃ + (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝐈ₘ ⊗ 𝛉) * 𝚺ₙ⁻¹ * 𝐔ₙ'
+            𝐓₄ = 𝐓₄ + (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
+            𝐓₅ = 𝐓₅ + (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
+        end
+        𝐓 = 𝐓 + 𝐓₁ + 𝐓₂ + 𝐓₃ - 𝐓₄ - 𝐓₅
+        n = n + 1
+    end
+    𝐓
+end
+
+function Iₘ(entity::FundamentalMatrix)
+    eye(1)
+end

test/cost_functions_tests.jl

@@ -49,6 +49,15 @@ Jₐₘₗ =  cost(AML(),FundamentalMatrix(), 𝐟 ,
 # The true parameters should lie in the null space of the matrix X.
 @test all(isapprox.(𝐗 * 𝐟, 0.0; atol = 1e-10))
 
+# 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(), reshape(𝐅,9,1),
+                                         [eye(4) for i = 1:length(ℳ)], ℳ, ℳʹ)
+
+# The true parameters should lie in the null space of the matrix H.
+@test all(isapprox.(𝐇 * 𝐟, 0.0; atol = 1e-10))
+
+
 # matches = ℳ, ℳʹ
 # Λ = [eye(4) for i = 1:length(ℳ)]
 # zip(matches, Λ)