add SpecialEuclidean(3) test

Author: dehann

test/manifolds/manifolddiff.jl

@@ -6,6 +6,7 @@ using ManifoldsBase
 using Manifolds, Manopt
 import Optim
 using FiniteDifferences, ManifoldDiff
+import Rotations as _Rot
 
 ##
 
@@ -159,5 +160,49 @@ Cq .= randn(3)
 @test isapprox( f(sol.minimizer), 0; atol=1e-8 )
 @test isapprox( 0, sum(abs.(log(M, e0, compose(M, inv(M,q), sol.minimizer)))); atol=1e-5)
 
+##
+end
+
+
+@testset "Modified ManifoldsWrapper for Optim.Manifolds, SpecialEuclidean(3)" begin
+##
+
+
+M = Manifolds.SpecialEuclidean(3)
+e0 = ArrayPartition([0,0,0.], Matrix(_Rot.RotXYZ(0,0,0.)))
+
+x0 = deepcopy(e0)
+Cq = 0.5*randn(6)
+q  = exp(M,e0,hat(M,e0,Cq))
+
+f(p) = distance(M, p, q)^2
+
+## finitediff gradient (non-manual)
+function g_FD!(X,p)
+  X .= ManifoldDiff.gradient(M, f, p, r_backend)
+  X
+end
+
+## sanity check gradients
+
+X = hat(M, e0, zeros(6))
+g_FD!(X, q)
+# gradient at the optimal point should be zero
+@test isapprox(0, sum(abs.(X[:])); atol=1e-8 )
+
+# gradient not the optimal point should be non-zero
+g_FD!(X, e0)
+@test 0.01 < sum(abs.(X[:]))
+
+## do optimization
+x0 = deepcopy(e0)
+sol = Optim.optimize(f, g_FD!, x0, Optim.ConjugateGradient(; manifold=ManifoldWrapper(M)))
+Cq .= 0.5*randn(6)
+# Cq[
+@show sol.minimizer
+@test isapprox( f(sol.minimizer), 0; atol=1e-8 )
+@test isapprox( 0, sum(abs.(log(M, e0, compose(M, inv(M,q), sol.minimizer)))); atol=1e-5)
+
+
 ##
 end