[WIP] Add LM Pose3Example and G2O 3D Reader

Examples/Pose3SLAMG2O/main.swift

@@ -0,0 +1,93 @@
+// 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()
+  optimizer.verbosity = .TRYLAMBDA
+
+  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()

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"]),

Sources/Benchmarks/Pose3SLAM.swift

@@ -18,11 +18,10 @@ import Benchmark
 import SwiftFusion
 
 let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
-
-  var gridDataset =
+  
+  var gridDataset_old =
     try! G2OReader.G2ONonlinearFactorGraph(g2oFile3D: try! cachedDataset("pose3example.txt"))
-//  check(gridDataset.graph.error(gridDataset.initialGuess), near: 12.99, accuracy: 1e-2)
-
+  // 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.
@@ -31,11 +30,11 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
     "NonlinearFactorGraph, Pose3Example, 50 Gauss-Newton steps, 200 CGLS steps",
     settings: Iterations(1)
   ) {
-    var val = gridDataset.initialGuess
-    gridDataset.graph += PriorFactor(0, Pose3())
+    var val = gridDataset_old.initialGuess
+    gridDataset_old.graph += PriorFactor(0, Pose3())
     for _ in 0..<40 {
-      print("error = \(gridDataset.graph.error(val))")
-      let gfg = gridDataset.graph.linearize(val)
+      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 {
@@ -49,4 +48,33 @@ let pose3SLAM = BenchmarkSuite(name: "Pose3SLAM") { suite in
       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.
+  // 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(
+    "NewFactorGraph, sphere2500, 30 LM steps, 200 CGLS steps",
+    settings: Iterations(1)
+  ) {
+    var val = gridDataset.initialGuess
+    var graph = gridDataset.graph
+
+    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)
+    } catch let error {
+      print("The solver gave up, message: \(error.localizedDescription)")
+    }
+  }
 }

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.

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.

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

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))
     })

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 {

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)