Conversion and SE(3) tests

ext/LieGroupsRecursiveArrayToolsExt/special_euclidean_group_RAT_ext.jl

@@ -15,28 +15,47 @@ function Base.convert(::Type{SpecialEuclideanProductTangentVector}, X::ArrayPart
 end
 # convert between both representations
 function Base.convert(
-    ::Type{AbstractMatrix}, g::SpecialEuclideanProductPoint{<:ArrayPartition}
+    ::Type{<:AbstractMatrix}, g::SpecialEuclideanProductPoint{<:ArrayPartition}
 )
     n = size(g.value.x[1])[1]
-    A = zeros(n + 1, n + 1)
-    A[n + 1, n + 1] = 1.0
+    A = zeros((n + 1), (n + 1))
+    A[(n + 1), (n + 1)] = 1.0
     # We do not know whether g.value ir (R,t) or (t,R) so we have to depend on sizes
     s = size(g.value.x[1])
     if length(s) == 2 # (R,t)
         A[1:n, 1:n] .= g.value.x[1]
-        A[1:n, n + 1] .= g.value.x[2]
+        A[1:n, (n + 1)] .= g.value.x[2]
     else # format (t,R)
         A[1:n, 1:n] .= g.value.x[2]
-        A[1:n, n + 1] .= g.value.x[1]
+        A[1:n, (n + 1)] .= g.value.x[1]
     end
+    return A
+end
+function Base.convert(::Type{<:SpecialEuclideanProductPoint}, g::AbstractMatrix)
+    return SpecialEuclideanProductPoint(
+        convert(ArrayPartition, SpecialEuclideanMatrixPoint(g))
+    )
+end
+function Base.convert(::Type{<:SpecialEuclideanProductTangentVector}, X::AbstractMatrix)
+    return SpecialEuclideanProductTangentVector(
+        convert(ArrayPartition, SpecialEuclideanMatrixTangentVector(X))
+    )
+end
+
+function Base.convert(
+    ::Type{<:ArrayPartition}, g::SpecialEuclideanMatrixPoint{<:AbstractMatrix}
+)
+    A = g.value
+    n = size(A)[1] - 1
+    return ArrayPartition(A[1:n, 1:n], A[1:n, (n + 1)])
 end
 function Base.convert(
-    ::Type{AbstractMatrix}, X::SpecialEuclideanProductTangentVector{<:ArrayPartition}
+    ::Type{<:AbstractMatrix}, X::SpecialEuclideanProductTangentVector{<:ArrayPartition}
 )
     n = size(X.value.x[1])[1]
     A = zeros(n + 1, n + 1)
     A[n + 1, n + 1] = 0.0
-    # We do not know whether g.value ir (R,t) or (t,R) so we have to depend on sizes
+    # We do not know whether g.value is (R,t) or (t,R) so we have to depend on sizes
     s = size(X.value.x[1])
     if length(s) == 2 # (R,t)
         A[1:n, 1:n] .= X.value.x[1]
@@ -45,52 +64,65 @@ function Base.convert(
         A[1:n, 1:n] .= X.value.x[2]
         A[1:n, n + 1] .= X.value.x[1]
     end
+    return A
 end
 # The reverse is hence not unique, but we can still cast to the special matrix point
-function Base.convert(::Type{SpecialEuclideanMatrixPoint}, g::ArrayPartition)
-    n = size(g.x[1])[1]
-    A = zeros(n + 1, n + 1)
-    A[n + 1, n + 1] = 1.0
-    # We do not know whether g.value ir (R,t) or (t,R) so we have to depend on sizes
-    s = size(g.x[1])
-    if length(s) == 2 # (R,t)
-        A[1:n, 1:n] .= g.x[1]
-        A[1:n, n + 1] .= g.x[2]
-    else # format (t,R)
-        A[1:n, 1:n] .= g.x[2]
-        A[1:n, n + 1] .= g.x[1]
-    end
-    return SpecialEuclideanMatrixPoint(A)
+function Base.convert(::Type{T}, g::ArrayPartition) where {T<:SpecialEuclideanMatrixPoint}
+    return T(convert(AbstractMatrix, SpecialEuclideanProductPoint(g)))
 end
-function Base.convert(::Type{SpecialEuclideanMatrixTangentVector}, g::ArrayPartition)
-    n = size(g.x[1])[1]
-    A = zeros(n + 1, n + 1)
-    A[n + 1, n + 1] = 0.0
+function Base.convert(
+    ::Type{T}, g::ArrayPartition
+) where {T<:SpecialEuclideanMatrixTangentVector}
+    return T(convert(AbstractMatrix, SpecialEuclideanProductTangentVector(g)))
+end
+function Base.convert(
+    ::M, g::SpecialEuclideanProductPoint{<:ArrayPartition}
+) where {T,M<:AbstractMatrix{T}}
+    v = g.value.x
+    n = size(v[1])[1]
+    A = zeros(T, n + 1, n + 1)
     # We do not know whether g.value ir (R,t) or (t,R) so we have to depend on sizes
     s = size(g.x[1])
     if length(s) == 2 # (R,t)
-        A[1:n, 1:n] .= g.x[1]
-        A[1:n, n + 1] .= g.x[2]
+        A[1:n, 1:n] .= v[1]
+        A[1:n, n + 1] .= v[2]
     else # format (t,R)
-        A[1:n, 1:n] .= g.x[2]
-        A[1:n, n + 1] .= g.x[1]
+        A[1:n, 1:n] .= v[2]
+        A[1:n, n + 1] .= v[1]
     end
-    return SpecialEuclideanMatrixPoint(A)
+    return A
 end
 # convert between both representation explicitly
 # the inverse is again not unique since both (R,t) and (t,R) are possible results.
 function Base.convert(
-    ::Type{SpecialEuclideanMatrixPoint}, g::SpecialEuclideanProductPoint{<:ArrayPartition}
+    ::Type{<:SpecialEuclideanMatrixPoint}, g::SpecialEuclideanProductPoint{<:ArrayPartition}
 )
     return SpecialEuclideanMatrixPoint(convert(AbstractMatrix, g))
 end
+function Base.convert(
+    ::Type{<:SpecialEuclideanProductPoint}, g::SpecialEuclideanMatrixPoint{<:AbstractMatrix}
+)
+    return SpecialEuclideanProductPoint(convert(ArrayPartition, g))
+end
 function Base.convert(
     ::Type{SpecialEuclideanMatrixTangentVector},
     g::SpecialEuclideanProductTangentVector{<:ArrayPartition},
 )
     return SpecialEuclideanMatrixTangentVector(convert(AbstractMatrix, g))
 end
-
+function Base.convert(
+    ::Type{<:ArrayPartition}, g::SpecialEuclideanMatrixTangentVector{<:AbstractMatrix}
+)
+    A = g.value
+    n = size(A)[1] - 1
+    return ArrayPartition(A[1:n, 1:n], A[1:n, (n + 1)])
+end
+function Base.convert(
+    ::Type{<:SpecialEuclideanProductTangentVector},
+    g::SpecialEuclideanMatrixTangentVector{<:AbstractMatrix},
+)
+    return SpecialEuclideanProductTangentVector(convert(ArrayPartition, g))
+end
 #
 # Functions specialised from the interface
 #
@@ -160,7 +192,7 @@ function ManifoldsBase.log!(
     X::ArrayPartition,
     g::ArrayPartition,
 )
-    _log_SE3!(G, g, X)
+    LieGroups._log_SE3!(G, X, g)
     return X
 end
 

src/groups/special_euclidean_group.jl

@@ -326,11 +326,11 @@ function _compose!(
 end
 
 Base.convert(::Type{<:AbstractMatrix}, g::SpecialEuclideanMatrixPoint) = g.value
-function Base.convert(::Type{SpecialEuclideanProductPoint}, p::AbstractMatrix)
+function Base.convert(::Type{<:SpecialEuclideanMatrixPoint}, p::AbstractMatrix)
     return SpecialEuclideanMatrixPoint(p)
 end
 Base.convert(::Type{<:AbstractMatrix}, X::SpecialEuclideanMatrixTangentVector) = X.value
-function Base.convert(::Type{SpecialEuclideanMatrixTangentVector}, X::AbstractMatrix)
+function Base.convert(::Type{<:SpecialEuclideanMatrixTangentVector}, X::AbstractMatrix)
     return SpecialEuclideanMatrixTangentVector(X)
 end
 
@@ -696,7 +696,7 @@ function ManifoldsBase.log!(
     g::AbstractMatrix,
 )
     init_constants!(LieAlgebra(G), X)
-    _log_SE3!(G, g, X)
+    _log_SE3!(G, X, g)
     return X
 end
 function _log_SE3!(G::SpecialEuclideanGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, X, g)
@@ -708,10 +708,10 @@ function _log_SE3!(G::SpecialEuclideanGroup{ManifoldsBase.TypeParameter{Tuple{3}
     log!(SO3, Y, Identity(SO3), R)
     α = norm(Y) / sqrt(2) # skew symmetric, so the norm counts everything “twice” in the sqrt.
     if α ≈ 0
-        copyto!(T2, v, t) # U(α) is the identity
+        copyto!(T3, v, t) # U(α) is the identity
     else
         β = 1 / α^2 - (1 + cos(α)) / (2 * α * sin(α))
-        Vα = LinearAlgebra.I .- Y ./ 2 .+ β .* Y^2
+        Vα = LinearAlgebra.I - Y ./ 2 .+ β .* Y^2
         mul!(v, Vα, t)
     end
     return X