Retrieving Factors from a Factor Graph

[ProfFan]

I am currently porting the Chordal initialization from GTSAM (which is the last step to the comparison benchmark) but I think I have a problem: after loading the G2O file we have a FG that we need to extract the between factors from.

The pseudo code looks like:

  func buildPose3graph(graph: FactorGraph) {
    var pose3graph = FactorGraph()

    for (key, factor) in graph.storage { // this step

      // recast to a between on Pose3
      let pose3Between = factor.cast(to: BetweenFactor3) // And this step
      if (pose3Between)
        pose3Graph.store(pose3Between);

      let pose3Prior = factor.cast(to: PriorFactor3)
      if (pose3Prior)
        pose3Graph.store(BetweenFactor3(kAnchorKey, pose3Prior.key, pose3Prior.value)
    }
    return pose3Graph;
  }

@marcrasi Is there a quick solution to this? Thank you very much in advance :)

[marcrasi]

Here is a thing that does what I think you need: https://github.com/borglab/SwiftFusion/pull/101 !

Let me know if you need it to do anything else :)

[ProfFan]

@marcrasi Yeah sorry to bother you again but I think I need to manually construct a GFG now (similar to the old GFG interface). I remember we dealt with this partially in #83 , but that one is only for scalar Jacobians in LM.

[marcrasi]

Do you have everything that you need to construct the JacobianFactors that go into the GFG? If so, then the only other thing you need is a store method on GaussianFactorGraph that lets you add them. I can add that pretty easily.

[marcrasi]

If you're working with concrete types, then I think it should be pretty easy to construct the JacobianFactors using this initializer.

It's harder to do this in a generic context. I think we can build something eventually that lets you do it, but it would be much easier to start with a concrete context.

[ProfFan]

Yeah I just need to add some JacobianFactors with a 9x9 Jacobian. Let's start with the concrete way first.

[ProfFan]

Already solved