diff --git a/data/teapot.mat b/data/teapot.mat new file mode 100755 index 0000000..a0d30de Binary files /dev/null and b/data/teapot.mat differ diff --git a/demo/example_fundamental_matrix.jl b/demo/example_fundamental_matrix.jl index 1f83423..64fd92c 100644 --- a/demo/example_fundamental_matrix.jl +++ b/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 diff --git a/demo/example_unrectified_triangulate.jl b/demo/example_unrectified_triangulate.jl new file mode 100644 index 0000000..7213e78 --- /dev/null +++ b/demo/example_unrectified_triangulate.jl @@ -0,0 +1,111 @@ +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 = 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(),𝐏₂,𝒳) + +# 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) + +# 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(𝒴))) +# 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/1000, p1) +# draw!(Camera3D(), πŠβ‚, 𝐏₁[1:3,1:3], 𝐏₁[:,4], 250/1000, p1) +# draw!(Camera3D(), πŠβ‚‚, 𝐏₂[1:3,1:3], 𝐏₂[:,4], 250/1000, p1) +# draw!(Camera3D(), πŠβ‚, 𝐑₁, 𝐭₁, 250, p1) +# draw!(Camera3D(), πŠβ‚‚, 𝐑₂, 𝐭₂, 250, p1) diff --git a/src/MultipleViewGeometry.jl b/src/MultipleViewGeometry.jl index 9cf085f..3248a32 100644 --- a/src/MultipleViewGeometry.jl +++ b/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 diff --git a/src/carriers/carriers.jl b/src/carriers/carriers.jl index 99cc025..093a956 100644 --- a/src/carriers/carriers.jl +++ b/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, 𝐦 , 𝐦ʹ) diff --git a/src/constraints/ModuleConstraints.jl b/src/constraints/ModuleConstraints.jl new file mode 100644 index 0000000..084d330 --- /dev/null +++ b/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 diff --git a/src/constraints/constraints.jl b/src/constraints/constraints.jl new file mode 100644 index 0000000..735ca0d --- /dev/null +++ b/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 diff --git a/src/construct/ModuleConstruct.jl b/src/construct/ModuleConstruct.jl index 81cbd19..31c6f40 100644 --- a/src/construct/ModuleConstruct.jl +++ b/src/construct/ModuleConstruct.jl @@ -1,4 +1,5 @@ module ModuleConstruct +using MultipleViewGeometry using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleMathAliases, MultipleViewGeometry.ModuleOperators using MultipleViewGeometry.ModuleProjection using StaticArrays @@ -6,4 +7,5 @@ export construct include("construct_projectionmatrix.jl") include("construct_fundamentalmatrix.jl") +include("construct_essentialmatrix.jl") end diff --git a/src/construct/construct_essentialmatrix.jl b/src/construct/construct_essentialmatrix.jl new file mode 100644 index 0000000..b37cb1a --- /dev/null +++ b/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 diff --git a/src/construct/construct_projectionmatrix.jl b/src/construct/construct_projectionmatrix.jl index 757e470..904583c 100644 --- a/src/construct/construct_projectionmatrix.jl +++ b/src/construct/construct_projectionmatrix.jl @@ -21,3 +21,54 @@ function construct( e::ProjectionMatrix, 𝐅::AbstractArray) SMatrix{3,4,Float64,3*4}(𝐏₁), SMatrix{3,4,Float64,3*4}(𝐏₂) end + +function construct( e::ProjectionMatrix, 𝐄::AbstractArray, π’Ÿ::Tuple{AbstractArray, Vararg{AbstractArray}}) + 𝐖 = SMatrix{3,3,Float64,3*3}([0 -1 0; 1 0 0; 0 0 1]) + 𝐙 = SMatrix{3,3,Float64,3*3}([0 1 0; -1 0 0; 0 0 0]) + 𝐔,𝐒,𝐕 = svd(𝐄) + 𝐭 = 𝐔[:,3] + 𝐏₁ = SMatrix{3,4,Float64,3*4}(eye(3,4)) + 𝐏₂₁ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖*𝐕' 𝐭]) + 𝐏₂₂ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖'*𝐕' 𝐭]) + 𝐏₂₃ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖*𝐕' -𝐭]) + 𝐏₂₄ = SMatrix{3,4,Float64,3*4}([𝐔*𝐖'*𝐕' -𝐭]) + + 𝒳₁ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₁, π’Ÿ) + 𝒳₂ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₂, π’Ÿ) + 𝒳₃ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₃, π’Ÿ) + 𝒳₄ = triangulate(DirectLinearTransform(), 𝐏₁, 𝐏₂₄, π’Ÿ) + + # Determine which projection matrix in the second view triangulated + # the majority of points in front of the cameras. + ℳ₁ = map(𝒳₁) do 𝐗 + 𝐦 = 𝐏₂₁ * 𝐗 + 𝐦[3] > 0 + end + + β„³β‚‚ = map(𝒳₂) do 𝐗 + 𝐦 = 𝐏₂₂ * 𝐗 + 𝐦[3] > 0 + end + + ℳ₃ = map(𝒳₃) do 𝐗 + 𝐦 = 𝐏₂₃ * 𝐗 + 𝐦[3] > 0 + end + + β„³β‚„ = map(𝒳₄) do 𝐗 + 𝐦 = 𝐏₂₄ * 𝐗 + 𝐦[3] > 0 + end + + total, index = findmax((sum(ℳ₁), sum(β„³β‚‚), sum(ℳ₃), sum(β„³β‚„))) + + if index == 1 + return 𝐏₁, 𝐏₂₁ + elseif index == 2 + return 𝐏₁, 𝐏₂₂ + elseif index == 3 + return 𝐏₁, 𝐏₂₃ + else + return 𝐏₁, 𝐏₂₄ + end +end diff --git a/src/cost_function/cost_functions.jl b/src/cost_function/cost_functions.jl index 91ba160..278e796 100644 --- a/src/cost_function/cost_functions.jl +++ b/src/cost_function/cost_functions.jl @@ -1,8 +1,8 @@ function cost(c::CostFunction, entity::FundamentalMatrix, 𝛉::AbstractArray, π’ž::Tuple{AbstractArray, Vararg{AbstractArray}}, π’Ÿ::Tuple{AbstractArray, Vararg{AbstractArray}}) - β„³, β„³ΚΉ = collect(π’Ÿ) - Λ₁, Ξ›β‚‚ = collect(π’ž) + β„³, β„³ΚΉ = π’Ÿ + Λ₁, Ξ›β‚‚ = π’ž Jβ‚β‚˜β‚— = 0.0 N = length(π’Ÿ[1]) πš²β‚™ = @MMatrix zeros(4,4) @@ -130,7 +130,7 @@ function covariance_matrix(c::CostFunction, s::CanonicalApproximation, entity::F 𝚲 = _covariance_matrix(AML(),FundamentalMatrix(), 𝛉₁, (Λ₁,Λ₁ʹ), (π’ͺ , π’ͺΚΉ)) 𝛉₁ = 𝛉₁ / norm(𝛉₁) - + # Derivative of the determinant of 𝚯 = reshape(𝛉₁,(3,3)). φ₁ = 𝛉₁[5]*𝛉₁[9] - 𝛉₁[8]*𝛉₁[6] Ο†β‚‚ = -(𝛉₁[4]*𝛉₁[5] - 𝛉₁[7]*𝛉₁[6]) @@ -158,8 +158,8 @@ end function _covariance_matrix(c::CostFunction, entity::FundamentalMatrix, 𝛉::AbstractArray, π’ž::Tuple{AbstractArray, Vararg{AbstractArray}}, π’Ÿ::Tuple{AbstractArray, Vararg{AbstractArray}}) 𝛉 = 𝛉 / norm(𝛉) - β„³, β„³ΚΉ = collect(π’Ÿ) - Λ₁, Ξ›β‚‚ = collect(π’ž) + β„³, β„³ΚΉ = π’Ÿ + Λ₁, Ξ›β‚‚ = π’ž N = length(π’Ÿ[1]) πš²β‚™ = @MMatrix zeros(4,4) πžβ‚ = @SMatrix [1.0; 0.0; 0.0] @@ -233,8 +233,8 @@ function _X(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray,π’ž: 𝐍 = fill(0.0,(l,l)) 𝐌 = fill(0.0,(l,l)) N = length(π’Ÿ[1]) - β„³, β„³ΚΉ = collect(π’Ÿ) - Λ₁, Ξ›β‚‚ = collect(π’ž) + β„³, β„³ΚΉ = π’Ÿ + Λ₁, Ξ›β‚‚ = π’ž πš²β‚™ = @MMatrix zeros(4,4) πžβ‚ = @SMatrix [1.0; 0.0; 0.0] πžβ‚‚ = @SMatrix [0.0; 1.0; 0.0] @@ -291,8 +291,8 @@ function T(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray, π’ž: 𝐌 = fill(0.0,(l,l)) 𝐓 = fill(0.0,(l,l)) N = length(π’Ÿ[1]) - β„³, β„³ΚΉ = collect(π’Ÿ) - Λ₁, Ξ›β‚‚ = collect(π’ž) + β„³, β„³ΚΉ = π’Ÿ + Λ₁, Ξ›β‚‚ = π’ž πš²β‚™ = @MMatrix zeros(4,4) πžβ‚ = @SMatrix [1.0; 0.0; 0.0] πžβ‚‚ = @SMatrix [0.0; 1.0; 0.0] diff --git a/src/draw/ModuleDraw.jl b/src/draw/ModuleDraw.jl index a1c3046..ba3ac43 100644 --- a/src/draw/ModuleDraw.jl +++ b/src/draw/ModuleDraw.jl @@ -4,6 +4,7 @@ using StaticArrays using Plots, Plotly using Juno -export draw!, EpipolarLineGraphic +export draw!, EpipolarLineGraphic, LineSegment3D, PlaneSegment3D, Camera3D +export WorldCoordinateSystem3D include("draw.jl") end diff --git a/src/draw/draw.jl b/src/draw/draw.jl index eadf1d7..094e88a 100644 --- a/src/draw/draw.jl +++ b/src/draw/draw.jl @@ -3,6 +3,18 @@ abstract type GraphicEntity end type EpipolarLineGraphic <: GraphicEntity end +type LineSegment3D <: GraphicEntity +end + +type PlaneSegment3D <: GraphicEntity +end + +type Camera3D <: GraphicEntity +end + +type WorldCoordinateSystem3D <: GraphicEntity +end + function draw!(g::EpipolarLineGraphic, l::AbstractVector, dim::Tuple{<:Number,<:Number}, p::RecipesBase.AbstractPlot{<:RecipesBase.AbstractBackend}) @@ -46,3 +58,69 @@ function is_inbounds(pt::AbstractVector, dim::Tuple{<:Number,<:Number}) nrow, ncol = dim pt[1] >= -1.5 && pt[1] < ncol+1.5 && pt[2] >= -1.5 && pt[2] <= nrow + 1.5 end + + +function draw!(g::LineSegment3D, 𝐨::AbstractArray, 𝐩::AbstractArray, col::Symbol, p::RecipesBase.AbstractPlot{<:RecipesBase.AbstractBackend}) + x = [𝐨; 𝐩][:,1] + y = [𝐨; 𝐩][:,2] + z = [𝐨; 𝐩][:,3] + Plots.path3d!(x,y,z, w = 2,grid = false, box = :none, legend = false, linecolor = col) +end + +function draw!(g::LineSegment3D, 𝐨::AbstractVector, 𝐩::AbstractVector, col::Symbol, p::RecipesBase.AbstractPlot{<:RecipesBase.AbstractBackend}) + draw!(LineSegment3D(), 𝐨', 𝐩', col, p) +end + +function draw!(g::PlaneSegment3D, 𝐩₁::AbstractArray, 𝐩₂::AbstractArray, 𝐩₃::AbstractArray, 𝐩₄::AbstractArray, col::Symbol, p::RecipesBase.AbstractPlot{<:RecipesBase.AbstractBackend}) + draw!(LineSegment3D(), 𝐩₁, 𝐩₂, col, p) + draw!(LineSegment3D(), 𝐩₂, 𝐩₃, col, p) + draw!(LineSegment3D(), 𝐩₃, 𝐩₄, col, p) + draw!(LineSegment3D(), 𝐩₄, 𝐩₁, col, p) +end + +function draw!(g::WorldCoordinateSystem3D, scale, p::RecipesBase.AbstractPlot{<:RecipesBase.AbstractBackend}) + πžβ‚ = [1, 0, 0] + πžβ‚‚ = [0, 1, 0] + πžβ‚ƒ = [0, 0, 1] + 𝐨 = [0, 0, 0] + + # Draw the world coordinate axes. + draw!(LineSegment3D(), 𝐨, 𝐨 + scale*πžβ‚, :red, p) + draw!(LineSegment3D(), 𝐨, 𝐨 + scale*πžβ‚‚, :green, p) + draw!(LineSegment3D(), 𝐨, 𝐨 + scale*πžβ‚ƒ, :blue, p) +end + +function draw!(g::Camera3D, 𝐊::AbstractArray, 𝐑::AbstractArray, 𝐭::AbstractArray, scale, p::RecipesBase.AbstractPlot{<:RecipesBase.AbstractBackend}) + + # Origin of the world coordinate system. + πžβ‚ = [1, 0, 0] + πžβ‚‚ = [0, 1, 0] + πžβ‚ƒ = [0, 0, 1] + 𝐨 = [0, 0, 0] + + # Initial camera imaging plane. + 𝐩₁ = [-125, 125, -50] + 𝐩₂ = [125, 125, -50] + 𝐩₃ = [125, -125, -50] + 𝐩₄ = [-125, -125, -50] + + # Initial camera center. + 𝐜 = [0.0, 0.0, 0.0] + 𝐜 = 𝐑*𝐜 + 𝐭 + Plots.plot!([𝐜[1]],[𝐜[2]],[𝐜[3]],seriestype = :scatter, ms=1, grid = false, box = :none, legend = false, markercolor=:Red) + + draw!(PlaneSegment3D(), 𝐑*𝐩₁ + 𝐭, 𝐑*𝐩₂ + 𝐭, 𝐑*𝐩₃ + 𝐭, 𝐑*𝐩₄ + 𝐭, :black, p) + + # Connect camera center with corners of plane segment. + draw!(LineSegment3D(), 𝐜, 𝐑*𝐩₁ + 𝐭, :black, p) + draw!(LineSegment3D(), 𝐜, 𝐑*𝐩₂ + 𝐭, :black, p) + draw!(LineSegment3D(), 𝐜, 𝐑*𝐩₃ + 𝐭, :black, p) + draw!(LineSegment3D(), 𝐜, 𝐑*𝐩₄ + 𝐭, :black, p) + + # Draw camera coordinate axes for the first camera. + draw!(LineSegment3D(), 𝐜, (𝐑*scale*πžβ‚ + 𝐭), :red, p) + draw!(LineSegment3D(), 𝐜, (𝐑*scale*πžβ‚‚ + 𝐭), :green, p) + draw!(LineSegment3D(), 𝐜, (𝐑*scale*πžβ‚ƒ + 𝐭), :blue, p) + + +end diff --git a/src/estimate/estimate_twoview.jl b/src/estimate/estimate_twoview.jl index 19603ed..820bc8f 100644 --- a/src/estimate/estimate_twoview.jl +++ b/src/estimate/estimate_twoview.jl @@ -1,5 +1,5 @@ function estimate(entity::FundamentalMatrix, method::DirectLinearTransform, π’Ÿ::Tuple{AbstractArray, Vararg{AbstractArray}}) - β„³, β„³ΚΉ = collect(π’Ÿ) + β„³, β„³ΚΉ = π’Ÿ N = length(β„³) if (N != length(β„³ΚΉ)) throw(ArgumentError("There should be an equal number of points for each view.")) diff --git a/src/noise/ModuleNoise.jl b/src/noise/ModuleNoise.jl new file mode 100644 index 0000000..c6bb26c --- /dev/null +++ b/src/noise/ModuleNoise.jl @@ -0,0 +1,6 @@ +module ModuleNoise +using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleMathAliases, MultipleViewGeometry.ModuleOperators +using StaticArrays +export perturb +include("perturb.jl") +end diff --git a/src/noise/perturb.jl b/src/noise/perturb.jl new file mode 100644 index 0000000..d9aef1e --- /dev/null +++ b/src/noise/perturb.jl @@ -0,0 +1,15 @@ +# Assume homogeneous coordinates +function perturb(noise::GaussianNoise, Οƒ::Real, π’Ÿ::Tuple{AbstractArray, Vararg{AbstractArray}}) + 𝓔 = deepcopy(π’Ÿ) + S = length(𝓔) + for s = 1:S + β„³ = 𝓔[s] + N = length(β„³) + for n = 1:N + 𝐦 = β„³[n] + D = length(𝐦) + 𝐦 .= 𝐦 + push(Οƒ*SVector(randn((D-1,1))...),0.0) + end + end + 𝓔 +end diff --git a/src/triangulation/ModuleTriangulation.jl b/src/triangulation/ModuleTriangulation.jl new file mode 100644 index 0000000..14b0532 --- /dev/null +++ b/src/triangulation/ModuleTriangulation.jl @@ -0,0 +1,9 @@ +module ModuleTriangulation +using MultipleViewGeometry.ModuleMathAliases, MultipleViewGeometry.ModuleDataNormalization +using MultipleViewGeometry.ModuleTypes, MultipleViewGeometry.ModuleProjection +using MultipleViewGeometry.ModuleConstraints, MultipleViewGeometry.ModuleConstruct +using MultipleViewGeometry.ModuleOperators +using StaticArrays +export triangulate +include("triangulate.jl") +end diff --git a/src/triangulation/triangulate.jl b/src/triangulation/triangulate.jl new file mode 100644 index 0000000..c8dd2ab --- /dev/null +++ b/src/triangulation/triangulate.jl @@ -0,0 +1,56 @@ +function triangulate(method::DirectLinearTransform, 𝐅::AbstractArray, π’Ÿ::Tuple{AbstractArray, Vararg{AbstractArray}}) + β„³, β„³ΚΉ = π’Ÿ + 𝐏₁, 𝐏₂ = construct(ProjectionMatrix(),𝐅) + N = length(β„³) + 𝒴 = Array{Point3DH}(N) + for n = 1:N + 𝐦 = β„³[n] + 𝐦ʹ = β„³ΚΉ[n] + 𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ; + (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ; + (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ; + (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ] + # 𝐀₁ = vec2antisym(𝐦)*𝐏₁ + # 𝐀₂ = vec2antisym(𝐦ʹ)*𝐏₂ + # @show typeof(𝐀₁) + # @show size(𝐀₁) + # @show size(𝐀₂) + # 𝐀 = vcat(𝐀₁,𝐀₂) + # @show size(𝐀) + # Ξ», f = smallest_eigenpair(Symmetric(Array(𝐀'*𝐀))) + # @show Ξ» + # 𝒴[n] = Point3DH(𝑛(f)) + + U,S,V = svd(Array(𝐀)) + 𝒴[n] = Point3DH(𝑛(V[:,4])) + end + 𝒴 +end + +function triangulate(method::DirectLinearTransform, 𝐏₁::AbstractArray, 𝐏₂::AbstractArray, π’Ÿ::Tuple{AbstractArray, Vararg{AbstractArray}}) + β„³, β„³ΚΉ = π’Ÿ + N = length(β„³) + 𝒴 = Array{Point3DH}(N) + for n = 1:N + 𝐦 = β„³[n] + 𝐦ʹ = β„³ΚΉ[n] + 𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ; + (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ; + (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ; + (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ] + # 𝐀₁ = vec2antisym(𝐦)*𝐏₁ + # 𝐀₂ = vec2antisym(𝐦ʹ)*𝐏₂ + # @show typeof(𝐀₁) + # @show size(𝐀₁) + # @show size(𝐀₂) + # 𝐀 = vcat(𝐀₁,𝐀₂) + # @show size(𝐀) + # Ξ», f = smallest_eigenpair(Symmetric(Array(𝐀'*𝐀))) + # @show Ξ» + # 𝒴[n] = Point3DH(𝑛(f)) + + U,S,V = svd(Array(𝐀)) + 𝒴[n] = Point3DH(𝑛(V[:,4])) + end + 𝒴 +end diff --git a/src/types/ModuleTypes.jl b/src/types/ModuleTypes.jl index e4ca9d7..1bf3158 100644 --- a/src/types/ModuleTypes.jl +++ b/src/types/ModuleTypes.jl @@ -2,14 +2,14 @@ module ModuleTypes using StaticArrays export HomogeneousPoint, ProjectiveEntity, FundamentalMatrix, ProjectionMatrix -export HomogeneousCoordinates +export HomogeneousCoordinates, EssentialMatrix export CameraModel, Pinhole, CanonicalLens -export EstimationAlgorithm, DirectLinearTransform, Taubin,FundamentalNumericalScheme +export EstimationAlgorithm, DirectLinearTransform, Taubin, FundamentalNumericalScheme export CostFunction, ApproximateMaximumLikelihood, AML export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical export CovarianceMatrices export Point2DH, Point3DH export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme - +export NoiseModel, GaussianNoise include("types.jl") end diff --git a/src/types/types.jl b/src/types/types.jl index 816bca3..4506044 100644 --- a/src/types/types.jl +++ b/src/types/types.jl @@ -47,9 +47,15 @@ abstract type CoordinateSystemTransformation end abstract type CovarianceEstimationScheme end +abstract type NoiseModel end + + type FundamentalMatrix <: ProjectiveEntity end +type EssentialMatrix <: ProjectiveEntity +end + type ProjectionMatrix <: ProjectiveEntity end @@ -77,8 +83,6 @@ end type HessianApproximation <: CovarianceEstimationScheme end - - type Taubin <: EstimationAlgorithm end @@ -94,3 +98,8 @@ end type CovarianceMatrices end + + +type GaussianNoise <: NoiseModel + +end diff --git a/test/construct_essentialmatrix_tests.jl b/test/construct_essentialmatrix_tests.jl new file mode 100644 index 0000000..a89d03a --- /dev/null +++ b/test/construct_essentialmatrix_tests.jl @@ -0,0 +1,28 @@ +using MultipleViewGeometry, MultipleViewGeometry.ModuleRotation, Base.Test + +# Intrinsic and extrinsic parameters for the first camera. +πŠβ‚ = zeros(3,3) +πŠβ‚[1,1] = 10 +πŠβ‚[2,2] = 10 +πŠβ‚[3,3] = 1 +𝐑₁ = rotxyz(deg2rad(10), deg2rad(15), deg2rad(45)) +𝐭₁ = [-250.0, 0.0, 2500.0] + +# Intrinsic and extrinsic parameters for the second camera. +πŠβ‚‚ = zeros(3,3) +πŠβ‚‚[1,1] = 5 +πŠβ‚‚[2,2] = 5 +πŠβ‚‚[3,3] = 1 +𝐑₂ = rotxyz(deg2rad(10), deg2rad(15), deg2rad(45)) +𝐭₂ = [250.0, 0.0, 2500.0] + +𝐅 = construct(FundamentalMatrix(),πŠβ‚,𝐑₁,𝐭₁,πŠβ‚‚,𝐑₂,𝐭₂) +𝐄 = construct(EssentialMatrix(),𝐅, πŠβ‚, πŠβ‚‚) + +# Result 9.17 of R. Hartley and A. Zisserman, β€œTwo-View Geometry,” Multiple View Geometry in Computer Vision +# A 3 by 3 matrix is an essential matrix if and only if two of its singular values +# are equal, and the third is zero. +U, S , V = svd(𝐄) + +@test isapprox.(S[1], S[2]; atol = 1e-14) +@test isapprox.(S[3], 0.0; atol = 1e-10) diff --git a/test/perturb_tests.jl b/test/perturb_tests.jl new file mode 100644 index 0000000..9a58491 --- /dev/null +++ b/test/perturb_tests.jl @@ -0,0 +1,23 @@ +using MultipleViewGeometry, Base.Test +using MultipleViewGeometry.ModuleCostFunction +using MultipleViewGeometry.ModuleTypes +using MultipleViewGeometry.ModuleConstraints +using MultipleViewGeometry.ModuleConstruct +using MultipleViewGeometry.ModuleNoise +using BenchmarkTools, Compat +using StaticArrays + +# Fix random seed. +srand(1234) + +𝒳 = [Point3DH(x,y,z,1.0) + for x=-1:1:10 for y=-1:1:10 for z=-1:1:10] +π’Ÿ = perturb(GaussianNoise(), 1.0, tuple(𝒳) ) +𝒳ʹ = π’Ÿ[1] + +N = length(𝒳ʹ) +for n = 1:N + @test !isapprox(sum(abs.(𝒳[1][1:3]-𝒳ʹ[1][1:3])/4), 0.0; atol = 1e-12) + # No noise should have been added to the last coordinate. + @test isapprox(sum(abs.(𝒳[1][4]-𝒳ʹ[1][4])), 0.0; atol = 1e-12) +end diff --git a/test/runtests.jl b/test/runtests.jl index e952379..34da0b8 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -6,8 +6,12 @@ using Base.Test @testset "Data Normalization Tests" begin include("data_normalization_tests.jl") end @testset "Transform Tests" begin include("transform_tests.jl") end @testset "Fundamental Matrix Construction Tests" begin include("construct_fundamentalmatrix_tests.jl") end +@testset "Essential Matrix Construction Tests" begin include("construct_essentialmatrix_tests.jl") end @testset "Estimate Two View Tests" begin include("estimate_twoview_tests.jl") end @testset "Projection Matrix Construction Tests" begin include("construct_projectionmatrix_tests.jl") end @testset "Rotations Tests" begin include("rotations_tests.jl") end @testset "Cost Function Tests" begin include("cost_functions_tests.jl") end @testset "Fundamental Matrix Covariance Test" begin include("fundamental_matrix_covariance_test.jl") end +@testset "Satisfy Epipolar Constraints Test" begin include("satisfy_epipolar_constraints_tests.jl") end +@testset "Triangulate Test" begin include("triangulate_tests.jl") end +@testset "Noise Test" begin include("perturb_tests.jl") end diff --git a/test/satisfy_epipolar_constraints_tests.jl b/test/satisfy_epipolar_constraints_tests.jl new file mode 100644 index 0000000..347588d --- /dev/null +++ b/test/satisfy_epipolar_constraints_tests.jl @@ -0,0 +1,86 @@ +using MultipleViewGeometry, Base.Test +using MultipleViewGeometry.ModuleCostFunction +using MultipleViewGeometry.ModuleTypes +using MultipleViewGeometry.ModuleConstraints +using BenchmarkTools, Compat +using StaticArrays + +# Fix random seed. +srand(1234) + +# Test for cost functions. + +# Test cost function on Fundamental matrix estimation. + +𝒳 = [Point3DH(x,y,z,1.0) + for x=-1:0.5:10 for y=-1:0.5:10 for z=2:-0.1:1] + +# Intrinsic and extrinsic parameters of camera one. +πŠβ‚ = @SMatrix eye(3) +𝐑₁ = @SMatrix eye(3) +𝐭₁ = @SVector [0.0, 0.0, -10] + +# Intrinsic and extrinsic parameters of camera two. +πŠβ‚‚ = @SMatrix eye(3) +𝐑₂ = @SMatrix eye(3) #SMatrix{3,3,Float64,9}(rotxyz(pi/10,pi/10,pi/10)) +𝐭₂ = @SVector [10.0, 10.0, -10.0] + +# Camera projection matrices. +𝐏₁ = construct(ProjectionMatrix(),πŠβ‚,𝐑₁,𝐭₁) +𝐏₂ = construct(ProjectionMatrix(),πŠβ‚‚,𝐑₂,𝐭₂) + +# Set of corresponding points. +β„³ = project(Pinhole(),𝐏₁,𝒳) +β„³ΚΉ = project(Pinhole(),𝐏₂,𝒳) + +𝐅 = construct(FundamentalMatrix(),πŠβ‚,𝐑₁,𝐭₁,πŠβ‚‚,𝐑₂,𝐭₂) + +# Verify that the algorithm returns the correct answer when the +# constraint is already satisfied. +π’ͺ ,π’ͺΚΉ = satisfy(FundamentalMatrix(), EpipolarConstraint(), 𝐅, (β„³, β„³ΚΉ)) + +# Verify that the original corresponding points satisfy the epipolar constraint. +N = length(β„³) +for n = 1:N + 𝐦 = β„³[n] + 𝐦ʹ = β„³ΚΉ[n] + @test isapprox(𝐦'*𝐅*𝐦ʹ, 0.0; atol = 1e-14) +end + +# Verify that the 'corrected' points satisfy the epipolar constraint. +N = length(β„³) +for n = 1:N + 𝐦 = π’ͺ[n] + 𝐦ʹ = π’ͺΚΉ[n] + @test isapprox(𝐦'*𝐅*𝐦ʹ, 0.0; atol = 1e-14) +end + +# Perturb the original corresponding points slightly so that they no-longer +# satisfy the epipolar constraint. +N = length(β„³) +Οƒ = 1e-7 +for n = 1:N + β„³[n] = β„³[n] + SVector{3}(Οƒ * vcat(rand(2,1),0)) + β„³ΚΉ[n] = β„³ΚΉ[n] + SVector{3}(Οƒ * vcat(rand(2,1),0)) + 𝐦 = β„³[n] + 𝐦ʹ = β„³ΚΉ[n] + @test abs(𝐦'*𝐅*𝐦ʹ) > 1e-12 +end + + +# Verify that the algorithm returns the correct answer when applied +# to sets of correspondences that do not satisfy the epipolar constraint. +π’ͺ ,π’ͺΚΉ = satisfy(FundamentalMatrix(), EpipolarConstraint(), 𝐅, (β„³, β„³ΚΉ)) + +# Verify that the 'corrected' points satisfy the epipolar constraint. +N = length(β„³) +for n = 1:N + 𝐦 = π’ͺ[n] + 𝐦ʹ = π’ͺΚΉ[n] + @test isapprox(𝐦'*𝐅*𝐦ʹ, 0.0; atol = 1e-14) +end + +# Οƒ = 1e-7 +# SVector{3}(Οƒ * vcat(rand(2,1),0)) +# +# β„³[1] - π’ͺ[1] diff --git a/test/triangulate_tests.jl b/test/triangulate_tests.jl new file mode 100644 index 0000000..9a27898 --- /dev/null +++ b/test/triangulate_tests.jl @@ -0,0 +1,62 @@ +using MultipleViewGeometry, Base.Test +using MultipleViewGeometry.ModuleCostFunction +using MultipleViewGeometry.ModuleTypes +using MultipleViewGeometry.ModuleConstraints +using MultipleViewGeometry.ModuleConstruct +using BenchmarkTools, Compat +using StaticArrays + +# Fix random seed. +srand(1234) + +𝒳 = [Point3DH(x,y,z,1.0) + for x=-1:0.5:10 for y=-1:0.5:10 for z=2:-0.1:1] + +# Intrinsic and extrinsic parameters of camera one. +πŠβ‚ = @SMatrix eye(3) +𝐑₁ = @SMatrix eye(3) +𝐭₁ = @SVector [0.0, 0.0, -10] + +# Intrinsic and extrinsic parameters of camera two. +πŠβ‚‚ = @SMatrix eye(3) +𝐑₂ = @SMatrix eye(3) #SMatrix{3,3,Float64,9}(rotxyz(pi/10,pi/10,pi/10)) +𝐭₂ = @SVector [10.0, 10.0, -10.0] + +# Camera projection matrices. +𝐏₁ = construct(ProjectionMatrix(),πŠβ‚,𝐑₁,𝐭₁) +𝐏₂ = construct(ProjectionMatrix(),πŠβ‚‚,𝐑₂,𝐭₂) + +# Set of corresponding points. +β„³ = project(Pinhole(),𝐏₁,𝒳) +β„³ΚΉ = project(Pinhole(),𝐏₂,𝒳) + +𝒴 = triangulate(DirectLinearTransform(),𝐏₁,𝐏₂,(β„³,β„³ΚΉ)) + +# Triangulating with the same projection matrices that were used to construct +# (β„³,β„³ΚΉ) should yield the same 3D points as the original 𝒳. +N = length(𝒴) +for n = 1:N + @test isapprox(sum(abs.(𝒳[n]-𝒴[n])/4), 0.0; atol = 1e-12) +end + + +𝐅 = 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(),𝐅,(β„³,β„³ΚΉ)) + +𝐐₁, 𝐐₂ = construct(ProjectionMatrix(),𝐅) +π’ͺ = project(Pinhole(),𝐐₁,𝒴) +π’ͺΚΉ= project(Pinhole(),𝐐₂,𝒴) +N = length(π’ͺ) +for n = 1:N + 𝐦 = π’ͺ[n] + 𝐦ʹ = π’ͺΚΉ[n] + @test isapprox(𝐦'*𝐅*𝐦ʹ, 0.0; atol = 1e-14) +end