Basic on-manifold example with Sphere(2) and one prior fail

[Affie]

I've copied the example out to a test, I'm not entirely sure if I'm doing it correctly though.

IncrementalInference.jl/test/testSphereMani.jl

Lines 1 to 46 in 6f1d949

	using DistributedFactorGraphs
	using IncrementalInference
	using Manifolds
	using StaticArrays
	using Test
	@testset "Test Sphere(2) prior" begin
	Base.convert(::Type{<:Tuple}, M::Sphere{2, ℝ}) = (:Euclid, :Euclid)
	Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{Sphere{2, ℝ}}) = (:Euclid, :Euclid)
	@defVariable Sphere2 Sphere(2) [1.0, 0.0, 0.0]
	M = getManifold(Sphere2)
	@test M == Sphere(2)
	pT = getPointType(Sphere2)
	@test pT == Vector{Float64}
	pϵ = getPointIdentity(Sphere2)
	@test pϵ == [1.0, 0.0, 0.0]
	@test is_point(getManifold(Sphere2), getPointIdentity(Sphere2))
	fg = initfg()
	v0 = addVariable!(fg, :x0, Sphere2)
	mp = ManifoldPrior(Sphere(2), SA[1., 0, 0], MvNormal([0.01, 0.01]))
	p = addFactor!(fg, [:x0], mp)
	doautoinit!(fg, :x0)
	vnd = getVariableSolverData(fg, :x0)
	@test all(isapprox.(mean(vnd.val), [1,0,0], atol=0.1))
	@test all(is_point.(Ref(M), vnd.val))
	v1 = addVariable!(fg, :x1, Sphere2)
	mf = ManifoldFactor(Sphere(2), MvNormal([0.1, 0.2], [0.05,0.05]))
	f = addFactor!(fg, [:x0, :x1], mf)
	##
	# Debugging Sphere error
	# DimensionMismatch("tried to assign 200 elements to 300 destinations")
	smtasks = Task[]
	solveTree!(fg; smtasks, verbose=true, recordcliqs=ls(fg))
	hists = fetchCliqHistoryAll!(smtasks);
	end

The same example previously errored as in: #1284

Edit: updated to the latest commit

[dehann]

I think this part will definitely cause problems:

Base.convert(::Type{<:Tuple}, M::Sphere{2, ℝ}) = (:Euclid, :Euclid, :Euclid) 
Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{Sphere{2, ℝ}})  = (:Euclid, :Euclid, :Euclid) 

Also not sure if this mean is working properly, since the manifold information is lost, and these are definitely not a standard Euclidean mean:

@test all(isapprox.(mean(vnd.val), [1,0,0], atol=0.1)) 

This mean is something much closer to:
https://manoptjl.org/stable/tutorials/MeanAndMedian.html#The-given-Dataset-1

[Affie]

I fixed the 3 x Euclid in the next commit of that branch. It should just be 2x.
I’ll update the first post

[Affie]

Also not sure if this mean is working properly, since the manifold information is lost, and these are definitely not a standard Euclidean mean

The covariance is very small, so it will be very close to 1.0 and the tolerance is 0.1.

[Affie]

I just remembered you said AMP is still based on the identity element. So Sphere(2) won't work for now.

[Affie]

JuliaRobotics/ApproxManifoldProducts.jl#94 fixes the dimension error.