diff --git a/.gitignore b/.gitignore
index 86d5333c..cc25848d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,7 +3,6 @@
 /Packages
 /*.xcodeproj
 xcuserdata/
-/Package.resolved
 /.swiftpm
 /.vscode
 /.idea
\ No newline at end of file
diff --git a/Package.resolved b/Package.resolved
new file mode 100644
index 00000000..60b40867
--- /dev/null
+++ b/Package.resolved
@@ -0,0 +1,25 @@
+{
+  "object": {
+    "pins": [
+      {
+        "package": "swift-argument-parser",
+        "repositoryURL": "https://github.com/apple/swift-argument-parser",
+        "state": {
+          "branch": null,
+          "revision": "223d62adc52d51669ae2ee19bdb8b7d9fd6fcd9c",
+          "version": "0.0.6"
+        }
+      },
+      {
+        "package": "Benchmark",
+        "repositoryURL": "https://github.com/google/swift-benchmark.git",
+        "state": {
+          "branch": "master",
+          "revision": "a952f1d7deed2368805ac29f09da6a676dde8015",
+          "version": null
+        }
+      }
+    ]
+  },
+  "version": 1
+}
diff --git a/Package.swift b/Package.swift
index ff7f0938..c27016fe 100644
--- a/Package.swift
+++ b/Package.swift
@@ -18,6 +18,7 @@ let package = Package(
   dependencies: [
     // Dependencies declare other packages that this package depends on.
     // .package(url: /* package url */, from: "1.0.0"),
+    .package(url: "https://github.com/google/swift-benchmark.git", .branch("master")),
   ],
   targets: [
     // Targets are the basic building blocks of a package. A target can define a module or a test suite.
@@ -25,6 +26,12 @@ let package = Package(
     .target(
       name: "SwiftFusion",
       dependencies: []),
+    .target(
+      name: "Benchmarks",
+      dependencies: [
+        "Benchmark",
+        "SwiftFusion",
+      ]),
     .target(
       name: "Pose2SLAMG2O",
       dependencies: ["SwiftFusion"],
diff --git a/README.md b/README.md
index 2c33d09d..8756aa72 100644
--- a/README.md
+++ b/README.md
@@ -2,6 +2,24 @@
 
 Differentiable Swift based sensor fusion library.
 
+## Run tests
+
+```
+swift test
+```
+
+## Run benchmarks
+
+```
+swift run -c release -Xswiftc -cross-module-optimization Benchmarks
+```
+
+## Update dependency versions
+
+```
+swift package update
+```
+
 # LICENSE
 
 
diff --git a/Sources/Benchmarks/Pose2SLAM.swift b/Sources/Benchmarks/Pose2SLAM.swift
new file mode 100644
index 00000000..7ec9be76
--- /dev/null
+++ b/Sources/Benchmarks/Pose2SLAM.swift
@@ -0,0 +1,70 @@
+// 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.
+
+/// Benchmarks Pose2SLAM solutions.
+
+import Benchmark
+import SwiftFusion
+
+let pose2SLAM = BenchmarkSuite(name: "Pose2SLAM") { suite in
+
+  let intelDataset = try! G2OFactorGraph(fromG2O: try! cachedDataset("input_INTEL_g2o.txt"))
+  check(intelDataset.graph.error(intelDataset.initialGuess), near: 73565.64, 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, Intel, 5 Gauss-Newton steps, 100 CGLS steps",
+    settings: .iterations(1)
+  ) {
+    var val = intelDataset.initialGuess
+    for _ in 0..<5 {
+      let gfg = intelDataset.graph.linearize(val)
+      let optimizer = CGLS(precision: 0, max_iteration: 100)
+      var dx = VectorValues()
+      for i in 0..<val.count {
+        dx.insert(i, Vector(zeros: 3))
+      }
+      optimizer.optimize(gfg: gfg, initial: &dx)
+      val.move(along: dx)
+    }
+    check(intelDataset.graph.error(val), near: 35.59, accuracy: 1e-2)
+  }
+}
+
+/// Builds an initial guess and a factor graph from a g2o file.
+struct G2OFactorGraph: G2OReader {
+  /// The initial guess.
+  var initialGuess: Values = Values()
+
+  /// The factor graph representing the measurements.
+  var graph: NonlinearFactorGraph = NonlinearFactorGraph()
+
+  public mutating func addInitialGuess(index: Int, pose: Pose2) {
+    initialGuess.insert(index, pose)
+  }
+
+  public mutating func addMeasurement(frameIndex: Int, measuredIndex: Int, pose: Pose2) {
+    graph += BetweenFactor(frameIndex, measuredIndex, pose)
+  }
+}
+
+func check(_ actual: Double, near expected: Double, accuracy: Double) {
+  if abs(actual - expected) > accuracy {
+    print("ERROR: \(actual) != \(expected) (accuracy \(accuracy))")
+    fatalError()
+  }
+}
diff --git a/Sources/Benchmarks/main.swift b/Sources/Benchmarks/main.swift
new file mode 100644
index 00000000..e256f0c1
--- /dev/null
+++ b/Sources/Benchmarks/main.swift
@@ -0,0 +1,19 @@
+// 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.
+
+import Benchmark
+
+Benchmark.main([
+  pose2SLAM
+])
diff --git a/Sources/SwiftFusion/Datasets/DatasetCache.swift b/Sources/SwiftFusion/Datasets/DatasetCache.swift
new file mode 100644
index 00000000..78be863b
--- /dev/null
+++ b/Sources/SwiftFusion/Datasets/DatasetCache.swift
@@ -0,0 +1,70 @@
+// 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.
+
+import Foundation
+#if !os(macOS)
+import FoundationNetworking
+#endif
+
+/// Dictionary from dataset id to dataset url.
+fileprivate let datasets = [
+  "input_INTEL_g2o.txt":
+    "https://github.com/pkchen1129/Mobile-Robotics/raw/8551df10ba8af36801403daeba710c1f9c9e54cd/ps4/code/dataset/input_INTEL_g2o.txt"
+]
+
+/// Returns a dataset cached on the local system.
+///
+/// If the dataset with `id` is available on the local system, returns it immediately.
+/// Otherwise, downloads the dataset to the cache and then returns it.
+///
+/// - Parameter `cacheDirectory`: The directory where the cached datasets are stored.
+public func cachedDataset(
+  _ id: String,
+  cacheDirectory: URL = URL(fileURLWithPath: NSHomeDirectory())
+    .appendingPathComponent(".SwiftFusionDatasetCache", isDirectory: true)
+) throws -> URL {
+  try createDirectoryIfMissing(at: cacheDirectory.path)
+  let cacheEntry = cacheDirectory.appendingPathComponent(id)
+  if FileManager.default.fileExists(atPath: cacheEntry.path) {
+    return cacheEntry
+  }
+  guard let url = datasets[id] else {
+    throw DatasetCacheError(message: "No such dataset: \(id)")
+  }
+  print("Downloading \(url) to \(cacheEntry)")
+  guard let source = URL(string: url) else {
+    throw DatasetCacheError(message: "Could not parse URL: \(url)")
+  }
+  let data = try Data.init(contentsOf: source)
+  try data.write(to: cacheEntry)
+  print("Downloaded \(cacheEntry)!")
+  return cacheEntry
+}
+
+/// An error from getting a cached dataset.
+public struct DatasetCacheError: Swift.Error {
+  public let message: String
+}
+
+/// Creates a directory at a path, if missing. If the directory exists, this does nothing.
+///
+/// - Parameters:
+///   - path: The path of the desired directory.
+fileprivate func createDirectoryIfMissing(at path: String) throws {
+    guard !FileManager.default.fileExists(atPath: path) else { return }
+    try FileManager.default.createDirectory(
+        atPath: path,
+        withIntermediateDirectories: true,
+        attributes: nil)
+}
diff --git a/Sources/SwiftFusion/Datasets/G2OReader.swift b/Sources/SwiftFusion/Datasets/G2OReader.swift
index af09c7cd..a08122e0 100644
--- a/Sources/SwiftFusion/Datasets/G2OReader.swift
+++ b/Sources/SwiftFusion/Datasets/G2OReader.swift
@@ -18,6 +18,9 @@ import Foundation
 ///
 /// See https://lucacarlone.mit.edu/datasets/ for g2o specification and example datasets.
 public protocol G2OReader {
+  /// Creates an empty reader.
+  init()
+
   /// Adds an initial guess that vertex `index` has pose `pose`.
   mutating func addInitialGuess(index: Int, pose: Pose2)
 
@@ -29,7 +32,13 @@ public protocol G2OReader {
 }
 
 extension G2OReader {
-  /// Reads a g2o dataset from `url`.
+  /// Creates a g2o dataset from `url`.
+  public init(fromG2O url: URL) throws {
+    self.init()
+    try self.read(fromG2O: url)
+  }
+
+  /// Adds the g2o dataset at `url` into `self`.
   public mutating func read(fromG2O url: URL) throws {
     let lines = try String(contentsOf: url).split(separator: "\n")
     for (lineIndex, line) in lines.enumerated() {