diff --git a/Examples/Pose3SLAMG2O/main.swift b/Examples/Pose3SLAMG2O/main.swift new file mode 100644 index 00000000..f12f4739 --- /dev/null +++ b/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.variableId { + 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 cc021137..509b40fc 100644 --- a/Sources/Benchmarks/Pose3SLAM.swift +++ b/Sources/Benchmarks/Pose3SLAM.swift @@ -18,24 +18,22 @@ 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) - - // 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.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.. { /// The initial guess. public var initialGuess = VariableAssignments() + public var variableId = Array>() + /// 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) + 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 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) + variableId.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)) }) 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..f2820ec9 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/Sources/SwiftFusion/Optimizers/LM.swift b/Sources/SwiftFusion/Optimizers/LM.swift new file mode 100644 index 00000000..f73f84c3 --- /dev/null +++ b/Sources/SwiftFusion/Optimizers/LM.swift @@ -0,0 +1,164 @@ +// 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 + var all_done = false + + for _ in 0..= .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: 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)") + } + 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 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 { + 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_success { + break + } + } + + if all_done { + break + } + + step += 1 + } + + if verbosity >= .SUMMARY { + print("[FINAL ] final error = \(graph.error(at: val))") + } + } +} diff --git a/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift b/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift index 91bf1462..ccedb30e 100644 --- a/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift +++ b/Tests/SwiftFusionTests/Inference/NewFactorGraphTests.swift @@ -46,4 +46,72 @@ 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) -> (Array>, VariableAssignments) { + var ids: Array> = [] + 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 _ in 0..(randomNormal: [6])))) + let id2 = x.store(hexagon[hexagonId[2]].retract(Vector6(s * Tensor(randomNormal: [6])))) + let id3 = x.store(hexagon[hexagonId[3]].retract(Vector6(s * Tensor(randomNormal: [6])))) + let id4 = x.store(hexagon[hexagonId[4]].retract(Vector6(s * Tensor(randomNormal: [6])))) + let id5 = x.store(hexagon[hexagonId[5]].retract(Vector6(s * Tensor(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] 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) + } +}