Merge pull request #43 from dehann/21Q2/enh/logmapso2

add logmap_SO2
dehann · May 23, 2021 · c4e13f5 · c4e13f5
2 parents 4347977 + 0d2e2ce
commit c4e13f5
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/Project.toml b/Project.toml
@@ -2,7 +2,7 @@ name = "TransformUtils"
 uuid = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 keywords = ["coordinates", "reference frames", "SE2", "SO3", "quaternion", "robotics", "navigation"]
 desc = "Handy coordinate transformation tools between various conventions."
-version = "0.2.9"
+version = "0.2.10"
 
 [deps]
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"

diff --git a/src/TransformUtils.jl b/src/TransformUtils.jl
@@ -38,6 +38,7 @@ export
   rotate,
   wrapRad,
   logmap,
+  logmap_SO2,
   rightJacExmap,
   rightJacExmapinv,
   deltaso3vee,
@@ -728,6 +729,12 @@ end
 
 
 
+# manifold distance, add, and subtract
+function logmap_SO2(Rl::Matrix{<:Real})
+  ct = abs(Rl[1,1]) > 1.0 ? 1.0 : Rl[1,1]  # reinserting the sign below
+  -sign(Rl[2,1])*acos(ct)
+end
+
 function wrapRad(th::Real)
   #rem is not the best function here so using rem2pi(x, RoundNearest) returns in interval [-π, π]
   #TODO is [-π, π] a problem or should it strictly be [-pi, pi)