New localProduct too eager on partials as 1DOF only

[dehann]

partials means dimension constrained, as in only some dimensions are effected by the partial factor. Something like a Range factor operates on XY (Z) dimensions simultaneously through the Minimization process. It is wrong to overconstrict partials to work one dimension at a time. Related issue #1010 .

MWE:

using RoME

fg = initfg()
getSolverParams(fg).useMsgLikelihoods = true
addVariable!(fg, :x0, Pose2)
addVariable!(fg, :l1, Point2)
addFactor!(fg, [:x0], PriorPose2( MvNormal([0; 0; 0], diagm([0.3;0.3;0.3].^2)) ))
ppr = Pose2Point2Range(MvNormal([7.323699045815659], diagm([0.3].^2)))
addFactor!(fg, [:x0; :l1], ppr )
addVariable!(fg, :x1, Pose2)
pp = Pose2Pose2(MvNormal([9.82039106655709;-0.040893643507463134;0.7851856362659602], diagm([0.3;0.3;0.05].^2)))
ppr = Pose2Point2Range(MvNormal([6.783355788017825], diagm([0.3].^2)))
addFactor!(fg, [Symbol("x$(i-1)"); Symbol("x$(i)")], pp )
addFactor!(fg, [Symbol("x$(i)"); :l1], ppr )


ensureAllInitialized!(fg)
pp, parr, partials, lb = IIF.localProduct(fg, :l1)

@assert length(partials) == 0

---

NOTE, RoME.Pose2Point2Range is indicated as a partial

https://github.com/JuliaRobotics/RoME.jl/blob/03129f5bd100a3d6e2600bf692b98c9bb9e928c4/src/factors/Range2D.jl#L51

[dehann]

Need to update (adding to ongoing AMP.MKD upgrades):

IncrementalInference.jl/src/GraphProductOperations.jl

Lines 28 to 31 in 22898c5

	# do each partial dimension individually
	for (dimnum,pp) in partials
	pGM[dimnum,:] = AMP.manifoldProduct(pp, (manis[dimnum],), Niter=1) |> getPoints
	end

IncrementalInference.jl/src/GraphProductOperations.jl

Lines 82 to 85 in 22898c5

	# do each partial dimension individually
	for (dimnum,pp) in partials
	pGM[dimnum,:] = AMP.manifoldProduct(pp, (manis[dimnum],), Niter=1) |> getPoints
	end

[dehann]

The plan is to do Manifolds.jl consolidation first

• (Integrate with Manifolds.jl ApproxManifoldProducts.jl#32, Integrate with Manifolds.jl RoME.jl#244), and then
• update IIF to new "multi-partials" mechanism (as found in Range drop partial, see IIF 1206 RoME.jl#422).
• From there finish Issue with differential constraint from partial factors #1010, and then
• replace BallTreeDensity to close out Ball/Metric Tree using NearestNeighbors.jl / Manifolds.jl / Distances.jl / CoordinateTransformations.jl ApproxManifoldProducts.jl#41

as the last step in this epic.

cc @Affie

[dehann]

Hi @lemauee,

Just an update. I think this issue is a big part of the problems you were seeing in JuliaRobotics/RoME.jl#378, JuliaRobotics/RoME.jl#380 where Pose2Point2Range factors were losing correlation between dimensions because partials we treated too naively (1DOF at a time).

A workaround is easy, but Optim.jl has to work harder. The proper fix is to combine with Manifolds.jl first (what i have been doing the past 3 weeks) and rebuild the approx belief product wrappers.

Basically, we have incompatible partials and Optim.jl plumbing in the existing/old code. This was not an issue before, but in consolidating parametric and nonparametric plumbing this incompatibility showed up through the refactoring. Previously Optim was just grinding away until the right answer was found.

Building out the new abstractions with Manifolds.jl is the right place to make these two aspects compatible. The fix should be available either in IIF v0.22.x or latest v0.23.0 (depending on how semver deprecation rules play out).

Best,
Dehann