Merge pull request #1346 from JuliaRobotics/21Q3/enh/matating_cost

Mutating function in _solveLambdaNumeric cost

Author: Affie

Project.toml

@@ -55,8 +55,8 @@ FunctionalStateMachine = "0.2.9"
 JLD2 = "0.3, 0.4"
 JSON2 = "0.3"
 KernelDensityEstimate = "0.5.6"
-Manifolds = "0.6"
-ManifoldsBase = "0.11, 0.12"
+Manifolds = "0.6.3"
+ManifoldsBase = "0.12.6"
 MetaGraphs = "0.6.4"
 NLSolversBase = "7.6"
 NLsolve = "3, 4"

src/NumericalCalculations.jl

@@ -94,22 +94,27 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
   # norm(M, p, X) == distance(M, p, X)
   #TODO fix closure for performance
   fM = getManifold(fcttype)
-  function cost(Xc)
-    p = exp(M, ϵ, hat(M, ϵ, Xc))  
+  function cost(p, X, Xc)
+    hat!(M, X, ϵ, Xc)
+    exp!(M, p, ϵ, X)  
     # X = objResX(p)
     # return norm(fM, p, X)^2 #TODO the manifold of p and X are not always the same
     #options getPointIdentity or leave it to factor 
     residual = objResX(p)
     return sum(residual.^2)
   end
 
-  # separate statements to try improve type-stability 
-  r = if islen1
-    Optim.optimize(cost, X0c, Optim.BFGS())
-  else
-    Optim.optimize(cost, X0c, Optim.NelderMead())
-  end
-
+  # # separate statements to try improve type-stability 
+  # r = if islen1
+  #   Optim.optimize(cost, X0c, Optim.BFGS())
+  # else
+  #   Optim.optimize(cost, X0c, Optim.NelderMead())
+  # end
+  alg = islen1 ? Optim.BFGS() : Optim.NelderMead()
+  X0 = hat(M, ϵ, X0c)
+  p0 = exp(M, ϵ, X0)
+  r = Optim.optimize(Xc->cost(p0, X0, Xc), X0c, alg)
+  
   return exp(M, ϵ, hat(M, ϵ, r.minimizer)) 
 
 end