1+
2+ # using Revise
3+ using Test
4+ using LinearAlgebra
5+ using IncrementalInference
6+ using ManifoldsBase
7+ using Manifolds, Manopt
8+ import Optim
9+ using FiniteDifferences, ManifoldDiff
10+ import Rotations as _Rot
11+
12+ # #
13+
14+ # finitediff setup
15+ r_backend = ManifoldDiff. TangentDiffBackend (
16+ ManifoldDiff. FiniteDifferencesBackend ()
17+ )
18+
19+ # #
20+ @testset " ManifoldDiff, Basic test" begin
21+ # #
22+
23+ # problem setup
24+ n = 100
25+ σ = π / 8
26+ M = Manifolds. Sphere (2 )
27+ p = 1 / sqrt (2 ) * [1.0 , 0.0 , 1.0 ]
28+ data = [exp (M, p, σ * rand (M; vector_at= p)) for i in 1 : n];
29+
30+ # objective function
31+ f (M, p) = sum (1 / (2 * n) * distance .(Ref (M), Ref (p), data) .^ 2 )
32+ # f_(p) = f(M,p)
33+
34+ # non-manual: intrinsic finite differences gradient
35+ function grad_f_FD (M,p)
36+ f_ (p_) = f (M,p_)
37+ ManifoldDiff. gradient (M, f_, p, r_backend)
38+ end
39+ # manual gradient
40+ # grad_f(M, p) = sum(1 / n * grad_distance.(Ref(M), data, Ref(p)));
41+
42+
43+ # and solve
44+ @time m1 = gradient_descent (M, f, grad_f_FD, data[1 ])
45+
46+ @info " Basic Manopt test" string (m1' )
47+ @test isapprox (p, m1; atol= 0.15 )
48+
49+ # #
50+ end
51+
52+ # #
53+
54+ """
55+ ManifoldWrapper{TM<:AbstractManifold} <: Optim.Manifold
56+
57+ Adapts Manifolds.jl manifolds for use in Optim.jl
58+ """
59+ struct ManifoldWrapper{TM<: AbstractManifold } <: Optim.Manifold
60+ M:: TM
61+ end
62+
63+ function Optim. retract! (M:: ManifoldWrapper , x)
64+ ManifoldsBase. embed_project! (M. M, x, x)
65+ return x
66+ end
67+
68+ function Optim. project_tangent! (M:: ManifoldWrapper , g, x)
69+ ManifoldsBase. embed_project! (M. M, g, x, g)
70+ return g
71+ end
72+
73+ # #
74+ @testset " Optim.jl ManifoldWrapper example from mateuszbaran (copied to catch issues on future changes)" begin
75+ # #
76+ # Example modified from: https://gist.github.com/mateuszbaran/0354c0edfb9cdf25e084a2b915816a09
77+
78+ # example usage of Manifolds.jl manifolds in Optim.jl
79+ M = Manifolds. Sphere (2 )
80+ x0 = [1.0 , 0.0 , 0.0 ]
81+ q = [0.0 , 1.0 , 0.0 ]
82+
83+ f (p) = 0.5 * distance (M, p, q)^ 2
84+
85+ # manual gradient
86+ function g! (X, p)
87+ log! (M, X, p, q)
88+ X .*= - 1
89+ println (p, X)
90+ end
91+
92+ # #
93+
94+ sol = Optim. optimize (f, g!, x0, Optim. ConjugateGradient (; manifold= ManifoldWrapper (M)))
95+ @test isapprox ([0 ,1 ,0. ], sol. minimizer; atol= 1e-8 )
96+
97+
98+ # # finitediff gradient (non-manual)
99+
100+ function g_FD! (X,p)
101+ X .= ManifoldDiff. gradient (M, f, p, r_backend)
102+ X
103+ end
104+
105+ #
106+ x0 = [1.0 , 0.0 , 0.0 ]
107+
108+ sol = Optim. optimize (f, g_FD!, x0, Optim. ConjugateGradient (; manifold= ManifoldWrapper (M)))
109+ @test isapprox ([0 ,1 ,0. ], sol. minimizer; atol= 1e-8 )
110+
111+ # #
112+
113+ # x0 = [1.0, 0.0, 0.0]
114+ # # internal ForwardDfif doesnt work out the box on Manifolds
115+ # sol = Optim.optimize(f, x0, Optim.ConjugateGradient(; manifold=ManifoldWrapper(M)); autodiff=:forward )
116+ # @test isapprox([0,1,0.], sol.minimizer; atol=1e-8)
117+
118+ # #
119+ end
120+
121+
122+ @testset " Modified Manifolds.jl ManifoldWrapper <: Optim.Manifold for SpecialEuclidean(2)" begin
123+ # #
124+
125+ M = Manifolds. SpecialEuclidean (2 )
126+ e0 = ArrayPartition ([0 ,0. ], [1 0 ; 0 1. ])
127+
128+ x0 = deepcopy (e0)
129+ Cq = 9 * ones (3 )
130+ while 1.5 < abs (Cq[3 ])
131+ @show Cq .= randn (3 )
132+ # Cq[3] = 1.5 # breaks ConjugateGradient
133+ end
134+ q = exp (M,e0,hat (M,e0,Cq))
135+
136+ f (p) = distance (M, p, q)^ 2
137+
138+ # # finitediff gradient (non-manual)
139+ function g_FD! (X,p)
140+ X .= ManifoldDiff. gradient (M, f, p, r_backend)
141+ X
142+ end
143+
144+ # # sanity check gradients
145+
146+ X = hat (M, e0, zeros (3 ))
147+ g_FD! (X, q)
148+ # gradient at the optimal point should be zero
149+ @show X_ = [X. x[1 ][:]; X. x[2 ][:]]
150+ @test isapprox (0 , sum (abs .(X_)); atol= 1e-8 )
151+
152+ # gradient not the optimal point should be non-zero
153+ g_FD! (X, e0)
154+ @show X_ = [X. x[1 ][:]; X. x[2 ][:]]
155+ @test 0.01 < sum (abs .(X_))
156+
157+ # # do optimization
158+ x0 = deepcopy (e0)
159+ sol = Optim. optimize (f, g_FD!, x0, Optim. ConjugateGradient (; manifold= ManifoldWrapper (M)))
160+ Cq .= randn (3 )
161+ # Cq[
162+ @show sol. minimizer
163+ @test isapprox ( f (sol. minimizer), 0 ; atol= 1e-3 )
164+ @test isapprox ( 0 , sum (abs .(log (M, e0, compose (M, inv (M,q), sol. minimizer)))); atol= 1e-5 )
165+
166+ # #
167+ end
168+
169+
170+ @testset " Modified ManifoldsWrapper for Optim.Manifolds, SpecialEuclidean(3)" begin
171+ # #
172+
173+
174+ M = Manifolds. SpecialEuclidean (3 )
175+ e0 = ArrayPartition ([0 ,0 ,0. ], Matrix (_Rot. RotXYZ (0 ,0 ,0. )))
176+
177+ x0 = deepcopy (e0)
178+ Cq = 0.5 * randn (6 )
179+ q = exp (M,e0,hat (M,e0,Cq))
180+
181+ f (p) = distance (M, p, q)^ 2
182+
183+ # # finitediff gradient (non-manual)
184+ function g_FD! (X,p)
185+ X .= ManifoldDiff. gradient (M, f, p, r_backend)
186+ X
187+ end
188+
189+ # # sanity check gradients
190+
191+ X = hat (M, e0, zeros (6 ))
192+ g_FD! (X, q)
193+
194+ @show X_ = [X. x[1 ][:]; X. x[2 ][:]]
195+ # gradient at the optimal point should be zero
196+ @test isapprox (0 , sum (abs .(X_)); atol= 1e-8 )
197+
198+ # gradient not the optimal point should be non-zero
199+ g_FD! (X, e0)
200+ @show X_ = [X. x[1 ][:]; X. x[2 ][:]]
201+ @test 0.01 < sum (abs .(X_))
202+
203+ # # do optimization
204+ x0 = deepcopy (e0)
205+ sol = Optim. optimize (f, g_FD!, x0, Optim. ConjugateGradient (; manifold= ManifoldWrapper (M)))
206+ # Cq .= 0.5*randn(6)
207+ # Cq[
208+ @show sol. minimizer
209+ @test isapprox ( f (sol. minimizer), 0 ; atol= 1e-3 )
210+ @test isapprox ( 0 , sum (abs .(log (M, e0, compose (M, inv (M,q), sol. minimizer)))); atol= 1e-3 )
211+
212+
213+ # #
214+ end
215+
216+
217+ @testset " Optim.Manifolds, SpecialEuclidean(3), using IIF.optimizeManifold_FD" begin
218+ # #
219+
220+ M = Manifolds. SpecialEuclidean (3 )
221+ e0 = ArrayPartition ([0 ,0 ,0. ], Matrix (_Rot. RotXYZ (0 ,0 ,0. )))
222+
223+ x0 = deepcopy (e0)
224+ Cq = 0.5 * randn (6 )
225+ q = exp (M,e0,hat (M,e0,Cq))
226+
227+ f (p) = distance (M, p, q)^ 2
228+
229+ sol = IncrementalInference. optimizeManifold_FD (M,f,x0)
230+
231+ @show sol. minimizer
232+ @test isapprox ( f (sol. minimizer), 0 ; atol= 1e-3 )
233+ @test isapprox ( 0 , sum (abs .(log (M, e0, compose (M, inv (M,q), sol. minimizer)))); atol= 1e-5 )
234+
235+
236+ # #
237+ end
0 commit comments