From 39d99a1daeb4e2a1f01d14bcddd1738a50404633 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 3 Dec 2025 17:16:10 +0200 Subject: [PATCH] Upgrade to LieGroups --- Project.toml | 4 ++-- src/CameraModels.jl | 2 +- src/services/Utils.jl | 5 ++--- test/multiview_manifolds.jl | 7 ++++--- test/testutils.jl | 21 ++++++++++++++------- 5 files changed, 23 insertions(+), 16 deletions(-) diff --git a/Project.toml b/Project.toml index e080a07..41ca82f 100644 --- a/Project.toml +++ b/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" diff --git a/src/CameraModels.jl b/src/CameraModels.jl index 4982a8b..f850e34 100644 --- a/src/CameraModels.jl +++ b/src/CameraModels.jl @@ -1,7 +1,7 @@ module CameraModels using LinearAlgebra -using Manifolds +using LieGroups using DocStringExtensions using StaticArrays import Rotations as Rot_ diff --git a/src/services/Utils.jl b/src/services/Utils.jl index daa8765..0338177 100644 --- a/src/services/Utils.jl +++ b/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 diff --git a/test/multiview_manifolds.jl b/test/multiview_manifolds.jl index ee79c8a..97211e9 100644 --- a/test/multiview_manifolds.jl +++ b/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 diff --git a/test/testutils.jl b/test/testutils.jl index 0f8b990..5a69c1c 100644 --- a/test/testutils.jl +++ b/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 \ No newline at end of file