Merge pull request #225 from JuliaArrays/cjf-matrix-inverse

Another try at 4x4 matrix inverses

Author: c42f

src/SUnitRange.jl

@@ -19,7 +19,7 @@ end
 @pure SUnitRange(a::Int, b::Int) = SUnitRange{a, max(0, b - a + 1)}()
 
 @pure @propagate_inbounds function getindex(x::SUnitRange{Start, L}, i::Int) where {Start, L}
-    @boundscheck if i < Start || i >= (Start + L)
+    @boundscheck if i < 1 || i > L
         throw(BoundsError(x, i))
     end
     return Start + i - 1

src/det.jl

@@ -24,12 +24,13 @@ end
     return vecdot(x0, cross(x1, x2))
 end
 
-@generated function _det{S,T}(::Size{S}, A::StaticMatrix{T})
+@generated function _det{S}(::Size{S}, A::StaticMatrix)
     if S[1] != S[2]
         throw(DimensionMismatch("matrix is not square"))
     end
     return quote # Implementation from Base
         @_inline_meta
+        T = eltype(A)
         T2 = typeof((one(T)*zero(T) + zero(T))/one(T))
         if istriu(A) || istril(A)
             return convert(T2, det(UpperTriangular(A))) # Is this a Julia bug that a convert is not type stable??

src/inv.jl

@@ -30,6 +30,34 @@ end
     @inbounds return newtype((y0[1], y1[1], y2[1], y0[2], y1[2], y2[2], y0[3], y1[3], y2[3]))
 end
 
+Base.@pure function splitrange(r::SUnitRange)
+    mid = (first(r) + last(r)) ÷ 2
+    (SUnitRange(first(r), mid), SUnitRange(mid+1, last(r)))
+end
+
+@noinline function _inv(::Size{(4,4)}, A)
+    # Partition matrix into four 2x2 blocks.  For 4x4 matrices this seems to be
+    # more stable than directly using the adjugate expansion.
+    # See http://www.freevec.org/function/inverse_matrix_4x4_using_partitioning
+    #
+    # TODO: This decomposition works in higher dimensions, but numerical
+    # stability doesn't seem good for badly conditioned matrices.  Can be
+    # fixed?
+    (i1,i2) = splitrange(SUnitRange(1,4))
+    @inbounds P = A[i1,i1]
+    @inbounds Q = A[i1,i2]
+    @inbounds R = A[i2,i1]
+    @inbounds S = A[i2,i2]
+    invP = inv(P)
+    invP_Q = invP*Q
+    S2 = inv(S - R*invP_Q)
+    R2 = -S2*(R*invP)
+    Q2 = -invP_Q*S2
+    P2 = invP - invP_Q*R2
+    [[P2 Q2];
+     [R2 S2]]
+end
+
 @inline function _inv(::Size, A)
     T = eltype(A)
     S = typeof((one(T)*zero(T) + zero(T))/one(T))

test/SUnitRange.jl

@@ -2,9 +2,12 @@
     @test length(StaticArrays.SUnitRange(1,3)) === 3
     @test length(StaticArrays.SUnitRange(1,-10)) === 0
 
+    @test_throws BoundsError StaticArrays.SUnitRange(2,4)[0]
+    @test StaticArrays.SUnitRange(2,4)[1] === 2
     @test StaticArrays.SUnitRange(2,4)[2] === 3
-    
-    @test_throws BoundsError StaticArrays.SUnitRange(1,3)[4]
+    @test StaticArrays.SUnitRange(2,4)[3] === 4
+    @test_throws BoundsError StaticArrays.SUnitRange(2,4)[4]
+
     @test_throws Exception StaticArrays.SUnitRange{1, -1}()
     @test_throws TypeError StaticArrays.SUnitRange{1, 1.5}()
 end

test/det.jl

@@ -7,6 +7,7 @@
     #triu/tril
     @test det(@SMatrix [1 2; 0 3]) === 3
     @test det(@SMatrix [1 2 3 4; 0 5 6 7; 0 0 8 9; 0 0 0 10]) === 400.0
+    @test @inferred(det(ones(SMatrix{10,10,Complex{Float64}}))) == 0
 
     # Unsigned specializations
     @test det(@SMatrix [0x00 0x01; 0x01 0x00])::Int8 == -1

test/inv.jl

@@ -1,15 +1,125 @@
+using StaticArrays, Base.Test
+
+"""
+Create an almost singular `Matrix{Float64}` of size `N×N`.
+
+The first `rank` columns are chosen randomly, followed by `N-rank` linear
+combinations with random weights to create an `N×N` singular matrix `S`.
+A noise matrix of norm `ϵ*norm(S)` is added.
+
+The condition number of this matrix should be approximately ϵ/eps(Float64)
+"""
+function almost_singular_matrix(N, rank, ϵ)
+    B = rand(N,rank)
+    weights = rand(rank, N-rank) ./ rank
+    S = [B B*weights]
+    S = S ./ norm(S)
+    noise = rand(N,N)
+    S + ϵ*norm(S)/norm(noise) * noise
+end
+
 @testset "Matrix inverse" begin
     @test inv(@SMatrix [2])::SMatrix ≈ @SMatrix [0.5]
     @test inv(@SMatrix [1 2; 2 1])::SMatrix ≈ [-1/3 2/3; 2/3 -1/3]
     @test inv(@SMatrix [1 2 0; 2 1 0; 0 0 1])::SMatrix ≈ [-1/3 2/3 0; 2/3 -1/3 0; 0 0 1]
-    m = randn(Float64, 4,4) + eye(4) # well conditioned
-    @test inv(SMatrix{4,4}(m))::StaticMatrix ≈ inv(m)
 
+    m = randn(Float64, 10,10) + eye(10) # well conditioned
+    @test inv(SMatrix{10,10}(m))::StaticMatrix ≈ inv(m)
+
+
+    # Unsigned versions
     @test inv(@SMatrix [0x01 0x02; 0x02 0x01])::SMatrix ≈ [-1/3 2/3; 2/3 -1/3]
     @test inv(@SMatrix [0x01 0x02 0x00; 0x02 0x01 0x00; 0x00 0x00 0x01])::SMatrix ≈ [-1/3 2/3 0; 2/3 -1/3 0; 0 0 1]
-    
+
     m = triu(randn(Float64, 4,4) + eye(4))
     @test inv(SMatrix{4,4}(m))::StaticMatrix ≈ inv(m)
     m = tril(randn(Float64, 4,4) + eye(4))
     @test inv(SMatrix{4,4}(m))::StaticMatrix ≈ inv(m)
 end
+
+
+@testset "Matrix inverse 4x4" begin
+    # A random well-conditioned matrix generated with `rand(1:10,4,4)`
+    m = Float64[8 4 3 1;
+                9 1 9 8;
+                5 0 2 1;
+                7 5 7 2]
+    sm = SMatrix{4,4}(m)
+    @test isapprox(inv(sm)::StaticMatrix, inv(m), rtol=2e-15)
+
+    # Poorly conditioned matrix; almost_singular_matrix(4, 3, 1e-7)
+    m = [
+        2.83056817904263402e-01 1.26822318692296848e-01 2.59665505365002547e-01 1.24524798964590747e-01
+        4.29869029098094768e-01 2.29977256378012789e-03 4.63841570354149135e-01 1.51975221831027241e-01
+        2.33883312850939995e-01 7.69469213907006122e-02 3.16525493661056589e-01 1.06181613512460249e-01
+        1.63327582123091175e-01 1.70552365412100865e-01 4.55720450934200327e-01 1.23299968650232419e-01
+    ]
+    sm = SMatrix{4,4}(m)
+    @test norm(Matrix(sm*inv(sm) - eye(4))) < 10*norm(m*inv(m) - eye(4))
+    @test norm(Matrix(inv(sm)*sm - eye(4))) < 10*norm(inv(m)*m - eye(4))
+
+    # Poorly conditioned matrix, generated by fuzz testing with
+    # almost_singular_matrix(N, 3, 1e-7)
+    # Case where the 4x4 algorithm fares badly compared to Base inv
+    m = [1.80589503332920231e-02 2.37595944073211218e-01 3.80770755039433806e-01 5.70474286930408372e-02
+         2.17494123374967520e-02 2.94069278741667661e-01 2.57334651147732463e-01 4.02348559939922357e-02
+         3.42945636322714076e-01 3.29329508494837497e-01 2.25635541033857107e-01 6.03912636987153917e-02
+         4.77828437366344727e-01 4.86406974015710591e-01 1.95415684569693188e-01 6.74775080892497797e-02]
+    sm = SMatrix{4,4}(m)
+    @test_broken norm(Matrix(inv(sm)*sm - eye(4))) < 10*norm(inv(m)*m - eye(4))
+    @test_broken norm(Matrix(sm*inv(sm) - eye(4))) < 10*norm(m*inv(m) - eye(4))
+end
+
+
+#-------------------------------------------------------------------------------
+# More comprehensive but qualitiative testing for inv() accuracy
+#=
+using PyPlot
+
+inv_residual(A::AbstractMatrix) = norm(A*inv(A) - eye(size(A,1)))
+
+"""
+    plot_residuals(N, rank, ϵ)
+
+Plot `inv_residual(::StaticMatrix)` vs `inv_residual(::Matrix)`
+
+"""
+function plot_residuals(N, rank, ϵ)
+    A_residuals = []
+    SA_residuals = []
+    for i=1:10000
+        A = almost_singular_matrix(N, rank, ϵ)
+        SA = SMatrix{N,N}(A)
+        SA_residual = norm(Matrix(SA*inv(SA) - eye(N)))
+        A_residual = norm(A*inv(A) - eye(N))
+        push!(SA_residuals, SA_residual)
+        push!(A_residuals, A_residual)
+        #= if SA_residual/A_residual > 10000 =#
+        #=     @printf("%10e %.4f\n", SA_residual, SA_residual/A_residual) =#
+        #=     println("[") =#
+        #=     for i=1:4 =#
+        #=         for j=1:4 =#
+        #=             @printf("%.17e ", A[i,j]) =#
+        #=         end =#
+        #=         println() =#
+        #=     end =#
+        #=     println("]") =#
+        #= end =#
+    end
+    loglog(A_residuals, SA_residuals, ".", markersize=1.5)
+end
+
+# Plot the accuracy of inv implementations for almost singular matrices of
+# various rank
+clf()
+N = 4
+title("inv() accuracy for poorly conditioned $(N)x$(N) - Base vs block decomposition")
+labels = []
+for i in N:-1:1
+    plot_residuals(N, i, 1e-7)
+    push!(labels, "rank $i")
+end
+xlabel("Residual norm: `inv(::Array)`")
+ylabel("Residual norm: `inv(::StaticArray)`")
+legend(labels)
+=#