New implementation working fine. Factor graph properly built with joint factors as appropriate and spanning tree computation.