Improve performance of selected operations on SE(n)

[mateuszbaran]

See JuliaRobotics/IncrementalInference.jl#1546 (comment) for explanation.

[codecov]

Codecov Report

Merging #655 (62d2264) into master (71cd2aa) will increase coverage by 0.00%.
The diff coverage is 100.00%.

@@           Coverage Diff           @@
##           master     #655   +/-   ##
=======================================
  Coverage   99.22%   99.22%           
=======================================
  Files         106      106           
  Lines       10440    10481   +41     
=======================================
+ Hits        10359    10400   +41     
  Misses         81       81           

Files	Coverage Δ
src/groups/special_euclidean.jl	99.62% <100.00%> (+0.02%)	⬆️
src/manifolds/GeneralUnitaryMatrices.jl	100.00% <100.00%> (ø)

📣 We’re building smart automated test selection to slash your CI/CD build times. Learn more

[Affie]

I tried something similar but missed the SE(N) dispatches.
I did get_vector_orthogonal, exp, log, and get_coordinates.
I see you already have get_vector_orthogonal and exp.
Is it worth it to include log and get_coordinates as well?
this is what I tried:

function Manifolds.log(M::Manifolds.GeneralUnitaryMatrices{3,ℝ}, p::SMatrix, q::SMatrix)
  U = transpose(p) * q
  cosθ = (tr(U) - 1) / 2
  if cosθ ≈ -1
      eig = Manifolds.eigen_safe(U)
      ival = findfirst(λ -> isapprox(λ, 1), eig.values)
      inds = SVector{3}(1:3)
      #TODO this is to stop convert error of ax as a complex number
      ax::Vector{Float64} = eig.vectors[inds, ival]
      return get_vector(M, p, π * ax, DefaultOrthogonalBasis())
  end
  X = U ./ Manifolds.usinc_from_cos(cosθ)
  return (X .- X') ./ 2
end

function Manifolds.get_coordinates(
  ::Manifolds.GeneralUnitaryMatrices{3,ℝ},
  p::SMatrix,
  X::SMatrix,
  ::DefaultOrthogonalBasis{ℝ,Manifolds.TangentSpaceType},
)
  return SA[X[3, 2], X[1, 3], X[2, 1]]
end

[mateuszbaran]

Is it worth it to include log and get_coordinates as well?
this is what I tried:

Sure, I will add those too.

[Affie]

@dehann, it looks like with these kinds of optimizations we can get down to zero allocations in the factor residual functions.

[dehann]

Oh, that is great, acknowledged thanks!