Upgrade to LieGroups (#35)

Co-authored-by: Johannes Terblanche <[email protected]>

Author: Affie

Project.toml

@@ -5,15 +5,15 @@ version = "0.2.4"
 
 [deps]
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
+LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
-Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 
 [compat]
 DocStringExtensions = "0.8, 0.9"
-Manifolds = "0.10, 0.11"
+LieGroups = "0.1"
 RecursiveArrayTools = "3.27.0"
 Rotations = "1"
 StaticArrays = "1"

src/CameraModels.jl

@@ -1,7 +1,7 @@
 module CameraModels
 
 using LinearAlgebra
-using Manifolds
+using LieGroups
 using DocStringExtensions
 using StaticArrays
 import Rotations as Rot_

src/services/Utils.jl

@@ -153,9 +153,8 @@ function intersectRayToPlane(
     a_F::AbstractVector{<:Real},
     l_nFL::AbstractVector{<:Real},
     l_FL::AbstractVector{<:Real};
-    M = SpecialEuclidean(3),
-    R0 = [1 0 0; 0 1 0; 0 0 1.],
-    l_T_ex = ArrayPartition([0;0;0.], exp_lie(M.manifold[2], hat(M.manifold[2], R0, [0;0.2;0.]))),
+    M = SpecialEuclideanGroup(3; variant = :right),
+    l_T_ex = ArrayPartition([0;0;0.], exp(SpecialOrthogonalGroup(3), hat(LieAlgebra(SpecialOrthogonalGroup(3)), [0;0.2;0.]))),
     ex_T_c = ArrayPartition([0;0;0.], [0 0 1; -1 0 0; 0 -1 0.]),
 )
     # camera in level (or camera to level) manifold element as ArrayPartition

test/multiview_manifolds.jl

@@ -4,14 +4,15 @@
 # using Revise
 using Test
 import CameraModels
-using Optim, Manifolds
+using Optim, LieGroups
 using StaticArrays
+using LieGroups: SpecialEuclideanProductPoint
 # using ManifoldDiff
 # import FiniteDifferences as FD
 
 ##
 
-M = SpecialEuclidean(3)
+M = SpecialEuclideanGroup(3; variant = :right)
 
 ##
 @testset "Multiview optimization of point in front of 2 cameras" begin
@@ -49,7 +50,7 @@ function projectPointFrom(cam, c_H_w, w_Ph)
 end
 
 function cameraResidual(cam, meas, M, w_T_c, w_Ph, κ=1000)
-  pred = projectPointFrom(cam, inv(affine_matrix(M,w_T_c)), w_Ph)
+  pred = projectPointFrom(cam, inv(convert(AbstractMatrix, SpecialEuclideanProductPoint(w_T_c))), w_Ph)
   # experimental cost function to try force bad reprojects in front of the camera during optimization
   κ*(abs(pred.depth) - pred.depth)^2 + (meas[1]-pred[1])^2 + (meas[2]-pred[2])^2
 end

test/testutils.jl

@@ -1,6 +1,13 @@
 using Test
 using CameraModels
-import Manifolds as MJL
+import LieGroups
+using LieGroups:
+  SpecialEuclideanGroup,
+  SpecialOrthogonalGroup,
+  hat,
+  exp,
+  compose,
+  LieAlgebra
 
 
 @testset "Test intersect of line and plane" begin
@@ -20,8 +27,8 @@ end
 
 @testset "Test raytracing to plane" begin
 
-M = MJL.SpecialEuclidean(3)
-Mr = M.manifold[2]
+M = SpecialEuclideanGroup(3; variant = :right)
+Mr = SpecialOrthogonalGroup(3)
 R0 = [1 0 0; 0 1 0; 0 0 1.]
 
 
@@ -36,8 +43,8 @@ l_FL = [0; 0; -2.]
 
 # local level to body to extrinsic transform 
 l_T_b = ArrayPartition([0;0;0.], R0)
-b_T_ex = ArrayPartition([0;0;0.], MJL.exp_lie(Mr, MJL.hat(Mr, R0, [0;0.2;0.2])))
-l_T_ex = MJL.compose(M, l_T_b, b_T_ex)
+b_T_ex = ArrayPartition([0;0;0.], exp(Mr, hat(LieAlgebra(Mr), [0;0.2;0.2])))
+l_T_ex = compose(M, l_T_b, b_T_ex)
 
 # Ray trace
 l_Forb = intersectRayToPlane(
@@ -50,8 +57,8 @@ l_Forb = intersectRayToPlane(
 
 
 ## Place the body somewhere in the world
-w_T_b = ArrayPartition([0.;0.;2.], MJL.exp_lie(Mr, MJL.hat(Mr, R0, [0;0;0.])))
+w_T_b = ArrayPartition([0.;0.;2.], LieGroups.exp(Mr, LieGroups.hat(LieAlgebra(Mr), [0;0;0.])))
 # find feature points in the world frame
-_w_Forb = MJL.affine_matrix(M, w_T_b)*[l_Forb; 1.]
+# _w_Forb = MJL.affine_matrix(M, w_T_b)*[l_Forb; 1.]
 
 end