Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Experiment with factor gradients in solve #1825

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

Experiment with factor gradients in solve #1825

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 6 additions & 1 deletion src/parametric/services/ParametricManopt.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,12 +62,17 @@ function CalcFactorResidualAP(fg::GraphsDFG, factorLabels::Vector{Symbol}, varIn
return ArrayPartition{CalcFactorResidual, typeof(parts_tuple)}(parts_tuple)
end

function (cfm::CalcFactorResidual)(p)
function (cfm::CalcFactorResidual)(p::Vector)
meas = cfm.meas
points = map(idx->p[idx], cfm.varOrderIdxs)
return cfm.sqrt_iΣ * cfm(meas, points...)
end

function (cfm::CalcFactorResidual)(p::ArrayPartition)
points = map(idx->p.x[idx], cfm.varOrderIdxs)
return cfm.sqrt_iΣ * cfm(cfm.meas, points...)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Post AMP 41, we should look at consolidating this with the kernel evaluations done in AMP. Specifically this Malahanobis distance part. The reasoning is that we need to build belief trees on each clique and then be able to evaluation, which is exactly the same requirement as what this code here is doing.

end

# cost function f: M->ℝᵈ for Riemannian Levenberg-Marquardt
struct CostFres_cond!{PT, CFT}
points::PT
Expand Down
29 changes: 29 additions & 0 deletions src/services/FactorGradients.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,36 @@ function factorJacobian(
return ManifoldDiff.jacobian(M_dom, M_codom, costf, p0, backend)
end

function factorGradient(
cf::CalcFactorResidual,
M,
p,
backend = ManifoldDiff.TangentDiffBackend(ManifoldDiff.FiniteDiffBackend()),
)
ManifoldDiff.gradient(M, (x) -> 1//2 * norm(cf(x))^2, p, backend)
end

function factorJacobian(
cf::CalcFactorResidual,
M_dom,
p,
backend = ManifoldDiff.TangentDiffBackend(ManifoldDiff.FiniteDiffBackend()),
)
# M_dom = ProductManifold(getManifold.(fg, varlabels)...)
M_codom = Euclidean(manifold_dimension(getManifold(cf)))

return ManifoldDiff.jacobian(M_dom, M_codom, cf, p, backend)
end

#
function factorGradient(
cf::CalcFactorNormSq,
M,
p,
backend = ManifoldDiff.TangentDiffBackend(ManifoldDiff.FiniteDiffBackend()),
)
ManifoldDiff.gradient(M, cf, p, backend)
end

export getCoordSizes
export checkGradientsToleranceMask, calcPerturbationFromVariable
Expand Down
18 changes: 15 additions & 3 deletions src/services/NumericalCalculations.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,15 +71,16 @@ function _solveLambdaNumeric(
return r.minimizer
end

# struct OptimCalcConv end
struct OptimCalcConv end
# CalcFactorNormSq cost function for an input in coordinates as used by Optim.jl
function (hypoCalcFactor::CalcFactorNormSq)(M::AbstractManifold, Xc::AbstractVector)
function (hypoCalcFactor::CalcFactorNormSq)(::Type{OptimCalcConv}, M::AbstractManifold, Xc::AbstractVector)
# hypoCalcFactor.manifold is the factor's manifold, not the variable's manifold that is needed here
ϵ = getPointIdentity(M)
X = get_vector(M, ϵ, SVector(Xc), DefaultOrthogonalBasis())
p = exp(M, ϵ, X)
return hypoCalcFactor(CalcConv, p)
end
(hypoCalcFactor::CalcFactorNormSq)(M::AbstractManifold, p) = hypoCalcFactor(OptimCalcConv, M, p)

struct ManoptCalcConv end

Expand Down Expand Up @@ -127,10 +128,19 @@ function _solveLambdaNumeric(
retraction_method = ExponentialRetraction()
)
return r
elseif false
r = gradient_descent(
M,
(M,x)->hypoCalcFactor(x),
(M, x)-> factorGradient(hypoCalcFactor, M, x),
u0;
stepsize=ConstantStepsize(0.1),
)
return r
end

r = Optim.optimize(
x->hypoCalcFactor(M, x),
x->hypoCalcFactor(OptimCalcConv, M, x),
X0c,
alg
)
Expand Down Expand Up @@ -396,6 +406,8 @@ function (cf::CalcFactorNormSq)(::Type{CalcConv}, x)
res = isnothing(cf.slack) ? res : res .- cf.slack
return sum(x->x^2, res)
end
#default to conv
(cf::CalcFactorNormSq)(x) = cf(CalcConv, x)

function _buildHypoCalcFactor(ccwl::CommonConvWrapper, smpid::Integer, _slack)
# build a view to the decision variable memory
Expand Down
Loading