From 161115033a511e417f93c37d6aed0a2bc9081414 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 14 Jun 2020 23:11:53 -0400
Subject: [PATCH 1/9] Add LM Pose3Example and G2O 3D Reader

---
 Sources/Benchmarks/Pose3SLAM.swift            | 127 +++++++++++++++---
 .../SwiftFusion/Datasets/DatasetCache.swift   |   6 +-
 Sources/SwiftFusion/Datasets/G2OReader.swift  |  21 ++-
 .../Inference/NewFactorGraph.swift            |   2 +-
 .../Inference/NewGaussianFactorGraph.swift    |   2 +-
 5 files changed, 136 insertions(+), 22 deletions(-)

diff --git a/Sources/Benchmarks/Pose3SLAM.swift b/Sources/Benchmarks/Pose3SLAM.swift
index cc021137..db7935ba 100644
--- a/Sources/Benchmarks/Pose3SLAM.swift
+++ b/Sources/Benchmarks/Pose3SLAM.swift
@@ -16,13 +16,32 @@
 
 import Benchmark
 import SwiftFusion
+import PenguinStructures
 
-let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
+import Foundation
 
-  var gridDataset =
-    try! G2OReader.G2ONonlinearFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
-//  check(gridDataset.graph.error(gridDataset.initialGuess), near: 12.99, accuracy: 1e-2)
+struct FileHandlerOutputStream: TextOutputStream {
+  private let fileHandle: FileHandle
+  let encoding: String.Encoding
+  
+  init(_ fileHandle: FileHandle, encoding: String.Encoding = .utf8) {
+    self.fileHandle = fileHandle
+    self.encoding = encoding
+  }
+  
+  mutating func write(_ string: String) {
+    if let data = string.data(using: encoding) {
+      fileHandle.write(data)
+    }
+  }
+}
 
+let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
+  
+  var gridDataset =  // try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
+        try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("sphere_bignoise_vertex3.g2o"))
+  //  check(gridDataset.graph.error(gridDataset.initialGuess), near: 12.99, accuracy: 1e-2)
+  
   // Uses `NonlinearFactorGraph` on the Intel dataset.
   // The solvers are configured to run for a constant number of steps.
   // The nonlinear solver is 5 iterations of Gauss-Newton.
@@ -31,22 +50,96 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
     "NonlinearFactorGraph, Pose3Example, 50 Gauss-Newton steps, 200 CGLS steps",
     settings: Iterations(1)
   ) {
+    let fileManager = FileManager.default
+    var filePath = URL(fileURLWithPath: fileManager.currentDirectoryPath)
+    filePath.appendPathComponent("./result_pose3.txt")
+    
+    print("Storing result at \(filePath.path)")
+    
+    if fileManager.fileExists(atPath: filePath.path) {
+      do {
+        try fileManager.removeItem(atPath: filePath.path)
+      } catch let error {
+        print("error occurred, here are the details:\n \(error)")
+      }
+    }
+    
     var val = gridDataset.initialGuess
-    gridDataset.graph += PriorFactor(0, Pose3())
-    for _ in 0..<40 {
-      print("error = \(gridDataset.graph.error(val))")
-      let gfg = gridDataset.graph.linearize(val)
-      let optimizer = CGLS(precision: 0, max_iteration: 200)
-      var dx = VectorValues()
-      for i in 0..<val.count {
-        dx.insert(i, Vector(zeros: 6))
+    var graph = gridDataset.graph
+    
+    graph.store(NewPriorFactor3(TypedID(0), Pose3(Rot3.fromTangent(Vector3.zero), Vector3.zero)))
+    
+    var old_error = graph.error(at: val)
+    print("[LM OUTER] initial error = \(old_error)")
+    
+    var lambda = 1e-6
+    var inner_iter_step = 0
+    var inner_success = false
+    
+    for _ in 0..<20 { // outer loop
+      print("[LM OUTER] outer loop start, error = \(graph.error(at: val))")
+      let gfg = graph.linearized(at: val)
+      var dx = val.tangentVectorZeros
+      
+      for _ in 0..<6 {
+        print("[LM INNER] starting one iteration, lambda = \(lambda)")
+        var damped = gfg
+        
+        damped.addScalarJacobians(lambda)
+        
+        let old_linear_error = damped.errorVectors(at: dx).squaredNorm
+        
+        var dx_t = dx
+        var optimizer = GenericCGLS(precision: 0, max_iteration: 50)
+        optimizer.optimize(gfg: damped, initial: &dx_t)
+        print("[LM INNER] damped error = \(damped.errorVectors(at: dx_t).squaredNorm), lambda = \(lambda)")
+        var oldval = val
+        val.move(along: -1 * dx_t)
+        let this_error = graph.error(at: val)
+        let delta_error = old_error - this_error
+        print("[LM INNER] nonlinear error = \(this_error), delta error = \(delta_error)")
+        
+        let new_linear_error = damped.errorVectors(at: dx_t).squaredNorm
+        let model_fidelity = delta_error / (old_linear_error - new_linear_error)
+        
+        print("[LM INNER] model fidelity = \(model_fidelity)")
+        if delta_error > .ulpOfOne && model_fidelity > 0.01 {
+          old_error = this_error
+          
+          // Success, decrease lambda
+          if lambda > 1e-10 {
+            lambda = lambda / 10
+          } else {
+            break
+          }
+          inner_success = true
+        } else {
+          print("[LM INNER] fail, trying to increase lambda")
+          // increase lambda and retry
+          val = oldval
+          if lambda > 1e20 {
+            print("[LM INNER] giving up in lambda search")
+            break
+          }
+          lambda = lambda * 10
+        }
+        
+        inner_iter_step += 1
+        if inner_iter_step > 5 && inner_success {
+          break
+        }
       }
-      optimizer.optimize(gfg: gfg, initial: &dx)
-      print("gfg error = \(gfg.residual(dx).norm)")
-      val.move(along: dx)
     }
-    for i in val.keys.sorted() {
-      print(val[i, as: Pose3.self].t)
+    
+    print("[FINAL   ] final error = \(graph.error(at: val))")
+    
+    fileManager.createFile(atPath: filePath.path, contents: nil)
+    let fileHandle = try! FileHandle(forUpdating: URL(fileURLWithPath: filePath.path))
+    var output = FileHandlerOutputStream(fileHandle)
+    
+    for i in gridDataset.initialGuessId {
+      let t = val[i].t
+      output.write("\(t.x), \(t.y), \(t.z)\n")
     }
   }
 }
diff --git a/Sources/SwiftFusion/Datasets/DatasetCache.swift b/Sources/SwiftFusion/Datasets/DatasetCache.swift
index dfee1d5d..b6b85864 100644
--- a/Sources/SwiftFusion/Datasets/DatasetCache.swift
+++ b/Sources/SwiftFusion/Datasets/DatasetCache.swift
@@ -24,7 +24,11 @@ fileprivate let datasets = [
   "sphere_bignoise_vertex3.g2o":
     "https://github.com/HeYijia/GraphSLAM_tutorials_code/raw/master/g2o_test/data/sphere_bignoise_vertex3.g2o",
   "pose3example.txt":
-    "https://github.com/borglab/gtsam/raw/master/examples/Data/pose3example.txt"
+    "https://github.com/borglab/gtsam/raw/master/examples/Data/pose3example.txt",
+  "parking-garage.g2o":
+    "https://github.com/david-m-rosen/SE-Sync/raw/master/data/parking-garage.g2o",
+  "sphere2500.g2o":
+    "https://github.com/david-m-rosen/SE-Sync/raw/master/data/sphere2500.g2o"
 ]
 
 /// Returns a dataset cached on the local system.
diff --git a/Sources/SwiftFusion/Datasets/G2OReader.swift b/Sources/SwiftFusion/Datasets/G2OReader.swift
index 6059e575..628d5355 100644
--- a/Sources/SwiftFusion/Datasets/G2OReader.swift
+++ b/Sources/SwiftFusion/Datasets/G2OReader.swift
@@ -47,25 +47,42 @@ public enum G2OReader {
   }
 
   /// A G2O problem expressed as a `NewFactorGraph`.
-  public struct G2ONewFactorGraph {
+  public struct G2ONewFactorGraph<Pose: LieGroup> {
     /// The initial guess.
     public var initialGuess = VariableAssignments()
 
+    public var initialGuessId = Array<TypedID<Pose, Int>>()
+    
     /// The factor graph representing the measurements.
     public var graph = NewFactorGraph()
 
     /// Creates a problem from the given 2D file.
-    public init(g2oFile2D: URL) throws {
+    public init(g2oFile2D: URL) throws where Pose == Pose2 {
       try G2OReader.read2D(file: g2oFile2D) { entry in
         switch entry {
         case .initialGuess(index: let id, pose: let guess):
           let typedID = initialGuess.store(guess)
           assert(typedID.perTypeID == id)
+          initialGuessId.append(typedID)
         case .measurement(frameIndex: let id1, measuredIndex: let id2, pose: let difference):
           graph.store(NewBetweenFactor2(TypedID(id1), TypedID(id2), difference))
         }
       }
     }
+    
+    /// Creates a problem from the given 2D file.
+    public init(g2oFile3D: URL) throws where Pose == Pose3 {
+      try G2OReader.read3D(file: g2oFile3D) { entry in
+        switch entry {
+        case .initialGuess(index: let id, pose: let guess):
+          let typedID = initialGuess.store(guess)
+          assert(typedID.perTypeID == id)
+          initialGuessId.append(typedID)
+        case .measurement(frameIndex: let id1, measuredIndex: let id2, pose: let difference):
+          graph.store(NewBetweenFactor3(TypedID(id1), TypedID(id2), difference))
+        }
+      }
+    }
   }
 
   /// An entry in a G2O file.
diff --git a/Sources/SwiftFusion/Inference/NewFactorGraph.swift b/Sources/SwiftFusion/Inference/NewFactorGraph.swift
index ca17e2ba..26afc911 100644
--- a/Sources/SwiftFusion/Inference/NewFactorGraph.swift
+++ b/Sources/SwiftFusion/Inference/NewFactorGraph.swift
@@ -57,7 +57,7 @@ public struct NewFactorGraph {
   }
 
   /// Returns the error vectors, at `x`, of all the linearizable factors.
-  func errorVectors(at x: VariableAssignments) -> AllVectors {
+  public func errorVectors(at x: VariableAssignments) -> AllVectors {
     return AllVectors(storage: storage.compactMapValues { factors in
       guard let linearizableFactors = factors.cast(to: AnyLinearizableFactorStorage.self) else {
         return nil
diff --git a/Sources/SwiftFusion/Inference/NewGaussianFactorGraph.swift b/Sources/SwiftFusion/Inference/NewGaussianFactorGraph.swift
index a0625247..e27f971d 100644
--- a/Sources/SwiftFusion/Inference/NewGaussianFactorGraph.swift
+++ b/Sources/SwiftFusion/Inference/NewGaussianFactorGraph.swift
@@ -36,7 +36,7 @@ public struct NewGaussianFactorGraph {
   }
 
   /// Returns the error vectors, at `x`, of all the factors.
-  func errorVectors(at x: AllVectors) -> AllVectors {
+  public func errorVectors(at x: AllVectors) -> AllVectors {
     return AllVectors(storage: storage.mapValues { factors in
       AnyArrayBuffer(factors.errorVectors(at: x))
     })

From 5088991c65d5402acda22687fb9aa9f9a386a049 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 15 Jun 2020 11:41:03 -0400
Subject: [PATCH 2/9] Fixed LM and CGLS error, added tests

---
 Sources/Benchmarks/Pose3SLAM.swift            | 13 ++--
 .../Inference/NewJacobianFactor.swift         |  2 +-
 Sources/SwiftFusion/Optimizers/CGLS.swift     |  2 +
 .../Inference/NewFactorGraphTests.swift       | 67 +++++++++++++++++++
 .../Inference/NewFactorTests.swift            |  5 +-
 5 files changed, 80 insertions(+), 9 deletions(-)

diff --git a/Sources/Benchmarks/Pose3SLAM.swift b/Sources/Benchmarks/Pose3SLAM.swift
index db7935ba..b90a4795 100644
--- a/Sources/Benchmarks/Pose3SLAM.swift
+++ b/Sources/Benchmarks/Pose3SLAM.swift
@@ -39,7 +39,7 @@ struct FileHandlerOutputStream: TextOutputStream {
 let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   
   var gridDataset =  // try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
-        try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("sphere_bignoise_vertex3.g2o"))
+        try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("sphere2500.g2o"))
   //  check(gridDataset.graph.error(gridDataset.initialGuess), near: 12.99, accuracy: 1e-2)
   
   // Uses `NonlinearFactorGraph` on the Intel dataset.
@@ -47,7 +47,7 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   // The nonlinear solver is 5 iterations of Gauss-Newton.
   // The linear solver is 100 iterations of CGLS.
   suite.benchmark(
-    "NonlinearFactorGraph, Pose3Example, 50 Gauss-Newton steps, 200 CGLS steps",
+    "NonlinearFactorGraph, Pose3Example, 30 LM steps, max 6 G-N steps, 200 CGLS steps",
     settings: Iterations(1)
   ) {
     let fileManager = FileManager.default
@@ -76,7 +76,7 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
     var inner_iter_step = 0
     var inner_success = false
     
-    for _ in 0..<20 { // outer loop
+    for _ in 0..<30 { // outer loop
       print("[LM OUTER] outer loop start, error = \(graph.error(at: val))")
       let gfg = graph.linearized(at: val)
       var dx = val.tangentVectorZeros
@@ -90,7 +90,7 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
         let old_linear_error = damped.errorVectors(at: dx).squaredNorm
         
         var dx_t = dx
-        var optimizer = GenericCGLS(precision: 0, max_iteration: 50)
+        var optimizer = GenericCGLS(precision: 0, max_iteration: 200)
         optimizer.optimize(gfg: damped, initial: &dx_t)
         print("[LM INNER] damped error = \(damped.errorVectors(at: dx_t).squaredNorm), lambda = \(lambda)")
         var oldval = val
@@ -102,7 +102,10 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
         let new_linear_error = damped.errorVectors(at: dx_t).squaredNorm
         let model_fidelity = delta_error / (old_linear_error - new_linear_error)
         
+        print("[LM INNER] linear error = \(new_linear_error), delta error = \(old_linear_error - new_linear_error)")
         print("[LM INNER] model fidelity = \(model_fidelity)")
+        
+        inner_success = false
         if delta_error > .ulpOfOne && model_fidelity > 0.01 {
           old_error = this_error
           
@@ -125,7 +128,7 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
         }
         
         inner_iter_step += 1
-        if inner_iter_step > 5 && inner_success {
+        if inner_iter_step > 5 || inner_success {
           break
         }
       }
diff --git a/Sources/SwiftFusion/Inference/NewJacobianFactor.swift b/Sources/SwiftFusion/Inference/NewJacobianFactor.swift
index 0ba5aa10..4c0eea04 100644
--- a/Sources/SwiftFusion/Inference/NewJacobianFactor.swift
+++ b/Sources/SwiftFusion/Inference/NewJacobianFactor.swift
@@ -62,7 +62,7 @@ public struct NewJacobianFactor<
   }
 
   public func errorVector(at x: Variables) -> ErrorVector {
-    return errorVector_linearComponent(x) + error
+    return error - errorVector_linearComponent(x)
   }
 
   public func errorVector_linearComponent(_ x: Variables) -> ErrorVector {
diff --git a/Sources/SwiftFusion/Optimizers/CGLS.swift b/Sources/SwiftFusion/Optimizers/CGLS.swift
index 022edbe7..d0ec4973 100644
--- a/Sources/SwiftFusion/Optimizers/CGLS.swift
+++ b/Sources/SwiftFusion/Optimizers/CGLS.swift
@@ -45,6 +45,7 @@ public class CGLS {
     var gamma = s.norm // γ(0) = ||s(0)||^2
     
     while step < max_iteration {
+      print("[CGLS    ] residual = \(r.norm), true = \(gfg.residual(x).norm)")
       let q = gfg * p // q(k) = A * p(k)
       let alpha: Double = gamma / q.norm // α(k) = γ(k)/||q(k)||^2
       x = x + (alpha * p) // x(k+1) = x(k) + α(k) * p(k)
@@ -93,6 +94,7 @@ public struct GenericCGLS {
     var gamma = s.squaredNorm // γ(0) = ||s(0)||^2
 
     while step < max_iteration {
+      // print("[CGLS    ] residual = \(r.squaredNorm), true = \(gfg.errorVectors(at: x).squaredNorm)")
       let q = gfg.errorVectors_linearComponent(at: p) // q(k) = A * p(k)
       let alpha: Double = gamma / q.squaredNorm // α(k) = γ(k)/||q(k)||^2
       x = x + (alpha * p) // x(k+1) = x(k) + α(k) * p(k)
diff --git a/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift b/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift
index 91bf1462..1581e0ab 100644
--- a/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift
+++ b/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift
@@ -46,4 +46,71 @@ class NewFactorGraphTests: XCTestCase {
     // Test condition: pose 5 should be identical to pose 1 (close loop).
     XCTAssertEqual(between(x[pose1ID], x[pose5ID]).t.norm, 0.0, accuracy: 1e-2)
   }
+  
+  /// circlePose3 generates a set of poses in a circle. This function
+  /// returns those poses inside a gtsam.Values object, with sequential
+  /// keys starting from 0. An optional character may be provided, which
+  /// will be stored in the msb of each key (i.e. gtsam.Symbol).
+
+  /// We use aerospace/navlab convention, X forward, Y right, Z down
+  /// First pose will be at (R,0,0)
+  /// ^y   ^ X
+  /// |    |
+  /// z-->xZ--> Y  (z pointing towards viewer, Z pointing away from viewer)
+  /// Vehicle at p0 is looking towards y axis (X-axis points towards world y)
+  func circlePose3(numPoses: Int = 8, radius: Double = 1.0) -> Values {
+    var values = Values()
+    var theta = 0.0
+    let dtheta = 2.0 * .pi / Double(numPoses)
+    let gRo = Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1)
+    for i in 0..<numPoses {
+      let key = 0 + i
+      let gti = Vector3(radius * cos(theta), radius * sin(theta), 0)
+      let oRi = Rot3.fromTangent(Vector3(0, 0, -theta))  // negative yaw goes counterclockwise, with Z down !
+      let gTi = Pose3(gRo * oRi, gti)
+      values.insert(key, gTi)
+      theta = theta + dtheta
+    }
+    return values
+  }
+  
+  func testGtsamPose3SLAMExample() {
+    // Create a hexagon of poses
+    let hexagon = circlePose3(numPoses: 6, radius: 1.0)
+    let p0 = hexagon[0, as: Pose3.self]
+    let p1 = hexagon[1, as: Pose3.self]
+    
+    var x = VariableAssignments()
+    
+    let s = 0.10
+    let id0 = x.store(p0)
+    let id1 = x.store(hexagon[1, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id2 = x.store(hexagon[2, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id3 = x.store(hexagon[3, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id4 = x.store(hexagon[4, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id5 = x.store(hexagon[5, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    
+    var fg = NewFactorGraph()
+    fg.store(NewPriorFactor3(id0, p0))
+    let delta: Pose3 = between(p0, p1)
+
+    fg.store(NewBetweenFactor3(id0, id1, delta))
+    fg.store(NewBetweenFactor3(id1, id2, delta))
+    fg.store(NewBetweenFactor3(id2, id3, delta))
+    fg.store(NewBetweenFactor3(id3, id4, delta))
+    fg.store(NewBetweenFactor3(id4, id5, delta))
+    fg.store(NewBetweenFactor3(id5, id0, delta))
+
+    // optimize
+    for _ in 0..<16 {
+      let gfg = fg.linearized(at: x)
+      var dx = x.tangentVectorZeros
+      var optimizer = GenericCGLS(precision: 1e-6, max_iteration: 500)
+      optimizer.optimize(gfg: gfg, initial: &dx)
+      x.move(along: (-1) * dx)
+    }
+
+    let pose_1 = x[id1]
+    assertAllKeyPathEqual(pose_1, p1, accuracy: 1e-2)
+  }
 }
diff --git a/Tests/SwiftFusionTests/Inference/NewFactorTests.swift b/Tests/SwiftFusionTests/Inference/NewFactorTests.swift
index dcf35874..0a9a1dea 100644
--- a/Tests/SwiftFusionTests/Inference/NewFactorTests.swift
+++ b/Tests/SwiftFusionTests/Inference/NewFactorTests.swift
@@ -183,9 +183,8 @@ class NewFactorTests: XCTestCase {
     ))
 
     let errorVectors = factors.errorVectors(at: variableAssignments)
-    XCTAssertEqual(errorVectors[0], Vector2(3, 2))  // matrix1 * [1 2] + [0 0]
-    XCTAssertEqual(errorVectors[1], Vector2(106, 207))  // matrix2 * [1 2] + [100 200]
-
+    XCTAssertEqual(errorVectors[0], Vector2(-3, -2))  // zero - matrix1 * [1 2]
+    XCTAssertEqual(errorVectors[1], Vector2(94, 193))  // [100 200] - matrix2 * [1 2]
     let forwardResult = factors.errorVector_linearComponent(variableAssignments)
     XCTAssertEqual(forwardResult[0], Vector2(3, 2))  // matrix1 * [1 2]
     XCTAssertEqual(forwardResult[1], Vector2(6, 7))  // matrix2 * [1 2]

From 21bc5fae5cf569d7a9bb991ddbb8379eb9d7c063 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 15 Jun 2020 18:11:58 -0400
Subject: [PATCH 3/9] Compartmentized the LM Solver

---
 Examples/Pose3SLAMG2O/main.swift        |  92 +++++++++++++++
 Package.swift                           |   4 +
 Sources/Benchmarks/Pose3SLAM.swift      |  71 +----------
 Sources/SwiftFusion/Optimizers/LM.swift | 151 ++++++++++++++++++++++++
 4 files changed, 253 insertions(+), 65 deletions(-)
 create mode 100644 Examples/Pose3SLAMG2O/main.swift
 create mode 100644 Sources/SwiftFusion/Optimizers/LM.swift

diff --git a/Examples/Pose3SLAMG2O/main.swift b/Examples/Pose3SLAMG2O/main.swift
new file mode 100644
index 00000000..85b2b3a4
--- /dev/null
+++ b/Examples/Pose3SLAMG2O/main.swift
@@ -0,0 +1,92 @@
+// Copyright 2020 The SwiftFusion Authors. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// Loads a g2o file into a factor graph and then runs inference on the factor graph.
+///
+/// See https://lucacarlone.mit.edu/datasets/ for g2o specification and example datasets.
+///
+/// Usage: Pose3SLAMG2O [path to .g2o file]
+///
+/// Missing features:
+/// - Does not take g2o information matrix into account.
+/// - Does not use a proper general purpose solver.
+/// - Has not been compared against other implementations, so it could be wrong.
+
+import Foundation
+import SwiftFusion
+import TensorFlow
+
+struct FileHandlerOutputStream: TextOutputStream {
+  private let fileHandle: FileHandle
+  let encoding: String.Encoding
+  
+  init(_ fileHandle: FileHandle, encoding: String.Encoding = .utf8) {
+    self.fileHandle = fileHandle
+    self.encoding = encoding
+  }
+  
+  mutating func write(_ string: String) {
+    if let data = string.data(using: encoding) {
+      fileHandle.write(data)
+    }
+  }
+}
+
+func main() {
+  // Parse commandline.
+  guard CommandLine.arguments.count == 3 else {
+    print("Usage: Pose3SLAMG2O [path to .g2o file] [path to output csv file]")
+    return
+  }
+  let g2oURL = URL(fileURLWithPath: CommandLine.arguments[1])
+  let fileManager = FileManager.default
+  let filePath = URL(fileURLWithPath: CommandLine.arguments[2])
+  
+  print("Storing result at \(filePath.path)")
+  
+  if fileManager.fileExists(atPath: filePath.path) {
+    do {
+      try fileManager.removeItem(atPath: filePath.path)
+    } catch let error {
+      print("error occurred, here are the details:\n \(error)")
+    }
+  }
+  
+  // Load .g2o file.
+  let problem = try! G2OReader.G2ONewFactorGraph(g2oFile3D: g2oURL)
+
+  var val = problem.initialGuess
+  var graph = problem.graph
+  
+  graph.store(NewPriorFactor3(TypedID(0), Pose3(Rot3.fromTangent(Vector3.zero), Vector3.zero)))
+  
+  var optimizer = LM()
+  
+  do {
+    try optimizer.optimize(graph: graph, initial: &val)
+  } catch let error {
+    print("The solver gave up, message: \(error.localizedDescription)")
+  }
+  
+  fileManager.createFile(atPath: filePath.path, contents: nil)
+  let fileHandle = try! FileHandle(forUpdating: URL(fileURLWithPath: filePath.path))
+  var output = FileHandlerOutputStream(fileHandle)
+  
+  for i in problem.initialGuessId {
+    let t = val[i].t
+    output.write("\(t.x), \(t.y), \(t.z)\n")
+  }
+}
+
+main()
diff --git a/Package.swift b/Package.swift
index d7d35585..6041a372 100644
--- a/Package.swift
+++ b/Package.swift
@@ -37,6 +37,10 @@ let package = Package(
       name: "Pose2SLAMG2O",
       dependencies: ["SwiftFusion"],
       path: "Examples/Pose2SLAMG2O"),
+    .target(
+      name: "Pose3SLAMG2O",
+      dependencies: ["SwiftFusion"],
+      path: "Examples/Pose3SLAMG2O"),
     .testTarget(
       name: "SwiftFusionTests",
       dependencies: ["SwiftFusion"]),
diff --git a/Sources/Benchmarks/Pose3SLAM.swift b/Sources/Benchmarks/Pose3SLAM.swift
index b90a4795..712c3599 100644
--- a/Sources/Benchmarks/Pose3SLAM.swift
+++ b/Sources/Benchmarks/Pose3SLAM.swift
@@ -47,7 +47,7 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   // The nonlinear solver is 5 iterations of Gauss-Newton.
   // The linear solver is 100 iterations of CGLS.
   suite.benchmark(
-    "NonlinearFactorGraph, Pose3Example, 30 LM steps, max 6 G-N steps, 200 CGLS steps",
+    "NewFactorGraph, sphere2500, 30 LM steps, max 6 G-N steps, 200 CGLS steps",
     settings: Iterations(1)
   ) {
     let fileManager = FileManager.default
@@ -69,73 +69,14 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
     
     graph.store(NewPriorFactor3(TypedID(0), Pose3(Rot3.fromTangent(Vector3.zero), Vector3.zero)))
     
-    var old_error = graph.error(at: val)
-    print("[LM OUTER] initial error = \(old_error)")
+    var optimizer = LM()
     
-    var lambda = 1e-6
-    var inner_iter_step = 0
-    var inner_success = false
-    
-    for _ in 0..<30 { // outer loop
-      print("[LM OUTER] outer loop start, error = \(graph.error(at: val))")
-      let gfg = graph.linearized(at: val)
-      var dx = val.tangentVectorZeros
-      
-      for _ in 0..<6 {
-        print("[LM INNER] starting one iteration, lambda = \(lambda)")
-        var damped = gfg
-        
-        damped.addScalarJacobians(lambda)
-        
-        let old_linear_error = damped.errorVectors(at: dx).squaredNorm
-        
-        var dx_t = dx
-        var optimizer = GenericCGLS(precision: 0, max_iteration: 200)
-        optimizer.optimize(gfg: damped, initial: &dx_t)
-        print("[LM INNER] damped error = \(damped.errorVectors(at: dx_t).squaredNorm), lambda = \(lambda)")
-        var oldval = val
-        val.move(along: -1 * dx_t)
-        let this_error = graph.error(at: val)
-        let delta_error = old_error - this_error
-        print("[LM INNER] nonlinear error = \(this_error), delta error = \(delta_error)")
-        
-        let new_linear_error = damped.errorVectors(at: dx_t).squaredNorm
-        let model_fidelity = delta_error / (old_linear_error - new_linear_error)
-        
-        print("[LM INNER] linear error = \(new_linear_error), delta error = \(old_linear_error - new_linear_error)")
-        print("[LM INNER] model fidelity = \(model_fidelity)")
-        
-        inner_success = false
-        if delta_error > .ulpOfOne && model_fidelity > 0.01 {
-          old_error = this_error
-          
-          // Success, decrease lambda
-          if lambda > 1e-10 {
-            lambda = lambda / 10
-          } else {
-            break
-          }
-          inner_success = true
-        } else {
-          print("[LM INNER] fail, trying to increase lambda")
-          // increase lambda and retry
-          val = oldval
-          if lambda > 1e20 {
-            print("[LM INNER] giving up in lambda search")
-            break
-          }
-          lambda = lambda * 10
-        }
-        
-        inner_iter_step += 1
-        if inner_iter_step > 5 || inner_success {
-          break
-        }
-      }
+    do {
+      try optimizer.optimize(graph: graph, initial: &val)
+    } catch let error {
+      print("The solver gave up, message: \(error.localizedDescription)")
     }
     
-    print("[FINAL   ] final error = \(graph.error(at: val))")
-    
     fileManager.createFile(atPath: filePath.path, contents: nil)
     let fileHandle = try! FileHandle(forUpdating: URL(fileURLWithPath: filePath.path))
     var output = FileHandlerOutputStream(fileHandle)
diff --git a/Sources/SwiftFusion/Optimizers/LM.swift b/Sources/SwiftFusion/Optimizers/LM.swift
new file mode 100644
index 00000000..084a49a6
--- /dev/null
+++ b/Sources/SwiftFusion/Optimizers/LM.swift
@@ -0,0 +1,151 @@
+// Copyright 2019 The SwiftFusion Authors. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+import Foundation
+
+/// An error from a failure in searching for the solution in LM iterations
+public struct LevenbergMarquardtError: Swift.Error {
+  public let message: String
+}
+
+/// Levenberg-Marquadt optimizer
+///
+/// Implements the Levenberg-Marquardt algorithm for optimizing non-linear functions.
+public struct LM {
+  /// The set of steps taken.
+  public var step: Int = 0
+  
+  /// Desired precision, TODO(fan): make this actually work
+  public var precision: Double = 1e-10
+  
+  /// Maximum number of L-M iterations
+  public var max_iteration: Int = 50
+  
+  /// Maximum number of G-N iterations
+  public var max_inner_iteration: Int = 400
+  
+  /// Type of the verbosity of the logging inside the optimizer
+  public enum Verbosity: Int, Comparable {
+    case SILENT = 0 , SUMMARY, TRYLAMBDA
+    
+    // Implement Comparable
+    public static func < (a: Verbosity, b: Verbosity) -> Bool {
+        return a.rawValue < b.rawValue
+    }
+  }
+  
+  /// Verbosity of the logging
+  public var verbosity: Verbosity = .SILENT
+  
+  public init(precision p: Double = 1e-10, max_iteration maxiter: Int = 50) {
+    self.precision = p
+    self.max_iteration = maxiter
+    self.step = 0
+  }
+  
+  public mutating func optimize(graph: NewFactorGraph, initial val: inout VariableAssignments) throws {
+    var old_error = graph.error(at: val)
+    
+    if verbosity >= .SUMMARY {
+      print("[LM OUTER] initial error = \(old_error)")
+    }
+    
+    var lambda = 1e-6
+    var inner_iter_step = 0
+    var inner_success = false
+    
+    for _ in 0..<max_iteration { // outer loop
+      
+      if verbosity >= .SUMMARY {
+        print("[LM OUTER] outer loop start, error = \(graph.error(at: val))")
+      }
+      
+      let gfg = graph.linearized(at: val)
+      let dx = val.tangentVectorZeros
+      
+      // Try lambda steps
+      while true {
+        if verbosity >= .TRYLAMBDA {
+          print("[LM INNER] starting one iteration, lambda = \(lambda)")
+        }
+        
+        var damped = gfg
+        
+        damped.addScalarJacobians(lambda)
+        
+        let old_linear_error = damped.errorVectors(at: dx).squaredNorm
+        
+        var dx_t = dx
+        var optimizer = GenericCGLS(precision: 0, max_iteration: 200)
+        optimizer.optimize(gfg: damped, initial: &dx_t)
+        if verbosity >= .TRYLAMBDA {
+          print("[LM INNER] damped error = \(damped.errorVectors(at: dx_t).squaredNorm), lambda = \(lambda)")
+        }
+        let oldval = val
+        val.move(along: -1 * dx_t)
+        let this_error = graph.error(at: val)
+        let delta_error = old_error - this_error
+        
+        if verbosity >= .TRYLAMBDA {
+          print("[LM INNER] nonlinear error = \(this_error), delta error = \(delta_error)")
+        }
+        
+        let new_linear_error = damped.errorVectors(at: dx_t).squaredNorm
+        let model_fidelity = delta_error / (old_linear_error - new_linear_error)
+        
+        if verbosity >= .TRYLAMBDA {
+          print("[LM INNER] linear error = \(new_linear_error), delta error = \(old_linear_error - new_linear_error)")
+          print("[LM INNER] model fidelity = \(model_fidelity)")
+        }
+        
+        inner_success = false
+        if delta_error > .ulpOfOne && model_fidelity > 0.01 {
+          old_error = this_error
+          
+          // Success, decrease lambda
+          if lambda > 1e-10 {
+            lambda = lambda / 10
+          } else {
+            break
+          }
+          inner_success = true
+        } else {
+          if verbosity >= .TRYLAMBDA {
+            print("[LM INNER] fail, trying to increase lambda")
+          }
+          // increase lambda and retry
+          val = oldval
+          if lambda > 1e20 {
+            if verbosity >= .TRYLAMBDA {
+              print("[LM INNER] giving up in lambda search")
+            }
+            throw LevenbergMarquardtError(message: "maximum lambda reached, giving up")
+          }
+          lambda = lambda * 10
+        }
+        
+        inner_iter_step += 1
+        if inner_iter_step > 5 || inner_success {
+          break
+        }
+      }
+      
+      step += 1
+    }
+    
+    if verbosity >= .SUMMARY {
+      print("[FINAL   ] final error = \(graph.error(at: val))")
+    }
+  }
+}

From 755806b4358877671dda5eb08eec2b44a88611ab Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 15 Jun 2020 18:13:35 -0400
Subject: [PATCH 4/9] Remove file writing from benchmark

---
 Sources/Benchmarks/Pose3SLAM.swift | 42 ------------------------------
 1 file changed, 42 deletions(-)

diff --git a/Sources/Benchmarks/Pose3SLAM.swift b/Sources/Benchmarks/Pose3SLAM.swift
index 712c3599..a6658a54 100644
--- a/Sources/Benchmarks/Pose3SLAM.swift
+++ b/Sources/Benchmarks/Pose3SLAM.swift
@@ -16,25 +16,6 @@
 
 import Benchmark
 import SwiftFusion
-import PenguinStructures
-
-import Foundation
-
-struct FileHandlerOutputStream: TextOutputStream {
-  private let fileHandle: FileHandle
-  let encoding: String.Encoding
-  
-  init(_ fileHandle: FileHandle, encoding: String.Encoding = .utf8) {
-    self.fileHandle = fileHandle
-    self.encoding = encoding
-  }
-  
-  mutating func write(_ string: String) {
-    if let data = string.data(using: encoding) {
-      fileHandle.write(data)
-    }
-  }
-}
 
 let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   
@@ -50,20 +31,6 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
     "NewFactorGraph, sphere2500, 30 LM steps, max 6 G-N steps, 200 CGLS steps",
     settings: Iterations(1)
   ) {
-    let fileManager = FileManager.default
-    var filePath = URL(fileURLWithPath: fileManager.currentDirectoryPath)
-    filePath.appendPathComponent("./result_pose3.txt")
-    
-    print("Storing result at \(filePath.path)")
-    
-    if fileManager.fileExists(atPath: filePath.path) {
-      do {
-        try fileManager.removeItem(atPath: filePath.path)
-      } catch let error {
-        print("error occurred, here are the details:\n \(error)")
-      }
-    }
-    
     var val = gridDataset.initialGuess
     var graph = gridDataset.graph
     
@@ -76,14 +43,5 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
     } catch let error {
       print("The solver gave up, message: \(error.localizedDescription)")
     }
-    
-    fileManager.createFile(atPath: filePath.path, contents: nil)
-    let fileHandle = try! FileHandle(forUpdating: URL(fileURLWithPath: filePath.path))
-    var output = FileHandlerOutputStream(fileHandle)
-    
-    for i in gridDataset.initialGuessId {
-      let t = val[i].t
-      output.write("\(t.x), \(t.y), \(t.z)\n")
-    }
   }
 }

From 1dd5e2eca51c677d06c1829b1a34d3dd7cf1b015 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 15 Jun 2020 18:16:09 -0400
Subject: [PATCH 5/9] Fixed max inner iterations

---
 Sources/Benchmarks/Pose3SLAM.swift      | 5 ++++-
 Sources/SwiftFusion/Optimizers/LM.swift | 4 ++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/Sources/Benchmarks/Pose3SLAM.swift b/Sources/Benchmarks/Pose3SLAM.swift
index a6658a54..88154f6d 100644
--- a/Sources/Benchmarks/Pose3SLAM.swift
+++ b/Sources/Benchmarks/Pose3SLAM.swift
@@ -28,7 +28,7 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   // The nonlinear solver is 5 iterations of Gauss-Newton.
   // The linear solver is 100 iterations of CGLS.
   suite.benchmark(
-    "NewFactorGraph, sphere2500, 30 LM steps, max 6 G-N steps, 200 CGLS steps",
+    "NewFactorGraph, sphere2500, 30 LM steps, 200 CGLS steps",
     settings: Iterations(1)
   ) {
     var val = gridDataset.initialGuess
@@ -37,6 +37,9 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
     graph.store(NewPriorFactor3(TypedID(0), Pose3(Rot3.fromTangent(Vector3.zero), Vector3.zero)))
     
     var optimizer = LM()
+    optimizer.verbosity = .SUMMARY
+    optimizer.max_iteration = 30
+    optimizer.max_inner_iteration = 200
     
     do {
       try optimizer.optimize(graph: graph, initial: &val)
diff --git a/Sources/SwiftFusion/Optimizers/LM.swift b/Sources/SwiftFusion/Optimizers/LM.swift
index 084a49a6..33af72c1 100644
--- a/Sources/SwiftFusion/Optimizers/LM.swift
+++ b/Sources/SwiftFusion/Optimizers/LM.swift
@@ -87,7 +87,7 @@ public struct LM {
         let old_linear_error = damped.errorVectors(at: dx).squaredNorm
         
         var dx_t = dx
-        var optimizer = GenericCGLS(precision: 0, max_iteration: 200)
+        var optimizer = GenericCGLS(precision: 0, max_iteration: max_inner_iteration)
         optimizer.optimize(gfg: damped, initial: &dx_t)
         if verbosity >= .TRYLAMBDA {
           print("[LM INNER] damped error = \(damped.errorVectors(at: dx_t).squaredNorm), lambda = \(lambda)")
@@ -136,7 +136,7 @@ public struct LM {
         }
         
         inner_iter_step += 1
-        if inner_iter_step > 5 || inner_success {
+        if inner_success {
           break
         }
       }

From db37d8f2565cec7d563cd530d50a140d60441bfb Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 15 Jun 2020 18:17:23 -0400
Subject: [PATCH 6/9] Comment out debug code

---
 Sources/SwiftFusion/Optimizers/CGLS.swift | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Sources/SwiftFusion/Optimizers/CGLS.swift b/Sources/SwiftFusion/Optimizers/CGLS.swift
index d0ec4973..f2820ec9 100644
--- a/Sources/SwiftFusion/Optimizers/CGLS.swift
+++ b/Sources/SwiftFusion/Optimizers/CGLS.swift
@@ -45,7 +45,7 @@ public class CGLS {
     var gamma = s.norm // γ(0) = ||s(0)||^2
     
     while step < max_iteration {
-      print("[CGLS    ] residual = \(r.norm), true = \(gfg.residual(x).norm)")
+      // print("[CGLS    ] residual = \(r.norm), true = \(gfg.residual(x).norm)")
       let q = gfg * p // q(k) = A * p(k)
       let alpha: Double = gamma / q.norm // α(k) = γ(k)/||q(k)||^2
       x = x + (alpha * p) // x(k+1) = x(k) + α(k) * p(k)

From eb94c3f3d377c96399124f723db212171f3e41ec Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 15 Jun 2020 18:36:12 -0400
Subject: [PATCH 7/9] Add back old benchmark

---
 Examples/Pose3SLAMG2O/main.swift   |  1 +
 Sources/Benchmarks/Pose3SLAM.swift | 34 ++++++++++++++++++++++++++++--
 2 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/Examples/Pose3SLAMG2O/main.swift b/Examples/Pose3SLAMG2O/main.swift
index 85b2b3a4..25aa7aa3 100644
--- a/Examples/Pose3SLAMG2O/main.swift
+++ b/Examples/Pose3SLAMG2O/main.swift
@@ -72,6 +72,7 @@ func main() {
   graph.store(NewPriorFactor3(TypedID(0), Pose3(Rot3.fromTangent(Vector3.zero), Vector3.zero)))
   
   var optimizer = LM()
+  optimizer.verbosity = .TRYLAMBDA
   
   do {
     try optimizer.optimize(graph: graph, initial: &val)
diff --git a/Sources/Benchmarks/Pose3SLAM.swift b/Sources/Benchmarks/Pose3SLAM.swift
index 88154f6d..e9714f28 100644
--- a/Sources/Benchmarks/Pose3SLAM.swift
+++ b/Sources/Benchmarks/Pose3SLAM.swift
@@ -19,8 +19,38 @@ import SwiftFusion
 
 let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   
-  var gridDataset =  // try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
-        try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("sphere2500.g2o"))
+  var gridDataset_old =
+    try! G2OReader.G2ONonlinearFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
+  // check(gridDataset_old.graph.error(gridDataset_old.initialGuess), near: 12.99, accuracy: 1e-2)
+  // Uses `NonlinearFactorGraph` on the Intel dataset.
+  // The solvers are configured to run for a constant number of steps.
+  // The nonlinear solver is 5 iterations of Gauss-Newton.
+  // The linear solver is 100 iterations of CGLS.
+  suite.benchmark(
+    "NonlinearFactorGraph, Pose3Example, 50 Gauss-Newton steps, 200 CGLS steps",
+    settings: Iterations(1)
+  ) {
+    var val = gridDataset_old.initialGuess
+    gridDataset_old.graph += PriorFactor(0, Pose3())
+    for _ in 0..<40 {
+      print("error = \(gridDataset_old.graph.error(val))")
+      let gfg = gridDataset_old.graph.linearize(val)
+      let optimizer = CGLS(precision: 0, max_iteration: 200)
+      var dx = VectorValues()
+      for i in 0..<val.count {
+        dx.insert(i, Vector(zeros: 6))
+      }
+      optimizer.optimize(gfg: gfg, initial: &dx)
+      print("gfg error = \(gfg.residual(dx).norm)")
+      val.move(along: dx)
+    }
+    for i in val.keys.sorted() {
+      print(val[i, as: Pose3.self].t)
+    }
+  }
+  
+  var gridDataset =  try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
+        // try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("sphere2500.g2o"))
   //  check(gridDataset.graph.error(gridDataset.initialGuess), near: 12.99, accuracy: 1e-2)
   
   // Uses `NonlinearFactorGraph` on the Intel dataset.

From e5696af94d19430e4b5c56f3e20c822abf62a520 Mon Sep 17 00:00:00 2001
From: Fan Jiang <i@fanjiang.me>
Date: Tue, 16 Jun 2020 00:04:04 -0400
Subject: [PATCH 8/9] Address Marc's comments

---
 Sources/Benchmarks/Pose3SLAM.swift            | 20 +++++--------
 Sources/SwiftFusion/Datasets/G2OReader.swift  |  8 ++---
 .../Inference/NewFactorGraphTests.swift       | 29 ++++++++++---------
 3 files changed, 27 insertions(+), 30 deletions(-)

diff --git a/Sources/Benchmarks/Pose3SLAM.swift b/Sources/Benchmarks/Pose3SLAM.swift
index e9714f28..509b40fc 100644
--- a/Sources/Benchmarks/Pose3SLAM.swift
+++ b/Sources/Benchmarks/Pose3SLAM.swift
@@ -21,13 +21,12 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   
   var gridDataset_old =
     try! G2OReader.G2ONonlinearFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
-  // check(gridDataset_old.graph.error(gridDataset_old.initialGuess), near: 12.99, accuracy: 1e-2)
-  // Uses `NonlinearFactorGraph` on the Intel dataset.
+  // Uses `NonlinearFactorGraph` on the GTSAM pose3example dataset.
   // The solvers are configured to run for a constant number of steps.
-  // The nonlinear solver is 5 iterations of Gauss-Newton.
-  // The linear solver is 100 iterations of CGLS.
+  // The nonlinear solver is 40 iterations of Gauss-Newton.
+  // The linear solver is 200 iterations of CGLS.
   suite.benchmark(
-    "NonlinearFactorGraph, Pose3Example, 50 Gauss-Newton steps, 200 CGLS steps",
+    "NonlinearFactorGraph, Pose3Example, 40 Gauss-Newton steps, 200 CGLS steps",
     settings: Iterations(1)
   ) {
     var val = gridDataset_old.initialGuess
@@ -50,13 +49,10 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
   }
   
   var gridDataset =  try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
-        // try! G2OReader.G2ONewFactorGraph(g2oFile3D: try! cachedDataset("sphere2500.g2o"))
-  //  check(gridDataset.graph.error(gridDataset.initialGuess), near: 12.99, accuracy: 1e-2)
-  
-  // Uses `NonlinearFactorGraph` on the Intel dataset.
-  // The solvers are configured to run for a constant number of steps.
-  // The nonlinear solver is 5 iterations of Gauss-Newton.
-  // The linear solver is 100 iterations of CGLS.
+  // Uses `NewFactorGraph` on the GTSAM pose3example dataset.
+  // The solvers are configured to run for a constant number of *LM steps*, except when the LM solver is
+  // unable to progress even with maximum lambda.
+  // The linear solver is 200 iterations of CGLS.
   suite.benchmark(
     "NewFactorGraph, sphere2500, 30 LM steps, 200 CGLS steps",
     settings: Iterations(1)
diff --git a/Sources/SwiftFusion/Datasets/G2OReader.swift b/Sources/SwiftFusion/Datasets/G2OReader.swift
index 628d5355..e221b972 100644
--- a/Sources/SwiftFusion/Datasets/G2OReader.swift
+++ b/Sources/SwiftFusion/Datasets/G2OReader.swift
@@ -51,7 +51,7 @@ public enum G2OReader {
     /// The initial guess.
     public var initialGuess = VariableAssignments()
 
-    public var initialGuessId = Array<TypedID<Pose, Int>>()
+    public var variableId = Array<TypedID<Pose, Int>>()
     
     /// The factor graph representing the measurements.
     public var graph = NewFactorGraph()
@@ -63,21 +63,21 @@ public enum G2OReader {
         case .initialGuess(index: let id, pose: let guess):
           let typedID = initialGuess.store(guess)
           assert(typedID.perTypeID == id)
-          initialGuessId.append(typedID)
+          variableId.append(typedID)
         case .measurement(frameIndex: let id1, measuredIndex: let id2, pose: let difference):
           graph.store(NewBetweenFactor2(TypedID(id1), TypedID(id2), difference))
         }
       }
     }
     
-    /// Creates a problem from the given 2D file.
+    /// Creates a problem from the given 3D file.
     public init(g2oFile3D: URL) throws where Pose == Pose3 {
       try G2OReader.read3D(file: g2oFile3D) { entry in
         switch entry {
         case .initialGuess(index: let id, pose: let guess):
           let typedID = initialGuess.store(guess)
           assert(typedID.perTypeID == id)
-          initialGuessId.append(typedID)
+          variableId.append(typedID)
         case .measurement(frameIndex: let id1, measuredIndex: let id2, pose: let difference):
           graph.store(NewBetweenFactor3(TypedID(id1), TypedID(id2), difference))
         }
diff --git a/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift b/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift
index 1581e0ab..ccedb30e 100644
--- a/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift
+++ b/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift
@@ -58,37 +58,38 @@ class NewFactorGraphTests: XCTestCase {
   /// |    |
   /// z-->xZ--> Y  (z pointing towards viewer, Z pointing away from viewer)
   /// Vehicle at p0 is looking towards y axis (X-axis points towards world y)
-  func circlePose3(numPoses: Int = 8, radius: Double = 1.0) -> Values {
-    var values = Values()
+  func circlePose3(numPoses: Int = 8, radius: Double = 1.0) -> (Array<TypedID<Pose3, Int>>, VariableAssignments) {
+    var ids: Array<TypedID<Pose3, Int>> = []
+    var values = VariableAssignments()
     var theta = 0.0
     let dtheta = 2.0 * .pi / Double(numPoses)
     let gRo = Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1)
-    for i in 0..<numPoses {
-      let key = 0 + i
+    for _ in 0..<numPoses {
       let gti = Vector3(radius * cos(theta), radius * sin(theta), 0)
       let oRi = Rot3.fromTangent(Vector3(0, 0, -theta))  // negative yaw goes counterclockwise, with Z down !
       let gTi = Pose3(gRo * oRi, gti)
-      values.insert(key, gTi)
+      let id = values.store(gTi)
+      ids.append(id)
       theta = theta + dtheta
     }
-    return values
+    return (ids, values)
   }
   
   func testGtsamPose3SLAMExample() {
     // Create a hexagon of poses
-    let hexagon = circlePose3(numPoses: 6, radius: 1.0)
-    let p0 = hexagon[0, as: Pose3.self]
-    let p1 = hexagon[1, as: Pose3.self]
+    let (hexagonId, hexagon) = circlePose3(numPoses: 6, radius: 1.0)
+    let p0 = hexagon[hexagonId[0]]
+    let p1 = hexagon[hexagonId[1]]
     
     var x = VariableAssignments()
     
     let s = 0.10
     let id0 = x.store(p0)
-    let id1 = x.store(hexagon[1, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
-    let id2 = x.store(hexagon[2, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
-    let id3 = x.store(hexagon[3, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
-    let id4 = x.store(hexagon[4, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
-    let id5 = x.store(hexagon[5, as: Pose3.self].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id1 = x.store(hexagon[hexagonId[1]].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id2 = x.store(hexagon[hexagonId[2]].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id3 = x.store(hexagon[hexagonId[3]].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id4 = x.store(hexagon[hexagonId[4]].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
+    let id5 = x.store(hexagon[hexagonId[5]].retract(Vector6(s * Tensor<Double>(randomNormal: [6]))))
     
     var fg = NewFactorGraph()
     fg.store(NewPriorFactor3(id0, p0))

From dd3236d171748123aee60ab963d2f3fbd0660573 Mon Sep 17 00:00:00 2001
From: Fan Jiang <i@fanjiang.me>
Date: Tue, 16 Jun 2020 10:43:48 -0400
Subject: [PATCH 9/9] Fix tests

---
 Examples/Pose3SLAMG2O/main.swift              |  2 +-
 Sources/SwiftFusion/Optimizers/LM.swift       | 15 ++++++++-
 .../SwiftFusionTests/Optimizers/LMTests.swift | 32 +++++++++++++++++++
 3 files changed, 47 insertions(+), 2 deletions(-)
 create mode 100644 Tests/SwiftFusionTests/Optimizers/LMTests.swift

diff --git a/Examples/Pose3SLAMG2O/main.swift b/Examples/Pose3SLAMG2O/main.swift
index 25aa7aa3..f12f4739 100644
--- a/Examples/Pose3SLAMG2O/main.swift
+++ b/Examples/Pose3SLAMG2O/main.swift
@@ -84,7 +84,7 @@ func main() {
   let fileHandle = try! FileHandle(forUpdating: URL(fileURLWithPath: filePath.path))
   var output = FileHandlerOutputStream(fileHandle)
   
-  for i in problem.initialGuessId {
+  for i in problem.variableId {
     let t = val[i].t
     output.write("\(t.x), \(t.y), \(t.z)\n")
   }
diff --git a/Sources/SwiftFusion/Optimizers/LM.swift b/Sources/SwiftFusion/Optimizers/LM.swift
index 33af72c1..f73f84c3 100644
--- a/Sources/SwiftFusion/Optimizers/LM.swift
+++ b/Sources/SwiftFusion/Optimizers/LM.swift
@@ -64,6 +64,7 @@ public struct LM {
     var lambda = 1e-6
     var inner_iter_step = 0
     var inner_success = false
+    var all_done = false
     
     for _ in 0..<max_iteration { // outer loop
       
@@ -122,8 +123,16 @@ public struct LM {
           inner_success = true
         } else {
           if verbosity >= .TRYLAMBDA {
-            print("[LM INNER] fail, trying to increase lambda")
+            print("[LM INNER] fail, trying to increase lambda or give up")
           }
+          
+          if model_fidelity > 0.1 && delta_error < precision {
+            print("[LM INNER] reached the target precision, exiting")
+            inner_success = true
+            all_done = true
+            break
+          }
+          
           // increase lambda and retry
           val = oldval
           if lambda > 1e20 {
@@ -141,6 +150,10 @@ public struct LM {
         }
       }
       
+      if all_done {
+        break
+      }
+      
       step += 1
     }
     
diff --git a/Tests/SwiftFusionTests/Optimizers/LMTests.swift b/Tests/SwiftFusionTests/Optimizers/LMTests.swift
new file mode 100644
index 00000000..a9cf869b
--- /dev/null
+++ b/Tests/SwiftFusionTests/Optimizers/LMTests.swift
@@ -0,0 +1,32 @@
+// This file tests for the CGLS optimizer
+
+import SwiftFusion
+import TensorFlow
+import XCTest
+
+final class LMTests: XCTestCase {
+  /// test convergence for a simple gaussian factor graph
+  func testBasicLMConvergence() {
+    var x = VariableAssignments()
+    let pose1ID = x.store(Pose2(Rot2(0.2), Vector2(0.5, 0.0)))
+    let pose2ID = x.store(Pose2(Rot2(-0.2), Vector2(2.3, 0.1)))
+    let pose3ID = x.store(Pose2(Rot2(.pi / 2), Vector2(4.1, 0.1)))
+    let pose4ID = x.store(Pose2(Rot2(.pi), Vector2(4.0, 2.0)))
+    let pose5ID = x.store(Pose2(Rot2(-.pi / 2), Vector2(2.1, 2.1)))
+
+    var graph = NewFactorGraph()
+    graph.store(NewBetweenFactor2(pose2ID, pose1ID, Pose2(2.0, 0.0, .pi / 2)))
+    graph.store(NewBetweenFactor2(pose3ID, pose2ID, Pose2(2.0, 0.0, .pi / 2)))
+    graph.store(NewBetweenFactor2(pose4ID, pose3ID, Pose2(2.0, 0.0, .pi / 2)))
+    graph.store(NewBetweenFactor2(pose5ID, pose4ID, Pose2(2.0, 0.0, .pi / 2)))
+    graph.store(NewPriorFactor2(pose1ID, Pose2(0, 0, 0)))
+
+    var optimizer = LM(precision: 1e-3, max_iteration: 10)
+    optimizer.verbosity = .TRYLAMBDA
+    
+    try? optimizer.optimize(graph: graph, initial: &x)
+
+    // Test condition: pose 5 should be identical to pose 1 (close loop).
+    XCTAssertEqual(between(x[pose1ID], x[pose5ID]).t.norm, 0.0, accuracy: 1e-2)
+  }
+}