From 61290908f98c47222123862dc386848514d8f389 Mon Sep 17 00:00:00 2001
From: "Dr. Zygmunt L. Szpak" <zygmunt.szpak@gmail.com>
Date: Wed, 31 Oct 2018 16:48:09 +1030
Subject: [PATCH] Improves performance through consistent use of StaticArrays

---
 REQUIRE                                       |  3 +-
 src/MultipleViewGeometry.jl                   |  6 +-
 src/constraints/constraints.jl                |  8 +--
 src/construct/construct_essentialmatrix.jl    |  2 +-
 src/construct/construct_fundamentalmatrix.jl  |  4 +-
 src/cost_function/cost_functions.jl           | 54 +++++++--------
 .../hartley_transformation.jl                 | 15 ++--
 src/estimate/estimate_twoview.jl              |  5 +-
 src/moments/moments_fundamentalmatrix.jl      |  4 +-
 src/operators/operators.jl                    |  6 +-
 src/projection/project.jl                     |  2 +-
 src/triangulation/triangulate.jl              | 41 ++++++-----
 src/types/ModuleTypes.jl                      |  2 +-
 src/types/types.jl                            | 19 +++---
 test/construct_projectionmatrix_tests.jl      |  2 +-
 test/cost_functions_tests.jl                  |  9 ++-
 test/data_normalization_tests.jl              | 68 +++++++++----------
 test/estimate_twoview_tests.jl                | 12 ++--
 test/satisfy_epipolar_constraints_tests.jl    | 31 ++++-----
 test/transform_tests.jl                       | 56 +++++++--------
 test/triangulate_tests.jl                     | 11 ++-
 21 files changed, 183 insertions(+), 177 deletions(-)

diff --git a/REQUIRE b/REQUIRE
index bf90cf6..fa0c957 100644
--- a/REQUIRE
+++ b/REQUIRE
@@ -1,7 +1,6 @@
 julia 0.7
-StaticArrays
+StaticArrays 0.8.3
 Juno
-Plots
 LsqFit
 LinearAlgebra
 BenchmarkTools
diff --git a/src/MultipleViewGeometry.jl b/src/MultipleViewGeometry.jl
index f87d6bf..ede65d5 100644
--- a/src/MultipleViewGeometry.jl
+++ b/src/MultipleViewGeometry.jl
@@ -16,7 +16,7 @@ export CostFunction, ApproximateMaximumLikelihood, AML
 export HomogeneousCoordinates
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices
-export Point2DH, Point3DH
+export Point2D, Point2DH, Point3D, Point3DH
 export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme
 export NoiseModel, GaussianNoise
 
@@ -81,7 +81,7 @@ include("moments/ModuleMoments.jl")
 include("cost_function/ModuleCostFunction.jl")
 include("estimate/ModuleEstimation.jl")
 include("construct/ModuleConstruct.jl")
-include("draw/ModuleDraw.jl")
+#include("draw/ModuleDraw.jl")
 include("constraints/ModuleConstraints.jl")
 include("triangulation/ModuleTriangulation.jl")
 include("noise/ModuleNoise.jl")
@@ -98,7 +98,7 @@ using .ModuleEstimation
 using .ModuleMoments
 using .ModuleCostFunction
 using .ModuleConstruct
-using .ModuleDraw
+#using .ModuleDraw
 using .ModuleConstraints
 using .ModuleTriangulation
 using .ModuleNoise
diff --git a/src/constraints/constraints.jl b/src/constraints/constraints.jl
index f169dd0..9ed9063 100644
--- a/src/constraints/constraints.jl
+++ b/src/constraints/constraints.jl
@@ -14,8 +14,8 @@ function satisfy(entity::FundamentalMatrix, constraint::EpipolarConstraint, 𝐅
 
     I = 10
     for n = 1:N
-        𝐦 = ℳ[n]
-        𝐦ʹ = ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ = hom(ℳʹ[n])
         𝐦ₕ = init_correction_view_1(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
         𝐦ₕʹ= init_correction_view_2(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
         for i = 1:I
@@ -24,8 +24,8 @@ function satisfy(entity::FundamentalMatrix, constraint::EpipolarConstraint, 𝐅
             𝐦ₕ = update_correction_view_1(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
             𝐦ₕʹ = update_correction_view_2(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
         end
-        𝒪[n] = 𝐦ₕ
-        𝒪ʹ[n] = 𝐦ₕʹ
+        𝒪[n] = hom⁻¹(𝐦ₕ)
+        𝒪ʹ[n] = hom⁻¹(𝐦ₕʹ)
     end
     𝒪 ,𝒪ʹ
 end
diff --git a/src/construct/construct_essentialmatrix.jl b/src/construct/construct_essentialmatrix.jl
index b37cb1a..bc36810 100644
--- a/src/construct/construct_essentialmatrix.jl
+++ b/src/construct/construct_essentialmatrix.jl
@@ -7,5 +7,5 @@ function construct( e::EssentialMatrix, 𝐅::AbstractArray,  𝐊₁::AbstractA
     end
     # Equation 9.12 Chapter 9 from Hartley & Zisserman
     𝐄 = 𝐊₂'*𝐅*𝐊₁
-    MMatrix{3,3,Float64,3*3}(𝐄)
+    SMatrix{3,3,Float64,3*3}(𝐄)
 end
diff --git a/src/construct/construct_fundamentalmatrix.jl b/src/construct/construct_fundamentalmatrix.jl
index fb8b312..020efb8 100644
--- a/src/construct/construct_fundamentalmatrix.jl
+++ b/src/construct/construct_fundamentalmatrix.jl
@@ -14,7 +14,7 @@ function construct( e::FundamentalMatrix,
         throw(ArgumentError("Expect length-3 translation vectors."))
     end
     𝐅 = vec2antisym(𝐊₂*𝐑₂*(𝐭₁ .- 𝐭₂))*𝐊₂*𝐑₂/𝐑₁/𝐊₁
-    MMatrix{3,3,Float64,3*3}(𝐅)
+    SMatrix{3,3,Float64,3*3}(𝐅)
 end
 
 function construct( e::FundamentalMatrix, 𝐏₁::AbstractArray, 𝐏₂::AbstractArray)
@@ -24,5 +24,5 @@ function construct( e::FundamentalMatrix, 𝐏₁::AbstractArray, 𝐏₂::Abstr
     𝐜₁ = SVector{4,Float64}(nullspace(Array(𝐏₁)))
     𝐞₂ = 𝐏₂*𝐜₁
     𝐅 = vec2antisym(𝐞₂)*𝐏₂*pinv(Array(𝐏₁))
-    MMatrix{3,3,Float64,3*3}(𝐅)
+    SMatrix{3,3,Float64,3*3}(𝐅)
 end
diff --git a/src/cost_function/cost_functions.jl b/src/cost_function/cost_functions.jl
index e675945..ad2d9f2 100644
--- a/src/cost_function/cost_functions.jl
+++ b/src/cost_function/cost_functions.jl
@@ -12,8 +12,8 @@ function cost(c::CostFunction, entity::FundamentalMatrix, 𝛉::AbstractArray, 
     @inbounds for n = 1:N
         𝚲ₙ[1:2,1:2] .=  Λ₁[n][index,index]
         𝚲ₙ[3:4,3:4] .=  Λ₂[n][index,index]
-        𝐦 = ℳ[n]
-        𝐦ʹ= ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ= hom(ℳʹ[n])
         𝐔ₙ = (𝐦 ⊗ 𝐦ʹ)
         ∂ₓ𝐮ₙ =  [(𝐞₁ ⊗ 𝐦ʹ) (𝐞₂ ⊗ 𝐦ʹ) (𝐦 ⊗ 𝐞₁) (𝐦 ⊗ 𝐞₂)]
         𝐁ₙ =  ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
@@ -229,9 +229,9 @@ end
 
 function _X(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray,𝒞::Tuple{AbstractArray, Vararg{AbstractArray}}, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
     l = length(𝛉)
-    𝐈ₗ = Matrix{Float64}(I,l,l)
-    𝐍 = fill(0.0,(l,l))
-    𝐌 = fill(0.0,(l,l))
+    𝐈ₗ = SMatrix{l,l}(1.0I)
+    𝐍 = @SMatrix zeros(l,l)
+    𝐌 = @SMatrix zeros(l,l)
     N = length(𝒟[1])
     ℳ, ℳʹ = 𝒟
     Λ₁, Λ₂ = 𝒞
@@ -242,8 +242,8 @@ function _X(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray,𝒞:
         index = SVector(1,2)
         𝚲ₙ[1:2,1:2] .=  Λ₁[n][index,index]
         𝚲ₙ[3:4,3:4] .=  Λ₂[n][index,index]
-        𝐦 = ℳ[n]
-        𝐦ʹ= ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ= hom(ℳʹ[n])
         𝐔ₙ = (𝐦 ⊗ 𝐦ʹ)
         ∂ₓ𝐮ₙ =  [(𝐞₁ ⊗ 𝐦ʹ) (𝐞₂ ⊗ 𝐦ʹ) (𝐦 ⊗ 𝐞₁) (𝐦 ⊗ 𝐞₂)]
         𝐁ₙ =  ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
@@ -285,50 +285,50 @@ end
 
 function T(c::CostFunction, entity::ProjectiveEntity, 𝛉::AbstractArray, 𝒞::Tuple{AbstractArray, Vararg{AbstractArray}}, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
     l = length(𝛉)
-    𝐈ₗ = Matrix{Float64}(I, l, l)
-    𝐈ₘ = Iₘ(entity)
-    𝐍 = fill(0.0,(l,l))
-    𝐌 = fill(0.0,(l,l))
-    𝐓 = fill(0.0,(l,l))
+    𝐈ₗ = SMatrix{l,l}(1.0I)
+    𝐈ₘ =  Iₘ(entity)
+    𝐍 = @SMatrix zeros(l,l)
+    𝐌 = @SMatrix zeros(l,l)
+    𝐓 = @SMatrix zeros(l,l)
     N = length(𝒟[1])
     ℳ, ℳʹ = 𝒟
     Λ₁, Λ₂ = 𝒞
     𝚲ₙ = @MMatrix zeros(4,4)
     𝐞₁ = @SMatrix [1.0; 0.0; 0.0]
     𝐞₂ = @SMatrix [0.0; 1.0; 0.0]
-    for n = 1:N
+    for n = 1: N
         index = SVector(1,2)
         𝚲ₙ[1:2,1:2] .=  Λ₁[n][index,index]
         𝚲ₙ[3:4,3:4] .=  Λ₂[n][index,index]
-        𝐦 = ℳ[n]
-        𝐦ʹ= ℳʹ[n]
+        𝐦 = hom(ℳ[n])
+        𝐦ʹ= hom(ℳʹ[n])
         𝐔ₙ = (𝐦 ⊗ 𝐦ʹ)
         ∂ₓ𝐮ₙ =  [(𝐞₁ ⊗ 𝐦ʹ) (𝐞₂ ⊗ 𝐦ʹ) (𝐦 ⊗ 𝐞₁) (𝐦 ⊗ 𝐞₂)]
-        𝐁ₙ =  ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
+        𝐁ₙ = ∂ₓ𝐮ₙ * 𝚲ₙ * ∂ₓ𝐮ₙ'
         𝚺ₙ = 𝛉' * 𝐁ₙ * 𝛉
         𝚺ₙ⁻¹ = 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))
+        𝐓₁ = @SMatrix zeros(Float64,l,l)
+        𝐓₂ = @SMatrix zeros(Float64,l,l)
+        𝐓₃ = @SMatrix zeros(Float64,l,l)
+        𝐓₄ = @SMatrix zeros(Float64,l,l)
+        𝐓₅ = @SMatrix zeros(Float64,l,l)
+        # The additional parentheses around some of the terms are needed as
+        # a workaround to a bug where Base.afoldl allocates memory unnecessarily.
+        # https://github.com/JuliaArrays/StaticArrays.jl/issues/537
         for k = 1:l
-            𝐞ₖ = fill(0.0,(l,1))
-            𝐞ₖ[k] = 1
+            𝐞ₖ = 𝐈ₗ[:,k]
             ∂𝐞ₖ𝚺ₙ = (𝐈ₘ ⊗ 𝐞ₖ') * 𝐁ₙ * (𝐈ₘ ⊗ 𝛉) + (𝐈ₘ ⊗ 𝛉') * 𝐁ₙ * (𝐈ₘ ⊗ 𝐞ₖ)
-            𝐓₁ = 𝐓₁ + 𝐔ₙ * 𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 * 𝐞ₖ'
+            𝐓₁ = 𝐓₁ + (((𝐔ₙ * 𝚺ₙ⁻¹) * (∂𝐞ₖ𝚺ₙ)) * 𝚺ₙ⁻¹) * 𝐔ₙ' * 𝛉 * 𝐞ₖ'
             𝐓₂ = 𝐓₂ + (𝐞ₖ' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
             𝐓₄ = 𝐓₄ + (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
             𝐓₅ = 𝐓₅ + (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝚺ₙ⁻¹ * (∂𝐞ₖ𝚺ₙ) * 𝚺ₙ⁻¹ * 𝐔ₙ' * 𝛉 ⊗ 𝐈ₗ) * 𝛉 * 𝐞ₖ'
         end
         𝐓₃ =  (𝛉' * 𝐔ₙ * 𝚺ₙ⁻¹ ⊗ 𝐈ₗ) * 𝐁ₙ * (𝐈ₘ ⊗ 𝛉) * 𝚺ₙ⁻¹ * 𝐔ₙ'
         𝐓 = 𝐓 + 𝐓₁ + 𝐓₂ + 𝐓₃ - 𝐓₄ - 𝐓₅
-
     end
     𝐓
 end
 
-
 @inline function Iₘ(entity::FundamentalMatrix)
-     Matrix{Float64}(I, 1, 1)
+     SMatrix{1,1}(1.0I)
 end
diff --git a/src/data_normalization/hartley_transformation.jl b/src/data_normalization/hartley_transformation.jl
index 6b202b9..aaf80cd 100644
--- a/src/data_normalization/hartley_transformation.jl
+++ b/src/data_normalization/hartley_transformation.jl
@@ -48,26 +48,25 @@ function hartley_transformation(ℳ::Vector{T})::SMatrix where T <:AbstractArray
         throw(ArgumentError("Array cannot be empty."))
     end
     npts = length(ℳ)
-    ndim = length(ℳ[1])-1
+    ndim = length(ℳ[1])
     𝐜 = centroid(ℳ)
     σ = root_mean_square(ℳ, 𝐜)
-    σ⁻¹ = 1 ./ σ
-    𝐓 = SMatrix{ndim+1,ndim+1,Float64, (ndim+1)^2}([σ⁻¹*Matrix{Float64}(I,ndim,ndim) -σ⁻¹*𝐜[1:end-1] ; zeros(1,ndim) 1.0])
+    σ⁻¹ = 1 / σ
+    𝐓 = SMatrix{ndim+1,ndim+1,Float64, (ndim+1)^2}([σ⁻¹*Matrix{Float64}(I,ndim,ndim) -σ⁻¹*𝐜 ; zeros(1,ndim) 1.0])
 end
 
 function centroid(positions::Vector{T}) where T <: AbstractArray
     x = zeros(T)
     for pos ∈ positions
-        x .= (+).(x, pos)
+        x = x + pos
     end
-    x .= (/).(x,length(positions))
-    return x
+    return x / length(positions)
 end
 
 function root_mean_square(ℳ::Vector{T}, 𝐜::T ) where  T <: AbstractArray
     total = 0.0
     npts = length(ℳ)
-    ndim = length(ℳ[1])-1
+    ndim = length(ℳ[1])
     for 𝐦 ∈ ℳ
          total  = total + ∑((𝐦-𝐜).^2)
     end
@@ -126,7 +125,7 @@ end
 function hartley_normalization!(ℳ::Vector{<:AbstractArray})
     𝐓 = hartley_transformation(ℳ)
     map!(ℳ , ℳ) do 𝐦
-         𝐓 * 𝐦
+         hom⁻¹(𝐓 * hom(𝐦))
     end
      ℳ, 𝐓
 end
diff --git a/src/estimate/estimate_twoview.jl b/src/estimate/estimate_twoview.jl
index 2af1a3b..7c286d9 100644
--- a/src/estimate/estimate_twoview.jl
+++ b/src/estimate/estimate_twoview.jl
@@ -10,7 +10,7 @@ function estimate(entity::FundamentalMatrix, method::DirectLinearTransform, 𝒟
     λ, f = smallest_eigenpair(Symmetric(𝐀))
     𝐅 = reshape(f,(3,3))
     𝐅 = enforce_ranktwo!(Array(𝐅))
-    𝐅 = 𝐅 / norm(𝐅)
+    𝐅 = SMatrix{3,3,Float64,9}(𝐅 / norm(𝐅))
     # Transform estimate back to the original (unnormalised) coordinate system.
     𝐓ʹ'*𝐅*𝐓
 end
@@ -58,6 +58,7 @@ function estimate(entity::FundamentalMatrix, method::FundamentalNumericalScheme,
     end
     𝐅 = reshape(𝛉,(3,3))
     𝐅 = enforce_ranktwo!(Array(𝐅))
+    𝐅 = SMatrix{3,3,Float64,9}(𝐅)
     # Transform estimate back to the original (unnormalised) coordinate system.
     𝐅 = 𝐓ʹ'*𝐅*𝐓
 end
@@ -68,7 +69,7 @@ function estimate(entity::FundamentalMatrix, method::BundleAdjustment,  𝒟::Tu
     if (N != length(ℳʹ))
           throw(ArgumentError("There should be an equal number of points for each view."))
     end
-    𝐅 = reshape(method.𝛉₀,(3,3))
+    𝐅 = SMatrix{3,3,Float64,9}(reshape(method.𝛉₀,(3,3)))
     𝒳 = triangulate(DirectLinearTransform(),𝐅,(ℳ,ℳʹ))
 
     𝐏₁, 𝐏₂ = construct(ProjectionMatrix(),𝐅)
diff --git a/src/moments/moments_fundamentalmatrix.jl b/src/moments/moments_fundamentalmatrix.jl
index af0b961..919df10 100644
--- a/src/moments/moments_fundamentalmatrix.jl
+++ b/src/moments/moments_fundamentalmatrix.jl
@@ -7,8 +7,8 @@ function moments(entity::FundamentalMatrix, 𝒟::Tuple{AbstractArray, Vararg{Ab
     end
     𝐀 =  @SMatrix zeros(9,9)
     for n = 1:N
-        𝐦  = 𝑛(ℳ[n])
-        𝐦ʹ = 𝑛(ℳʹ[n])
+        𝐦  = hom(ℳ[n])
+        𝐦ʹ = hom(ℳʹ[n])
         𝐀 = 𝐀 + (𝐦*𝐦') ⊗ (𝐦ʹ*𝐦ʹ')
     end
     𝐀/N
diff --git a/src/operators/operators.jl b/src/operators/operators.jl
index c983bb6..4e41016 100644
--- a/src/operators/operators.jl
+++ b/src/operators/operators.jl
@@ -45,7 +45,11 @@ function 𝑛(v::SVector)
 end
 
 function hom⁻¹(v::SVector)
-    pop(v / v[end])
+    if isapprox(v[end], 0.0; atol = 1e-14)
+        pop(v)
+    else
+        pop(v / v[end])
+    end
 end
 
 function hom(v::SVector)
diff --git a/src/projection/project.jl b/src/projection/project.jl
index 871fd53..8b3f5bf 100644
--- a/src/projection/project.jl
+++ b/src/projection/project.jl
@@ -5,6 +5,6 @@ function project(e::Pinhole, 𝐏::AbstractArray, 𝒳::Vector{<:AbstractArray})
         throw(ArgumentError("Expect 3 x 4 projection matrix."))
     end
     ℳ = map(𝒳) do 𝐗
-        𝐦 = 𝑛(Point2DH(𝐏 * 𝐗))
+        𝐦 = hom⁻¹(𝐏 * hom(𝐗))
     end
 end
diff --git a/src/triangulation/triangulate.jl b/src/triangulation/triangulate.jl
index d869ce9..8e4da0d 100644
--- a/src/triangulation/triangulate.jl
+++ b/src/triangulation/triangulate.jl
@@ -2,14 +2,19 @@ function triangulate(method::DirectLinearTransform, 𝐅::AbstractArray, 𝒟::T
     ℳ, ℳʹ =  𝒟
     𝐏₁, 𝐏₂ = construct(ProjectionMatrix(),𝐅)
     N = length(ℳ)
-    𝒴 = Array{Point3DH}(undef,N)
+    𝒴 = Array{Point3D}(undef,N)
     for n = 1:N
         𝐦 = ℳ[n]
         𝐦ʹ = ℳʹ[n]
-        𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
-              (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
-              (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
-              (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
+        eq1 = 𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:]
+        eq2 = 𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:]
+        eq3 = 𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:]
+        eq4 = 𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:]
+        𝐀 = SMatrix{4,4}(transpose(hcat(eq1,eq2,eq3,eq4)))
+        # 𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
+        #       (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
+        #       (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
+        #       (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
         # 𝐀₁ = vec2antisym(𝐦)*𝐏₁
         # 𝐀₂ = vec2antisym(𝐦ʹ)*𝐏₂
         # @show typeof(𝐀₁)
@@ -20,9 +25,8 @@ function triangulate(method::DirectLinearTransform, 𝐅::AbstractArray, 𝒟::T
         # λ, f = smallest_eigenpair(Symmetric(Array(𝐀'*𝐀)))
         # @show λ
         # 𝒴[n] = Point3DH(𝑛(f))
-
-        U,S,V = svd(Array(𝐀))
-        𝒴[n] = Point3DH(𝑛(V[:,4]))
+        U,S,V = svd(𝐀)
+        𝒴[n] = hom⁻¹(V[:,4])
     end
     𝒴
 end
@@ -30,14 +34,20 @@ end
 function triangulate(method::DirectLinearTransform, 𝐏₁::AbstractArray, 𝐏₂::AbstractArray, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
     ℳ, ℳʹ =  𝒟
     N = length(ℳ)
-    𝒴 = Array{Point3DH}(undef,N)
+    𝒴 = Array{Point3D}(undef,N)
     for n = 1:N
         𝐦 = ℳ[n]
         𝐦ʹ = ℳʹ[n]
-        𝐀 = [ (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
-              (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
-              (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
-              (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
+        eq1 = 𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:]
+        eq2 = 𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:]
+        eq3 = 𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:]
+        eq4 = 𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:]
+        𝐀 = SMatrix{4,4}(transpose(hcat(eq1,eq2,eq3,eq4)))
+        # 𝐀 = [  (𝐦[1] * 𝐏₁[3,:] - 𝐏₁[1,:])' ;
+        #         (𝐦[2] * 𝐏₁[3,:] - 𝐏₁[2,:])' ;
+        #         (𝐦ʹ[1] * 𝐏₂[3,:] - 𝐏₂[1,:])' ;
+        #         (𝐦ʹ[2] * 𝐏₂[3,:] - 𝐏₂[2,:])' ]
+
         # 𝐀₁ = vec2antisym(𝐦)*𝐏₁
         # 𝐀₂ = vec2antisym(𝐦ʹ)*𝐏₂
         # @show typeof(𝐀₁)
@@ -48,9 +58,8 @@ function triangulate(method::DirectLinearTransform, 𝐏₁::AbstractArray, 𝐏
         # λ, f = smallest_eigenpair(Symmetric(Array(𝐀'*𝐀)))
         # @show λ
         # 𝒴[n] = Point3DH(𝑛(f))
-
-        U,S,V = svd(Array(𝐀))
-        𝒴[n] = Point3DH(𝑛(V[:,4]))
+        U,S,V = svd(𝐀)
+        𝒴[n] = hom⁻¹(V[:,4])
     end
     𝒴
 end
diff --git a/src/types/ModuleTypes.jl b/src/types/ModuleTypes.jl
index 6bcc530..6200253 100644
--- a/src/types/ModuleTypes.jl
+++ b/src/types/ModuleTypes.jl
@@ -9,7 +9,7 @@ export BundleAdjustment
 export CostFunction, ApproximateMaximumLikelihood, AML
 export CoordinateSystemTransformation, CanonicalToHartley, HartleyToCanonical
 export CovarianceMatrices
-export Point2DH, Point3DH
+export Point2D, Point2DH, Point3D, Point3DH
 export HessianApproximation, CanonicalApproximation, CovarianceEstimationScheme
 export NoiseModel, GaussianNoise
 include("types.jl")
diff --git a/src/types/types.jl b/src/types/types.jl
index 2ad2e56..668762d 100644
--- a/src/types/types.jl
+++ b/src/types/types.jl
@@ -14,8 +14,11 @@
 #const Point2DH = MMatrix{3,1,Float64,3}
 #const Point3DH = MMatrix{4,1,Float64,4}
 
-const Point2DH = MVector{3,Float64}
-const Point3DH = MVector{4,Float64}
+const Point2DH = SVector{3,Float64}
+const Point3DH = SVector{4,Float64}
+
+const Point2D = SVector{2,Float64}
+const Point3D = SVector{3,Float64}
 
 # mutable struct Point2DH <: MMatrix{3,1,Float64,3}
 #     x::Float64
@@ -71,15 +74,15 @@ end
 mutable struct DirectLinearTransform <: EstimationAlgorithm
 end
 
-mutable struct BundleAdjustment <: EstimationAlgorithm
-    𝛉₀::Matrix{Float64}
-    max_iter::Int8
+mutable struct BundleAdjustment{A<:AbstractVector} <: EstimationAlgorithm
+    𝛉₀::A
+    max_iter::Int64
     toleranceθ::Float64
 end
 
-mutable struct FundamentalNumericalScheme <: EstimationAlgorithm
-    𝛉₀::Matrix{Float64}
-    max_iter::Int8
+mutable struct FundamentalNumericalScheme{A<:AbstractVector} <: EstimationAlgorithm
+    𝛉₀::A
+    max_iter::Int64
     toleranceθ::Float64
 end
 
diff --git a/test/construct_projectionmatrix_tests.jl b/test/construct_projectionmatrix_tests.jl
index 8dc4537..d5739fd 100644
--- a/test/construct_projectionmatrix_tests.jl
+++ b/test/construct_projectionmatrix_tests.jl
@@ -8,7 +8,7 @@ using MultipleViewGeometry.ModuleTypes
 @test construct(ProjectionMatrix(),𝐊,𝐑,𝐭) == [Matrix{Float64}(I, 3, 3) -ones(3)]
 
 # 1. Construct a Fundamental matrix from Camera matrices.
-# 2. Construct projection matrices from the Fundamental matrix.
+# 2. Construct Projection matrices from the Fundamental matrix.
 # 3. Construct a Fundamental matrix from the projection matrices.
 # 4. The Fundamental matrices in step 2 and 3 should be equivalent up to sign
 #    and scale.
diff --git a/test/cost_functions_tests.jl b/test/cost_functions_tests.jl
index f1e0403..4fd1f46 100644
--- a/test/cost_functions_tests.jl
+++ b/test/cost_functions_tests.jl
@@ -8,8 +8,7 @@ using StaticArrays
 
 # 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]
+𝒳 = [Point3D(x,y,z) 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{3,3}(Matrix{Float64}(I, 3, 3))
@@ -34,7 +33,7 @@ using StaticArrays
 # Ensure the estimated and true matrix have the same scale and sign.
 𝐅 = 𝐅 / norm(𝐅)
 𝐅 = 𝐅 / sign(𝐅[1,3])
-𝐟 = reshape(𝐅,9,1)
+𝐟 = vec(𝐅)
 
 Λ₁ =  [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(ℳ)]
@@ -43,14 +42,14 @@ Jₐₘₗ =  cost(AML(),FundamentalMatrix(), SVector{9}(𝐟), (Λ₁,Λ₂), (
 @test isapprox(Jₐₘₗ, 0.0; atol = 1e-14)
 
 # Verify that the vectorised fundamental matrix is in the null space of X
-𝐗 = X(AML(),FundamentalMatrix(), reshape(𝐅,9,1), (Λ₁,Λ₂), (ℳ, ℳʹ))
+𝐗 = 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))
 
 # 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), (Λ₁,Λ₂), (ℳ, ℳʹ))
+𝐇 = 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))
diff --git a/test/data_normalization_tests.jl b/test/data_normalization_tests.jl
index 7c16025..916c049 100644
--- a/test/data_normalization_tests.jl
+++ b/test/data_normalization_tests.jl
@@ -3,53 +3,53 @@ using StaticArrays, LinearAlgebra
 
 # Tests for a set of two-dimensional Cartesian points represented by homogeneous
 # coordinates.
-ℳ = map(Point2DH,
-        [(-10.0, -10.0, 1.0),
-         (-10.0,  10.0, 1.0),
-         ( 10.0, -10.0, 1.0),
-         ( 10.0,  10.0, 1.0)])
+ℳ = map(Point2D, [(-10.0, -10.0),
+                   (-10.0,  10.0),
+                   ( 10.0, -10.0),
+                   ( 10.0,  10.0)])
 
 ℳʹ, 𝐓 = hartley_normalization(ℳ)
-@test ℳʹ == map(Point2DH,
-                                        [(-1.0,-1.0, 1.0),
-                                         (-1.0, 1.0, 1.0),
-                                         (1.0, -1.0, 1.0),
-                                         (1.0,  1.0, 1.0)])
+
+@test ℳʹ == map(Point2D,  [(-1.0,-1.0),
+                            (-1.0, 1.0),
+                            (1.0, -1.0),
+                            (1.0,  1.0,)])
+
  @test 𝐓 == [0.1 0.0 -0.0;
              0.0 0.1 -0.0;
              0.0 0.0 1.0]
  @test hartley_transformation(ℳ) == [0.1 0.0 -0.0;
-                                         0.0 0.1 -0.0;
-                                         0.0 0.0 1.0]
+                                      0.0 0.1 -0.0;
+                                      0.0 0.0 1.0]
 
 
 # Tests for a set of three-dimensional Cartesian points represented by homogeneous
 # coordinates.
-ℳ = map(Point3DH,
-           [(-10.0, -10.0, -10.0, 1.0),
-            (-10.0, -10.0,  10.0, 1.0),
-            (-10.0,  10.0, -10.0, 1.0),
-            (-10.0,  10.0,  10.0, 1.0),
-            ( 10.0, -10.0, -10.0, 1.0),
-            ( 10.0, -10.0,  10.0, 1.0),
-            ( 10.0,  10.0, -10.0, 1.0),
-            ( 10.0,  10.0,  10.0, 1.0)])
+ℳ = map(Point3D,
+           [(-10.0, -10.0, -10.0),
+            (-10.0, -10.0,  10.0),
+            (-10.0,  10.0, -10.0),
+            (-10.0,  10.0,  10.0),
+            ( 10.0, -10.0, -10.0),
+            ( 10.0, -10.0,  10.0),
+            ( 10.0,  10.0, -10.0),
+            ( 10.0,  10.0,  10.0)])
 
 ℳʹ, 𝐓 = hartley_normalization(ℳ)
-@test ℳʹ == map(Point3DH,
-                                         [(-1.0,-1.0, -1.0, 1.0),
-                                          (-1.0,-1.0,  1.0, 1.0),
-                                          (-1.0, 1.0, -1.0, 1.0),
-                                          (-1.0, 1.0,  1.0, 1.0),
-                                          (1.0, -1.0, -1.0, 1.0),
-                                          (1.0, -1.0,  1.0, 1.0),
-                                          (1.0,  1.0, -1.0, 1.0),
-                                          (1.0,  1.0,  1.0, 1.0)])
+@test ℳʹ == map(Point3D,  [(-1.0,-1.0, -1.0),
+                            (-1.0,-1.0,  1.0),
+                            (-1.0, 1.0, -1.0),
+                            (-1.0, 1.0,  1.0),
+                            (1.0, -1.0, -1.0),
+                            (1.0, -1.0,  1.0),
+                            (1.0,  1.0, -1.0),
+                            (1.0,  1.0,  1.0)])
+
 @test 𝐓 == [0.1 0.0 0.0 -0.0;
             0.0 0.1 0.0 -0.0;
             0.0 0.0 0.1 -0.0;
             0.0 0.0 0.0 1.0]
-@test hartley_transformation(ℳ) == [0.1 0.0 0.0 -0.0;
-                                        0.0 0.1 0.0 -0.0;
-                                        0.0 0.0 0.1 -0.0;
-                                        0.0 0.0 0.0 1.0]
+@test hartley_transformation(ℳ) ==  [0.1 0.0 0.0 -0.0;
+                                      0.0 0.1 0.0 -0.0;
+                                      0.0 0.0 0.1 -0.0;
+                                      0.0 0.0 0.0 1.0]
diff --git a/test/estimate_twoview_tests.jl b/test/estimate_twoview_tests.jl
index 9e383c4..0132d1c 100644
--- a/test/estimate_twoview_tests.jl
+++ b/test/estimate_twoview_tests.jl
@@ -4,7 +4,7 @@ using StaticArrays, Calculus
 # Tests for fundamental matrix estimation
 
 
-𝒳 = [Point3DH(x,y,z,1.0)
+𝒳 = [Point3D(x,y,z)
                         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.
@@ -42,19 +42,19 @@ npts = length(ℳ)
 residual = zeros(Float64,npts,1)
 for correspondence in zip(1:length(ℳ),ℳ, ℳʹ)
     i, m , mʹ = correspondence
-    𝐦  = 𝑛(m)
-    𝐦ʹ = 𝑛(mʹ)
+    𝐦  = 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(ℳ)]sum(residual)
+Λ₁ =  [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(reshape(𝐅₀,9,1), 5, 1e-10),
+                        FundamentalNumericalScheme(vec(𝐅₀), 5, 1e-10),
                                                           (Λ₁,Λ₂), (ℳ, ℳʹ))
 
 𝐅ₜ = construct(FundamentalMatrix(),𝐊₁,𝐑₁,𝐭₁,𝐊₂,𝐑₂,𝐭₂)
@@ -83,7 +83,7 @@ end
 
 # Test the Bundle Adjustment estimator on the Fundamental matrix problem.
 𝐅, lsqFit = estimate(FundamentalMatrix(),
-                        BundleAdjustment(reshape(𝐅₀,9,1), 5, 1e-10),
+                        BundleAdjustment(vec(𝐅₀), 5, 1e-10),
                                                            (ℳ, ℳʹ))
 𝐅 = 𝐅 / norm(𝐅)
 𝐅 = 𝐅 / sign(𝐅[1,2])
diff --git a/test/satisfy_epipolar_constraints_tests.jl b/test/satisfy_epipolar_constraints_tests.jl
index 5addae3..56c50fc 100644
--- a/test/satisfy_epipolar_constraints_tests.jl
+++ b/test/satisfy_epipolar_constraints_tests.jl
@@ -11,9 +11,7 @@ Random.seed!(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]
+𝒳 = [Point3D(x,y,z) 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{3,3}(1.0I)
@@ -37,21 +35,21 @@ Random.seed!(1234)
 
 # Verify that the algorithm returns the correct answer when the
 # constraint is already satisfied.
-𝒪 ,𝒪ʹ = satisfy(FundamentalMatrix(), EpipolarConstraint(), 𝐅, (ℳ, ℳʹ))
+𝒪,𝒪ʹ = satisfy(FundamentalMatrix(), EpipolarConstraint(), 𝐅, (ℳ, ℳʹ))
 
 # Verify that the original corresponding points satisfy the epipolar constraint.
 N = length(ℳ)
 for n = 1:N
-    𝐦 = ℳ[n]
-    𝐦ʹ = ℳʹ[n]
+    𝐦 = hom(ℳ[n])
+    𝐦ʹ = hom(ℳʹ[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]
+    𝐦 = hom(𝒪[n])
+    𝐦ʹ = hom(𝒪ʹ[n])
     @test  isapprox(𝐦'*𝐅*𝐦ʹ, 0.0; atol = 1e-14)
 end
 
@@ -60,10 +58,10 @@ end
 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]
+    ℳ[n] = ℳ[n] + SVector{2}(σ * rand(2,1))
+    ℳʹ[n] = ℳʹ[n] + SVector{2}(σ * rand(2,1))
+    𝐦 = hom(ℳ[n])
+    𝐦ʹ = hom(ℳʹ[n])
     @test abs(𝐦'*𝐅*𝐦ʹ) > 1e-12
 end
 
@@ -75,12 +73,7 @@ end
 # Verify that the 'corrected' points satisfy the epipolar constraint.
 N = length(ℳ)
 for n = 1:N
-    𝐦 = 𝒪[n]
-    𝐦ʹ = 𝒪ʹ[n]
+    𝐦 = hom(𝒪[n])
+    𝐦ʹ = hom(𝒪ʹ[n])
     @test  isapprox(𝐦'*𝐅*𝐦ʹ, 0.0; atol = 1e-14)
 end
-
-# σ = 1e-7
-# SVector{3}(σ * vcat(rand(2,1),0))
-#
-# ℳ[1] - 𝒪[1]
diff --git a/test/transform_tests.jl b/test/transform_tests.jl
index e169f6e..23e7fc9 100644
--- a/test/transform_tests.jl
+++ b/test/transform_tests.jl
@@ -3,18 +3,18 @@ using StaticArrays, LinearAlgebra
 
 # Tests for a set of two-dimensional Cartesian points represented by homogeneous
 # coordinates.
-ℳ = map(Point2DH,
-        [(-10.0, -10.0, 1.0),
-         (-10.0,  10.0, 1.0),
-         ( 10.0, -10.0, 1.0),
-         ( 10.0,  10.0, 1.0)])
+ℳ = map(Point2D,
+        [(-10.0, -10.0),
+         (-10.0,  10.0),
+         ( 10.0, -10.0),
+         ( 10.0,  10.0)])
 
 ℳʹ, 𝐓 = transform(HomogeneousCoordinates(),CanonicalToHartley(),ℳ)
-@test ℳʹ == map(Point2DH,
-                                        [(-1.0,-1.0, 1.0),
-                                         (-1.0, 1.0, 1.0),
-                                         (1.0, -1.0, 1.0),
-                                         (1.0,  1.0, 1.0)])
+@test ℳʹ == map(Point2D,
+                                        [(-1.0,-1.0),
+                                         (-1.0, 1.0),
+                                         (1.0, -1.0),
+                                         (1.0,  1.0)])
 
 Λ =  [MMatrix{3,3}(Matrix(Diagonal([1.0,1.0,0.0]))) for i = 1:length(ℳ)]
 Λʹ = transform(CovarianceMatrices(), CanonicalToHartley(), Λ , 𝐓)
@@ -28,26 +28,26 @@ end
 
 # Tests for a set of three-dimensional Cartesian points represented by homogeneous
 # coordinates.
-ℳ = map(Point3DH,
-           [(-10.0, -10.0, -10.0, 1.0),
-            (-10.0, -10.0,  10.0, 1.0),
-            (-10.0,  10.0, -10.0, 1.0),
-            (-10.0,  10.0,  10.0, 1.0),
-            ( 10.0, -10.0, -10.0, 1.0),
-            ( 10.0, -10.0,  10.0, 1.0),
-            ( 10.0,  10.0, -10.0, 1.0),
-            ( 10.0,  10.0,  10.0, 1.0)])
+ℳ = map(Point3D,
+           [(-10.0, -10.0, -10.0),
+            (-10.0, -10.0,  10.0),
+            (-10.0,  10.0, -10.0),
+            (-10.0,  10.0,  10.0),
+            ( 10.0, -10.0, -10.0),
+            ( 10.0, -10.0,  10.0),
+            ( 10.0,  10.0, -10.0),
+            ( 10.0,  10.0,  10.0)])
 
 ℳʹ, 𝐓 = transform(HomogeneousCoordinates(),CanonicalToHartley(),ℳ)
-@test ℳʹ == map(Point3DH,
-                                         [(-1.0,-1.0, -1.0, 1.0),
-                                          (-1.0,-1.0,  1.0, 1.0),
-                                          (-1.0, 1.0, -1.0, 1.0),
-                                          (-1.0, 1.0,  1.0, 1.0),
-                                          (1.0, -1.0, -1.0, 1.0),
-                                          (1.0, -1.0,  1.0, 1.0),
-                                          (1.0,  1.0, -1.0, 1.0),
-                                          (1.0,  1.0,  1.0, 1.0)])
+@test ℳʹ == map(Point3D,
+                                         [(-1.0,-1.0, -1.0),
+                                          (-1.0,-1.0,  1.0),
+                                          (-1.0, 1.0, -1.0),
+                                          (-1.0, 1.0,  1.0),
+                                          (1.0, -1.0, -1.0),
+                                          (1.0, -1.0,  1.0),
+                                          (1.0,  1.0, -1.0),
+                                          (1.0,  1.0,  1.0)])
 @test 𝐓 == [0.1 0.0 0.0 -0.0;
             0.0 0.1 0.0 -0.0;
             0.0 0.0 0.1 -0.0;
diff --git a/test/triangulate_tests.jl b/test/triangulate_tests.jl
index 04760b8..75b9be1 100644
--- a/test/triangulate_tests.jl
+++ b/test/triangulate_tests.jl
@@ -9,8 +9,7 @@ using StaticArrays
 # Fix random seed.
 Random.seed!(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]
+𝒳 = [Point3D(x,y,z) 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{3,3}(1.0I)
@@ -28,7 +27,7 @@ Random.seed!(1234)
 
 # Set of corresponding points.
 ℳ = project(Pinhole(),𝐏₁,𝒳)
-ℳʹ = project(Pinhole(),𝐏₂,𝒳)
+ℳʹ= project(Pinhole(),𝐏₂,𝒳)
 
 𝒴 = triangulate(DirectLinearTransform(),𝐏₁,𝐏₂,(ℳ,ℳʹ))
 
@@ -36,7 +35,7 @@ Random.seed!(1234)
 # (ℳ,ℳʹ) 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)
+    @test  isapprox(sum(abs.(𝒳[n]-𝒴[n])/3), 0.0; atol = 1e-12)
 end
 
 
@@ -56,7 +55,7 @@ end
 𝒪ʹ= project(Pinhole(),𝐐₂,𝒴)
 N = length(𝒪)
 for n = 1:N
-    𝐦 = 𝒪[n]
-    𝐦ʹ = 𝒪ʹ[n]
+    𝐦 = hom(𝒪[n])
+    𝐦ʹ = hom(𝒪ʹ[n])
     @test  isapprox(𝐦'*𝐅*𝐦ʹ, 0.0; atol = 1e-14)
 end