Add Nonlinear FG interface #34

ProfFan · 2020-04-22T01:57:29Z

This adds a nonlinear factor graph interface for SwiftFusion.

Currently only implemented BetweenFactor for Pose2.

TODO:

AnyComposable for generalizing BetweenFactor to Rot2, etc.
Test linear CG on linearize FG

ProfFan · 2020-04-22T05:40:33Z

@marcrasi I blatantly copied the AnyDifferentiable code to make AnyNonlinearFactor... I think some of the code is redundant but I am not sure. Also, do you have any suggestions for the API?

@dellaert Since the CGLS optimizer is working on the linearized factor graph, I think this PR is ready for an initial review :)

…actor_graph

ProfFan · 2020-04-22T05:58:38Z

BTW, for Pose2SLAM demo the solution converges applying CGLS two times, which is amazing!

dellaert · 2020-04-22T12:46:35Z

Very cool. Have a full day of meetings but hope to get to this before our 1:1. Amazing that the code works but convergence is not amazing if you start close to the optimum :-)

marcrasi

I read part of this but now I have to run to a meeting. I'll read more later. Exciting!

marcrasi · 2020-04-22T16:28:20Z

Sources/SwiftFusion/Inference/Factor.swift

+/// ================
+/// `Input`: the input values as key-value pairs
+///
+public protocol NonlinearFactor: Differentiable & Factor {


I think that the NonlinearFactor does not need to be Differentiable, because you never need to take derivatives with respect to the factor itself. You only take derivatives of the factor's error function with respect to the input values.

Removing Differentiable is useful because then you can have an Array<NonlinearFactor> without needing to create the AnyNonlinearFactor type.

Maybe NonlinearFactor should be renamed Factor and then LinearFactor should refine it? This would make sense to me because Factor is the most general thing that can behave in any way (nonlinearly or linearly), and LinearFactor is a special case that is guaranteed to be linear.

Yeah I agree on the first one, but we will still need the type-erased wrapper if we have associated types in NonlinearFactor, right?

@dellaert What's your stance on this (pt. 2)?

Yes, we will need the type-erased wrapper if there are associated types in NonlinearFactor. But currently there aren't any associated types (other than the TangentVector that comes from Differentiable), and I can't think of any associated types that you will need in the future.

marcrasi · 2020-04-22T16:36:28Z

Sources/SwiftFusion/Inference/Factor.swift

 /// The most general factor protocol.
 public protocol Factor {
-  var keys: Array<Int> { get set }
+  var keys: Array<Int> { get }


Is there any place where you need to get the keys from a Factor? (Of course, the implementations of the factors need to use their keys, but do any users of factors need to get the keys?) If not, you could simplify the Factor API by completely removing the keys requirement.

And if you can remove keys, then you can also simplify the protocol hierarchy by completely removing Factor.

I think we still have this in some of the algorithms for iterating through the keys to lookup the Values, but that may change if we have a better encapsulated API

We will need it for direct solvers, to decide on the ordering.

marcrasi · 2020-04-22T16:54:17Z

Sources/SwiftFusion/Inference/BetweenFactor.swift

+    return Vector3(error.rot.theta, error.t.x, error.t.y)
+  }
+
+  public func linearize(_ values: Values) -> JacobianFactor {


I think it's very likely that we can write a default generic linearize function that automatically does this for all NonlinearFactors, so that we don't have to repeat this every time we define a factor.

I have to run to a meeting soon so I don't have time to figure it out now, but I'll follow up later.

Done in #49!

marcrasi · 2020-04-22T16:56:01Z

Sources/SwiftFusion/Inference/BetweenFactor.swift

+///
+public struct BetweenFactor: NonlinearFactor {
+  @noDerivative
+  public var keys: Array<Int> = []


I would suggest storing these as public let key1, key2: Int instead of as an array, to make it clear that this isn't really a variable-length thing.

If you do need keys: Array<Int> to satisfy the Factor requirement, you can make that a computed property:

public var keys: Array<Int> { [key1, key2] }

Yeah this is me simply being lazy :)

marcrasi · 2020-04-22T17:00:14Z

Sources/SwiftFusion/Inference/BetweenFactor.swift

+      difference
+    )
+
+    return Vector3(error.rot.theta, error.t.x, error.t.y)


This should actually return something using local coordinates (between(values[key1].baseAs(Pose2.self), values[key2].baseAs(Pose2.self).localCoordinates(around: difference)), right?

If so, you could add a TODO and a github issue for this, because this seems to work for now.

Correct, this is on my TODO :)

Will change shortly...

marcrasi · 2020-04-22T19:59:15Z

Sources/SwiftFusion/Inference/Values.swift

+/// The class that holds Key-Value pairs.
+public struct Values: Differentiable & KeyPathIterable {
+  public typealias ScalarType = Double
+  var _values: [AnyDifferentiable] = []


The standard Swift style is to make fields private (or fileprivate so that you can use it in other extensions in the same file) instead of using _.

marcrasi · 2020-04-22T20:08:05Z

Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift

+
+      optimizer.optimize(gfg: gfg, initial: &dx)
+
+      for i in 0..<5 {


I see two possible improvements to the Values API.

You could make a subscript that does casting, which would allow you to mutate variables without storing them in an intermediate var. Something like val[i, as: Pose2.self].move(along: ...).

Values.TangentVector could be VectorValues, which would allow you to replace this whole loop with a single line val.move(along: dx).

Let me know if you want me to help write code for either of these.

agree on both!

I think (2) will become possible after #37 is merged? Then I think we can first merge this PR and then #37, and another PR for this one.

I think there might be some additional work required to enable (2) after #37 is merged -- it might not be totally easy to use Manifold with Values because Values contains some type erased stuff and type erased stuff sometimes needs some extra work to make it play nicely with everything else.

I'll investigate how to get (2) working.

Your suggestion of fixing this in a separate PR so that we don't block this PR sounds good to me!

Done in #49!

dellaert · 2020-04-24T15:13:41Z

Will review now...

dellaert

Nice progress - talk soon

dellaert · 2020-04-24T18:34:52Z

Sources/SwiftFusion/Core/MathUtil.swift

+
+import TensorFlow
+
+public func pinv(_ m: Tensor<Double>) -> Tensor<Double> {


Document - even when name is obvious. Say what it will compute in different cases.

Sources/SwiftFusion/Inference/AnyNonlinearFactor.swift

dellaert · 2020-04-24T20:08:27Z

Sources/SwiftFusion/Inference/Factor.swift

 /// The most general factor protocol.
 public protocol Factor {
-  var keys: Array<Int> { get set }
+  var keys: Array<Int> { get }


We will need it for direct solvers, to decide on the ordering.

Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift

dellaert · 2020-04-24T20:12:55Z

Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift

+
+      optimizer.optimize(gfg: gfg, initial: &dx)
+
+      for i in 0..<5 {


agree on both!

dellaert · 2020-04-24T20:13:46Z

Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift

+      }
+    }
+
+    let dumpjson = { (p: Pose2) -> String in


why are we printing stuff in a test?

marcrasi · 2020-04-29T23:24:47Z

Sources/SwiftFusion/Inference/NonlinearFactorGraph.swift

+public struct NonlinearFactorGraph: FactorGraph {
+  public typealias KeysType = Array<Int>
+
+  public typealias FactorsType = Array<AnyNonlinearFactor>


You said during our Friday meeting that you still haven't been able to eliminate AnyNonlinearFactor and replace this with Array<NonlinearFactor>.

I figured out why. The associatedtype FactorsType : Collection where FactorsType.Element: Factor in protocol FactorGraph doesn't work when you use Array<NonlinearFactor>, and the reason it doesn't work is that existentials can't conform to protocols. (This is a limitation of the Swift type system that probably won't go away any time soon.)

The easiest way to solve this would be to delete the protocol FactorGraph. Nothing actually relies on abstracting over different FactorGraph implementations yet, so this doesn't break anything.

I think this is worth it because deleting AnyNonlinearFactor makes everything a lot simpler and nicer.

It's also probably possible to redesign the FactorGraph protocol so that it doesn't have a constraint that breaks Array<NonlinearFactor>. I'd suggest writing the FactorGraph protocol later when we have a use case for that, because the use case will make it clearer what protocol FactorGraph needs to have.

@marcrasi Oh that's a great find! I'll remove the protocol for now :)

Thanks for the explanation!

Sources/SwiftFusion/Inference/AnyNonlinearFactor.swift

Tests/SwiftFusionTests/Inference/NonlinearFactorGraphTests.swift

marcrasi · 2020-04-29T23:31:44Z

AnyComposable for generalizing BetweenFactor to Rot2, etc.

I think you should be able to generalize BetweenFactor without introducing a new Any* type erasing type.

All you need is a protocol with the methods that BetweenFactor uses, and then you can use that to define a fully generic BetweenFactor. The protocol is likely something like LieGroup. Pseudocode:

protocol LieGroup {
  static func * (Self, Self) -> Self
  func inverse() -> Self
  func localCoordinates(Self) -> LocalCoordinates
}

struct BetweenFactor<T: LieGroup> {
  var difference: T
  init(_ key1: Int, _ key2: Int, _ difference: T) { ... }
  func error(_ value: Value) -> T.LocalCoordinates {
    let v1 = values[key1].baseAs(T.self)
    let v2 = values[key2].baseAs(T.self)
    return difference.localCoordinates(v2.inverse() * v1)
  }
}

fg += BetweenFactor(0, 1, Pose2(...))
fg += BetweenFactor(2, 3, Rot2(...))

ProfFan · 2020-04-29T23:38:58Z

@marcrasi That means the "Manifold" type in #37 right? Maybe let's first concentrate on merging that one and then open another PR for the generic BetweenFactor, etc.

CC @dellaert

marcrasi · 2020-04-29T23:50:50Z

@marcrasi That means the "Manifold" type in #37 right?

BetweenFactor needs * and inverse() operations that don't exist on Manifold. But yes, the localCoordinates operation is the same as the Manifold local coordinates operation from #37. I think it would make sense to make a LieGroup protocol that refines Manifold. e.g.

protocol LieGroup: Manifold {
  static func * (Self, Self) -> Self
  func inverse() -> Self
}

Maybe let's first concentrate on merging that one and then open another PR for the generic BetweenFactor, etc.

Sounds good to me!

…actor_graph

ProfFan · 2020-05-11T22:47:05Z

Closed as we have #44

Add Nonlinear FG interface

0f1658b

ProfFan requested review from dellaert and marcrasi April 22, 2020 01:57

ProfFan added 6 commits April 21, 2020 21:58

Typo

a90be09

Add custom output for Values

16ef0d2

Fix test case

d6d4960

Linearization should be at 0,0 tangent

0b3a196

Working CGLS optimization

05f6824

PriorFactor is working

81e5ccf

ProfFan added 3 commits April 22, 2020 01:42

Add TODO

6ad7467

Merge remote-tracking branch 'origin/master' into feature/nonlinear_f…

d2544cb

…actor_graph

Remove prints

1ab2800

Minor changes in comments

534a48a

marcrasi reviewed Apr 22, 2020

View reviewed changes

ProfFan added 2 commits April 22, 2020 16:35

Remove Differentiable for NonlinearFactor

887c234

Add pseudo inverse

45b0276

dellaert reviewed Apr 24, 2020

View reviewed changes

marcrasi reviewed Apr 29, 2020

View reviewed changes

Remove AnyNonlinearFactor

cc969de

Merge remote-tracking branch 'origin/master' into feature/nonlinear_f…

c9815a1

…actor_graph

ProfFan mentioned this pull request May 8, 2020

NLFG + Manifold Demo #44

Merged

2 tasks

ProfFan closed this May 11, 2020


		import TensorFlow

		public func pinv(_ m: Tensor<Double>) -> Tensor<Double> {

Add Nonlinear FG interface #34

Add Nonlinear FG interface #34

Uh oh!

Conversation

ProfFan commented Apr 22, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

ProfFan commented Apr 22, 2020

Uh oh!

ProfFan commented Apr 22, 2020

Uh oh!

dellaert commented Apr 22, 2020

Uh oh!

marcrasi left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

dellaert commented Apr 24, 2020

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

marcrasi commented Apr 29, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

ProfFan commented Apr 22, 2020 •

edited

Loading

marcrasi commented Apr 29, 2020 •

edited

Loading