diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index b2b815a..7872926 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -89,6 +89,37 @@
 )
 
 cc_library(
+    name = "target_mapper",
+    srcs = ["target_mapper.cc"],
+    hdrs = ["target_mapper.h"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":geometry_lib",
+        "//aos/events:simulated_event_loop",
+        "//frc971/control_loops:control_loop",
+        "//frc971/vision/ceres:pose_graph_2d_lib",
+        "//third_party:opencv",
+        "@com_google_ceres_solver//:ceres",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
+
+cc_test(
+    name = "target_mapper_test",
+    srcs = [
+        "target_mapper_test.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":target_mapper",
+        "//aos/events:simulated_event_loop",
+        "//aos/testing:googletest",
+        "//aos/testing:random_seed",
+    ],
+)
+
+cc_library(
     name = "geometry_lib",
     hdrs = [
         "geometry.h",
diff --git a/frc971/vision/ceres/BUILD b/frc971/vision/ceres/BUILD
new file mode 100644
index 0000000..5622b55
--- /dev/null
+++ b/frc971/vision/ceres/BUILD
@@ -0,0 +1,24 @@
+# Copy of ceres example code from their repo
+cc_library(
+    name = "pose_graph_2d_lib",
+    hdrs = [
+        "angle_local_parameterization.h",
+        "normalize_angle.h",
+        "pose_graph_2d_error_term.h",
+        "read_g2o.h",
+        "types.h",
+    ],
+    copts = [
+        # Needed to silence GFlags complaints.
+        "-Wno-sign-compare",
+        "-Wno-unused-parameter",
+        "-Wno-format-nonliteral",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "@com_github_gflags_gflags//:gflags",
+        "@com_google_ceres_solver//:ceres",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
diff --git a/frc971/vision/ceres/angle_local_parameterization.h b/frc971/vision/ceres/angle_local_parameterization.h
new file mode 100644
index 0000000..a81637c
--- /dev/null
+++ b/frc971/vision/ceres/angle_local_parameterization.h
@@ -0,0 +1,64 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+
+#include "ceres/local_parameterization.h"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// Defines a local parameterization for updating the angle to be constrained in
+// [-pi to pi).
+class AngleLocalParameterization {
+ public:
+  template <typename T>
+  bool operator()(const T* theta_radians,
+                  const T* delta_theta_radians,
+                  T* theta_radians_plus_delta) const {
+    *theta_radians_plus_delta =
+        NormalizeAngle(*theta_radians + *delta_theta_radians);
+
+    return true;
+  }
+
+  static ceres::LocalParameterization* Create() {
+    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+                                                     1,
+                                                     1>);
+  }
+};
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
diff --git a/frc971/vision/ceres/normalize_angle.h b/frc971/vision/ceres/normalize_angle.h
new file mode 100644
index 0000000..c215671
--- /dev/null
+++ b/frc971/vision/ceres/normalize_angle.h
@@ -0,0 +1,53 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+
+#include <cmath>
+
+#include "ceres/ceres.h"
+
+namespace ceres {
+namespace examples {
+
+// Normalizes the angle in radians between [-pi and pi).
+template <typename T>
+inline T NormalizeAngle(const T& angle_radians) {
+  // Use ceres::floor because it is specialized for double and Jet types.
+  T two_pi(2.0 * M_PI);
+  return angle_radians -
+         two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
+}
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
diff --git a/frc971/vision/ceres/pose_graph_2d_error_term.h b/frc971/vision/ceres/pose_graph_2d_error_term.h
new file mode 100644
index 0000000..2df31f6
--- /dev/null
+++ b/frc971/vision/ceres/pose_graph_2d_error_term.h
@@ -0,0 +1,119 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Cost function for a 2D pose graph formulation.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+
+namespace ceres {
+namespace examples {
+
+template <typename T>
+Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
+  const T cos_yaw = ceres::cos(yaw_radians);
+  const T sin_yaw = ceres::sin(yaw_radians);
+
+  Eigen::Matrix<T, 2, 2> rotation;
+  rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
+  return rotation;
+}
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement.
+//
+// residual =  information^{1/2} * [  r_a^T * (p_b - p_a) - \hat{p_ab}   ]
+//                                 [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
+//
+// where r_a is the rotation matrix that rotates a vector represented in frame A
+// into the global frame, and Normalize(*) ensures the angles are in the range
+// [-pi, pi).
+class PoseGraph2dErrorTerm {
+ public:
+  PoseGraph2dErrorTerm(double x_ab,
+                       double y_ab,
+                       double yaw_ab_radians,
+                       const Eigen::Matrix3d& sqrt_information)
+      : p_ab_(x_ab, y_ab),
+        yaw_ab_radians_(yaw_ab_radians),
+        sqrt_information_(sqrt_information) {}
+
+  template <typename T>
+  bool operator()(const T* const x_a,
+                  const T* const y_a,
+                  const T* const yaw_a,
+                  const T* const x_b,
+                  const T* const y_b,
+                  const T* const yaw_b,
+                  T* residuals_ptr) const {
+    const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
+    const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
+
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
+
+    residuals_map.template head<2>() =
+        RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
+    residuals_map(2) = ceres::examples::NormalizeAngle(
+        (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
+
+    // Scale the residuals by the square root information matrix to account for
+    // the measurement uncertainty.
+    residuals_map = sqrt_information_.template cast<T>() * residuals_map;
+
+    return true;
+  }
+
+  static ceres::CostFunction* Create(double x_ab,
+                                     double y_ab,
+                                     double yaw_ab_radians,
+                                     const Eigen::Matrix3d& sqrt_information) {
+    return (new ceres::
+                AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
+                    new PoseGraph2dErrorTerm(
+                        x_ab, y_ab, yaw_ab_radians, sqrt_information)));
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  // The position of B relative to A in the A frame.
+  const Eigen::Vector2d p_ab_;
+  // The orientation of frame B relative to frame A.
+  const double yaw_ab_radians_;
+  // The inverse square root of the measurement covariance matrix.
+  const Eigen::Matrix3d sqrt_information_;
+};
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
diff --git a/frc971/vision/ceres/read_g2o.h b/frc971/vision/ceres/read_g2o.h
new file mode 100644
index 0000000..fea32e9
--- /dev/null
+++ b/frc971/vision/ceres/read_g2o.h
@@ -0,0 +1,143 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Reads a file in the g2o filename format that describes a pose graph problem.
+
+#ifndef EXAMPLES_CERES_READ_G2O_H_
+#define EXAMPLES_CERES_READ_G2O_H_
+
+#include <fstream>
+#include <string>
+
+#include "glog/logging.h"
+
+namespace ceres {
+namespace examples {
+
+// Reads a single pose from the input and inserts it into the map. Returns false
+// if there is a duplicate entry.
+template <typename Pose, typename Allocator>
+bool ReadVertex(std::ifstream* infile,
+                std::map<int, Pose, std::less<int>, Allocator>* poses) {
+  int id;
+  Pose pose;
+  *infile >> id >> pose;
+
+  // Ensure we don't have duplicate poses.
+  if (poses->find(id) != poses->end()) {
+    LOG(ERROR) << "Duplicate vertex with ID: " << id;
+    return false;
+  }
+  (*poses)[id] = pose;
+
+  return true;
+}
+
+// Reads the contraints between two vertices in the pose graph
+template <typename Constraint, typename Allocator>
+void ReadConstraint(std::ifstream* infile,
+                    std::vector<Constraint, Allocator>* constraints) {
+  Constraint constraint;
+  *infile >> constraint;
+
+  constraints->push_back(constraint);
+}
+
+// Reads a file in the g2o filename format that describes a pose graph
+// problem. The g2o format consists of two entries, vertices and constraints.
+//
+// In 2D, a vertex is defined as follows:
+//
+// VERTEX_SE2 ID x_meters y_meters yaw_radians
+//
+// A constraint is defined as follows:
+//
+// EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement.
+//
+//
+// In 3D, a vertex is defined as follows:
+//
+// VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
+//
+// where the quaternion is in Hamilton form.
+// A constraint is defined as follows:
+//
+// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement. Only the upper-triangular part is stored. The measurement order
+// is the delta position followed by the delta orientation.
+template <typename Pose,
+          typename Constraint,
+          typename MapAllocator,
+          typename VectorAllocator>
+bool ReadG2oFile(const std::string& filename,
+                 std::map<int, Pose, std::less<int>, MapAllocator>* poses,
+                 std::vector<Constraint, VectorAllocator>* constraints) {
+  CHECK(poses != NULL);
+  CHECK(constraints != NULL);
+
+  poses->clear();
+  constraints->clear();
+
+  std::ifstream infile(filename.c_str());
+  if (!infile) {
+    return false;
+  }
+
+  std::string data_type;
+  while (infile.good()) {
+    // Read whether the type is a node or a constraint.
+    infile >> data_type;
+    if (data_type == Pose::name()) {
+      if (!ReadVertex(&infile, poses)) {
+        return false;
+      }
+    } else if (data_type == Constraint::name()) {
+      ReadConstraint(&infile, constraints);
+    } else {
+      LOG(ERROR) << "Unknown data type: " << data_type;
+      return false;
+    }
+
+    // Clear any trailing whitespace from the line.
+    infile >> std::ws;
+  }
+
+  return true;
+}
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // EXAMPLES_CERES_READ_G2O_H_
diff --git a/frc971/vision/ceres/types.h b/frc971/vision/ceres/types.h
new file mode 100644
index 0000000..3c13824
--- /dev/null
+++ b/frc971/vision/ceres/types.h
@@ -0,0 +1,101 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
+// the graph has a unique integer ID with a position and orientation. There are
+// delta transformation constraints between two vertices.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+
+#include <fstream>
+
+#include "Eigen/Core"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// The state for each vertex in the pose graph.
+struct Pose2d {
+  double x;
+  double y;
+  double yaw_radians;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() { return "VERTEX_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
+  input >> pose.x >> pose.y >> pose.yaw_radians;
+  // Normalize the angle between -pi to pi.
+  pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+  return input;
+}
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint2d {
+  int id_begin;
+  int id_end;
+
+  double x;
+  double y;
+  double yaw_radians;
+
+  // The inverse of the covariance matrix for the measurement. The order of the
+  // entries are x, y, and yaw.
+  Eigen::Matrix3d information;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() { return "EDGE_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
+  input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
+      constraint.y >> constraint.yaw_radians >> constraint.information(0, 0) >>
+      constraint.information(0, 1) >> constraint.information(0, 2) >>
+      constraint.information(1, 1) >> constraint.information(1, 2) >>
+      constraint.information(2, 2);
+
+  // Set the lower triangular part of the information matrix.
+  constraint.information(1, 0) = constraint.information(0, 1);
+  constraint.information(2, 0) = constraint.information(0, 2);
+  constraint.information(2, 1) = constraint.information(1, 2);
+
+  // Normalize the angle between -pi to pi.
+  constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
+  return input;
+}
+
+}  // namespace examples
+}  // namespace ceres
+
+#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
new file mode 100644
index 0000000..b806c03
--- /dev/null
+++ b/frc971/vision/target_mapper.cc
@@ -0,0 +1,324 @@
+#include "frc971/vision/target_mapper.h"
+
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/vision/ceres/angle_local_parameterization.h"
+#include "frc971/vision/ceres/normalize_angle.h"
+#include "frc971/vision/ceres/pose_graph_2d_error_term.h"
+#include "frc971/vision/geometry.h"
+
+DEFINE_uint64(max_num_iterations, 100,
+              "Maximum number of iterations for the ceres solver");
+
+namespace frc971::vision {
+
+Eigen::Affine3d PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d pose2d) {
+  Eigen::Affine3d H_world_pose =
+      Eigen::Translation3d(pose2d.x, pose2d.y, 0.0) *
+      Eigen::AngleAxisd(pose2d.yaw_radians, Eigen::Vector3d::UnitZ());
+  return H_world_pose;
+}
+
+ceres::examples::Pose2d PoseUtils::Affine3dToPose2d(Eigen::Affine3d H) {
+  Eigen::Vector3d T = H.translation();
+  double heading = std::atan2(H.rotation()(1, 0), H.rotation()(0, 0));
+  return ceres::examples::Pose2d{T[0], T[1],
+                                 ceres::examples::NormalizeAngle(heading)};
+}
+
+ceres::examples::Pose2d PoseUtils::ComputeRelativePose(
+    ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2) {
+  Eigen::Affine3d H_world_1 = Pose2dToAffine3d(pose_1);
+  Eigen::Affine3d H_world_2 = Pose2dToAffine3d(pose_2);
+
+  // Get the location of 2 in the 1 frame
+  Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2;
+  return Affine3dToPose2d(H_1_2);
+}
+
+ceres::examples::Pose2d PoseUtils::ComputeOffsetPose(
+    ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2_relative) {
+  auto H_world_1 = Pose2dToAffine3d(pose_1);
+  auto H_1_2 = Pose2dToAffine3d(pose_2_relative);
+  auto H_world_2 = H_world_1 * H_1_2;
+
+  return Affine3dToPose2d(H_world_2);
+}
+
+namespace {
+double ExponentiatedSinTerm(double theta) {
+  return (theta == 0.0 ? 1.0 : std::sin(theta) / theta);
+}
+
+double ExponentiatedCosTerm(double theta) {
+  return (theta == 0.0 ? 0.0 : (1 - std::cos(theta)) / theta);
+}
+}  // namespace
+
+ceres::examples::Pose2d DataAdapter::InterpolatePose(
+    const TimestampedPose &pose_start, const TimestampedPose &pose_end,
+    aos::distributed_clock::time_point time) {
+  auto delta_pose =
+      PoseUtils::ComputeRelativePose(pose_start.pose, pose_end.pose);
+  // Time from start of period, on the scale 0-1 where 1 is the end.
+  double interpolation_scalar =
+      static_cast<double>((time - pose_start.time).count()) /
+      static_cast<double>((pose_end.time - pose_start.time).count());
+
+  double theta = delta_pose.yaw_radians;
+  // Take the log of the transformation matrix:
+  // https://mathoverflow.net/questions/118533/how-to-compute-se2-group-exponential-and-logarithm
+  StdFormLine dx_line = {.a = ExponentiatedSinTerm(theta),
+                         .b = -ExponentiatedCosTerm(theta),
+                         .c = delta_pose.x};
+  StdFormLine dy_line = {.a = ExponentiatedCosTerm(theta),
+                         .b = ExponentiatedSinTerm(theta),
+                         .c = delta_pose.y};
+
+  std::optional<cv::Point2d> solution = dx_line.Intersection(dy_line);
+  CHECK(solution.has_value());
+
+  // Re-exponentiate with the new values scaled by the interpolation scalar to
+  // get an interpolated tranformation matrix
+  double a = solution->x * interpolation_scalar;
+  double b = solution->y * interpolation_scalar;
+  double alpha = theta * interpolation_scalar;
+
+  ceres::examples::Pose2d interpolated_pose = {
+      .x = a * ExponentiatedSinTerm(theta) - b * ExponentiatedCosTerm(theta),
+      .y = a * ExponentiatedCosTerm(theta) + b * ExponentiatedSinTerm(theta),
+      .yaw_radians = alpha};
+
+  return PoseUtils::ComputeOffsetPose(pose_start.pose, interpolated_pose);
+}  // namespace frc971::vision
+
+std::pair<std::vector<ceres::examples::Constraint2d>,
+          std::vector<ceres::examples::Pose2d>>
+DataAdapter::MatchTargetDetections(
+    const std::vector<TimestampedPose> &timestamped_robot_poses,
+    const std::vector<TimestampedDetection> &timestamped_target_detections) {
+  // Interpolate robot poses
+  std::map<aos::distributed_clock::time_point, ceres::examples::Pose2d>
+      interpolated_poses;
+
+  CHECK_GT(timestamped_robot_poses.size(), 1ul)
+      << "Need more than 1 robot pose";
+  auto robot_pose_it = timestamped_robot_poses.begin();
+  for (const auto &timestamped_detection : timestamped_target_detections) {
+    aos::distributed_clock::time_point target_time = timestamped_detection.time;
+    // Find the robot point right before this localization
+    while (robot_pose_it->time > target_time ||
+           (robot_pose_it + 1)->time <= target_time) {
+      robot_pose_it++;
+      CHECK(robot_pose_it < timestamped_robot_poses.end() - 1)
+          << "Need a robot pose before and after every target detection";
+    }
+
+    auto start = robot_pose_it;
+    auto end = robot_pose_it + 1;
+    interpolated_poses.emplace(target_time,
+                               InterpolatePose(*start, *end, target_time));
+  }
+
+  // Match consecutive detections
+  std::vector<ceres::examples::Constraint2d> target_constraints;
+  std::vector<ceres::examples::Pose2d> robot_delta_poses;
+
+  auto last_detection = timestamped_target_detections[0];
+  auto last_robot_pose =
+      interpolated_poses[timestamped_target_detections[0].time];
+
+  for (auto it = timestamped_target_detections.begin() + 1;
+       it < timestamped_target_detections.end(); it++) {
+    // Skip two consecutive detections of the same target, because the solver
+    // doesn't allow this
+    if (it->id == last_detection.id) {
+      continue;
+    }
+
+    auto robot_pose = interpolated_poses[it->time];
+    auto robot_delta_pose =
+        PoseUtils::ComputeRelativePose(last_robot_pose, robot_pose);
+    auto confidence = ComputeConfidence(last_detection.time, it->time);
+
+    target_constraints.emplace_back(ComputeTargetConstraint(
+        last_detection, PoseUtils::Pose2dToAffine3d(robot_delta_pose), *it,
+        confidence));
+    robot_delta_poses.emplace_back(robot_delta_pose);
+
+    last_detection = *it;
+    last_robot_pose = robot_pose;
+  }
+
+  return {target_constraints, robot_delta_poses};
+}
+
+Eigen::Matrix3d DataAdapter::ComputeConfidence(
+    aos::distributed_clock::time_point start,
+    aos::distributed_clock::time_point end) {
+  constexpr size_t kX = 0;
+  constexpr size_t kY = 1;
+  constexpr size_t kTheta = 2;
+
+  // Uncertainty matrix between start and end
+  Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
+
+  {
+    // Noise for odometry-based robot position measurements
+    Eigen::Matrix3d Q_odometry = Eigen::Matrix3d::Zero();
+    Q_odometry(kX, kX) = std::pow(0.045, 2);
+    Q_odometry(kY, kY) = std::pow(0.045, 2);
+    Q_odometry(kTheta, kTheta) = std::pow(0.01, 2);
+
+    // Add uncertainty for robot position measurements from start to end
+    int iterations = (end - start) / frc971::controls::kLoopFrequency;
+    P += static_cast<double>(iterations) * Q_odometry;
+  }
+
+  {
+    // Noise for vision-based target localizations
+    Eigen::Matrix3d Q_vision = Eigen::Matrix3d::Zero();
+    Q_vision(kX, kX) = std::pow(0.09, 2);
+    Q_vision(kY, kY) = std::pow(0.09, 2);
+    Q_vision(kTheta, kTheta) = std::pow(0.02, 2);
+
+    // Add uncertainty for the 2 vision measurements (1 at start and 1 at end)
+    P += 2.0 * Q_vision;
+  }
+
+  return P.inverse();
+}
+
+ceres::examples::Constraint2d DataAdapter::ComputeTargetConstraint(
+    const TimestampedDetection &target_detection_start,
+    const Eigen::Affine3d &H_robotstart_robotend,
+    const TimestampedDetection &target_detection_end,
+    const Eigen::Matrix3d &confidence) {
+  // Compute the relative pose (constraint) between the two targets
+  Eigen::Affine3d H_targetstart_targetend =
+      target_detection_start.H_robot_target.inverse() * H_robotstart_robotend *
+      target_detection_end.H_robot_target;
+  ceres::examples::Pose2d target_constraint =
+      PoseUtils::Affine3dToPose2d(H_targetstart_targetend);
+
+  return ceres::examples::Constraint2d{
+      target_detection_start.id,     target_detection_end.id,
+      target_constraint.x,           target_constraint.y,
+      target_constraint.yaw_radians, confidence};
+}
+
+TargetMapper::TargetMapper(
+    std::map<TargetId, ceres::examples::Pose2d> target_poses,
+    std::vector<ceres::examples::Constraint2d> target_constraints)
+    : target_poses_(target_poses), target_constraints_(target_constraints) {}
+
+std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
+    std::vector<TargetMapper::TargetPose> target_poses, TargetId target_id) {
+  for (auto target_pose : target_poses) {
+    if (target_pose.id == target_id) {
+      return target_pose;
+    }
+  }
+
+  return std::nullopt;
+}
+
+// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+void TargetMapper::OutputPoses(
+    const std::map<int, ceres::examples::Pose2d> &poses) {
+  for (const auto &pair : poses) {
+    std::cout << pair.first << ": " << pair.second.x << " " << pair.second.y
+              << ' ' << pair.second.yaw_radians << std::endl;
+  }
+}
+
+// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+void TargetMapper::BuildOptimizationProblem(
+    std::map<int, ceres::examples::Pose2d> *poses,
+    const std::vector<ceres::examples::Constraint2d> &constraints,
+    ceres::Problem *problem) {
+  CHECK_NOTNULL(poses);
+  CHECK_NOTNULL(problem);
+  if (constraints.empty()) {
+    LOG(WARNING) << "No constraints, no problem to optimize.";
+    return;
+  }
+
+  ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
+  ceres::LocalParameterization *angle_local_parameterization =
+      ceres::examples::AngleLocalParameterization::Create();
+
+  for (std::vector<ceres::examples::Constraint2d>::const_iterator
+           constraints_iter = constraints.begin();
+       constraints_iter != constraints.end(); ++constraints_iter) {
+    const ceres::examples::Constraint2d &constraint = *constraints_iter;
+
+    std::map<int, ceres::examples::Pose2d>::iterator pose_begin_iter =
+        poses->find(constraint.id_begin);
+    CHECK(pose_begin_iter != poses->end())
+        << "Pose with ID: " << constraint.id_begin << " not found.";
+    std::map<int, ceres::examples::Pose2d>::iterator pose_end_iter =
+        poses->find(constraint.id_end);
+    CHECK(pose_end_iter != poses->end())
+        << "Pose with ID: " << constraint.id_end << " not found.";
+
+    const Eigen::Matrix3d sqrt_information =
+        constraint.information.llt().matrixL();
+    // Ceres will take ownership of the pointer.
+    ceres::CostFunction *cost_function =
+        ceres::examples::PoseGraph2dErrorTerm::Create(
+            constraint.x, constraint.y, constraint.yaw_radians,
+            sqrt_information);
+    problem->AddResidualBlock(
+        cost_function, loss_function, &pose_begin_iter->second.x,
+        &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
+        &pose_end_iter->second.x, &pose_end_iter->second.y,
+        &pose_end_iter->second.yaw_radians);
+
+    problem->SetParameterization(&pose_begin_iter->second.yaw_radians,
+                                 angle_local_parameterization);
+    problem->SetParameterization(&pose_end_iter->second.yaw_radians,
+                                 angle_local_parameterization);
+  }
+
+  // The pose graph optimization problem has three DOFs that are not fully
+  // constrained. This is typically referred to as gauge freedom. You can apply
+  // a rigid body transformation to all the nodes and the optimization problem
+  // will still have the exact same cost. The Levenberg-Marquardt algorithm has
+  // internal damping which mitigates this issue, but it is better to properly
+  // constrain the gauge freedom. This can be done by setting one of the poses
+  // as constant so the optimizer cannot change it.
+  std::map<int, ceres::examples::Pose2d>::iterator pose_start_iter =
+      poses->begin();
+  CHECK(pose_start_iter != poses->end()) << "There are no poses.";
+  problem->SetParameterBlockConstant(&pose_start_iter->second.x);
+  problem->SetParameterBlockConstant(&pose_start_iter->second.y);
+  problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
+}
+
+// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
+  CHECK_NOTNULL(problem);
+
+  ceres::Solver::Options options;
+  options.max_num_iterations = FLAGS_max_num_iterations;
+  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, problem, &summary);
+
+  std::cout << summary.FullReport() << '\n';
+
+  return summary.IsSolutionUsable();
+}
+
+void TargetMapper::Solve() {
+  ceres::Problem problem;
+  BuildOptimizationProblem(&target_poses_, target_constraints_, &problem);
+
+  CHECK(SolveOptimizationProblem(&problem))
+      << "The solve was not successful, exiting.";
+
+  OutputPoses(target_poses_);
+}
+
+}  // namespace frc971::vision
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
new file mode 100644
index 0000000..55d3c7e
--- /dev/null
+++ b/frc971/vision/target_mapper.h
@@ -0,0 +1,134 @@
+#ifndef FRC971_VISION_TARGET_MAPPER_H_
+#define FRC971_VISION_TARGET_MAPPER_H_
+
+#include <unordered_map>
+
+#include "aos/events/simulated_event_loop.h"
+#include "frc971/vision/ceres/types.h"
+
+namespace frc971::vision {
+
+// Estimates positions of vision targets (ex. April Tags) using
+// target detections relative to a robot (which were computed using robot
+// positions at the time of those detections). Solves SLAM problem to estimate
+// target locations using deltas between consecutive target detections.
+class TargetMapper {
+ public:
+  using TargetId = int;
+
+  struct TargetPose {
+    TargetId id;
+    ceres::examples::Pose2d pose;
+  };
+
+  // target_poses are initial guesses for the actual locations of the targets on
+  // the field.
+  // target_constraints are the deltas between consecutive target detections,
+  // and are usually prepared by the DataAdapter class below.
+  TargetMapper(std::map<TargetId, ceres::examples::Pose2d> target_poses,
+               std::vector<ceres::examples::Constraint2d> target_constraints);
+
+  void Solve();
+
+  static std::optional<TargetPose> GetTargetPoseById(
+      std::vector<TargetPose> target_poses, TargetId target_id);
+
+  std::map<TargetId, ceres::examples::Pose2d> target_poses() {
+    return target_poses_;
+  }
+
+ private:
+  // Output the poses to std::cout with format: ID: x y yaw_radians.
+  static void OutputPoses(const std::map<int, ceres::examples::Pose2d> &poses);
+
+  // Constructs the nonlinear least squares optimization problem from the
+  // pose graph constraints.
+  void BuildOptimizationProblem(
+      std::map<TargetId, ceres::examples::Pose2d> *target_poses,
+      const std::vector<ceres::examples::Constraint2d> &constraints,
+      ceres::Problem *problem);
+
+  // Returns true if the solve was successful.
+  bool SolveOptimizationProblem(ceres::Problem *problem);
+
+  std::map<TargetId, ceres::examples::Pose2d> target_poses_;
+  std::vector<ceres::examples::Constraint2d> target_constraints_;
+};
+
+// Utility functions for dealing with ceres::examples::Pose2d structs
+class PoseUtils {
+ public:
+  // Embeds a 2d pose into a 3d affine transformation to be used in 3d
+  // computation
+  static Eigen::Affine3d Pose2dToAffine3d(ceres::examples::Pose2d pose2d);
+  // Assumes only x and y translation, and only z rotation (yaw)
+  static ceres::examples::Pose2d Affine3dToPose2d(Eigen::Affine3d H);
+
+  // Computes pose_2 relative to pose_1. This is equivalent to (pose_1^-1 *
+  // pose_2)
+  static ceres::examples::Pose2d ComputeRelativePose(
+      ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2);
+
+  // Computes pose_2 given a pose_1 and pose_2 relative to pose_1. This is
+  // equivalent to (pose_1 * pose_2_relative)
+  static ceres::examples::Pose2d ComputeOffsetPose(
+      ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2_relative);
+};
+
+// Transforms robot position and target detection data into target constraints
+// to be used for mapping. Interpolates continous-time data, converting it to
+// discrete detection time steps.
+class DataAdapter {
+ public:
+  // Pairs pose with a time point
+  struct TimestampedPose {
+    aos::distributed_clock::time_point time;
+    ceres::examples::Pose2d pose;
+  };
+
+  // Pairs target detection with a time point
+  struct TimestampedDetection {
+    aos::distributed_clock::time_point time;
+    // Pose of target relative to robot
+    Eigen::Affine3d H_robot_target;
+    TargetMapper::TargetId id;
+  };
+
+  // Pairs consecutive target detections into constraints, and interpolates
+  // robot poses based on time points to compute motion between detections. This
+  // prepares data to be used by TargetMapper. Also returns vector of delta
+  // robot poses corresponding to each constraint, to be used for testing.
+  //
+  // Assumes both inputs are in chronological order.
+  static std::pair<std::vector<ceres::examples::Constraint2d>,
+                   std::vector<ceres::examples::Pose2d>>
+  MatchTargetDetections(
+      const std::vector<TimestampedPose> &timestamped_robot_poses,
+      const std::vector<TimestampedDetection> &timestamped_target_detections);
+
+  // Computes inverse of covariance matrix, assuming there was a target
+  // detection between robot movement over the given time period. Ceres calls
+  // this matrix the "information"
+  static Eigen::Matrix3d ComputeConfidence(
+      aos::distributed_clock::time_point start,
+      aos::distributed_clock::time_point end);
+
+ private:
+  static ceres::examples::Pose2d InterpolatePose(
+      const TimestampedPose &pose_start, const TimestampedPose &pose_end,
+      aos::distributed_clock::time_point time);
+
+  // Computes the constraint between the start and end pose of the targets: the
+  // relative pose between the start and end target locations in the frame of
+  // the start target. Takes into account the robot motion in the time between
+  // the two detections.
+  static ceres::examples::Constraint2d ComputeTargetConstraint(
+      const TimestampedDetection &target_detection_start,
+      const Eigen::Affine3d &H_robotstart_robotend,
+      const TimestampedDetection &target_detection_end,
+      const Eigen::Matrix3d &confidence);
+};
+
+}  // namespace frc971::vision
+
+#endif  // FRC971_VISION_TARGET_MAPPER_H_
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
new file mode 100644
index 0000000..37b037b
--- /dev/null
+++ b/frc971/vision/target_mapper_test.cc
@@ -0,0 +1,413 @@
+#include "frc971/vision/target_mapper.h"
+
+#include <random>
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/random_seed.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace frc971::vision {
+
+namespace {
+constexpr double kToleranceMeters = 0.05;
+constexpr double kToleranceRadians = 0.05;
+}  // namespace
+
+#define EXPECT_POSE_NEAR(pose1, pose2)             \
+  EXPECT_NEAR(pose1.x, pose2.x, kToleranceMeters); \
+  EXPECT_NEAR(pose1.y, pose2.y, kToleranceMeters); \
+  EXPECT_NEAR(pose1.yaw_radians, pose2.yaw_radians, kToleranceRadians);
+
+#define EXPECT_POSE_EQ(pose1, pose2)  \
+  EXPECT_DOUBLE_EQ(pose1.x, pose2.x); \
+  EXPECT_DOUBLE_EQ(pose1.y, pose2.y); \
+  EXPECT_DOUBLE_EQ(pose1.yaw_radians, pose2.yaw_radians);
+
+#define EXPECT_BETWEEN_EXCLUSIVE(value, a, b) \
+  {                                           \
+    auto low = std::min(a, b);                \
+    auto high = std::max(a, b);               \
+    EXPECT_GT(value, low);                    \
+    EXPECT_LT(value, high);                   \
+  }
+
+namespace {
+// Expects angles to be normalized
+double DeltaAngle(double a, double b) {
+  double delta = std::abs(a - b);
+  return std::min(delta, (2.0 * M_PI) - delta);
+}
+}  // namespace
+
+// Expects angles to be normalized
+#define EXPECT_ANGLE_BETWEEN_EXCLUSIVE(theta, a, b)  \
+  EXPECT_LT(DeltaAngle(a, theta), DeltaAngle(a, b)); \
+  EXPECT_LT(DeltaAngle(b, theta), DeltaAngle(a, b));
+
+#define EXPECT_POSE_IN_RANGE(interpolated_pose, pose_start, pose_end)      \
+  EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.x, pose_start.x, pose_end.x); \
+  EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.y, pose_start.y, pose_end.y); \
+  EXPECT_ANGLE_BETWEEN_EXCLUSIVE(interpolated_pose.yaw_radians,            \
+                                 pose_start.yaw_radians,                   \
+                                 pose_end.yaw_radians);
+
+// Both confidence matrixes should have the same dimensions and be square
+#define EXPECT_CONFIDENCE_GT(confidence1, confidence2) \
+  {                                                    \
+    ASSERT_EQ(confidence1.rows(), confidence2.rows()); \
+    ASSERT_EQ(confidence1.rows(), confidence1.cols()); \
+    ASSERT_EQ(confidence2.rows(), confidence2.cols()); \
+    for (size_t i = 0; i < confidence1.rows(); i++) {  \
+      EXPECT_GT(confidence1(i, i), confidence2(i, i)); \
+    }                                                  \
+  }
+
+namespace {
+ceres::examples::Pose2d MakePose(double x, double y, double yaw_radians) {
+  return ceres::examples::Pose2d{x, y, yaw_radians};
+}
+
+bool TargetIsInView(TargetMapper::TargetPose target_detection) {
+  // And check if it is within the fov of the robot /
+  // camera, assuming camera is pointing in the
+  // positive x-direction of the robot
+  double angle_to_target =
+      atan2(target_detection.pose.y, target_detection.pose.x);
+
+  // Simulated camera field of view, in radians
+  constexpr double kCameraFov = M_PI_2;
+  if (fabs(angle_to_target) <= kCameraFov / 2.0) {
+    VLOG(2) << "Found target in view, based on T = " << target_detection.pose.x
+            << ", " << target_detection.pose.y << " with angle "
+            << angle_to_target;
+    return true;
+  } else {
+    return false;
+  }
+}
+
+aos::distributed_clock::time_point TimeInMs(size_t ms) {
+  return aos::distributed_clock::time_point(std::chrono::milliseconds(ms));
+}
+
+}  // namespace
+
+TEST(DataAdapterTest, Interpolation) {
+  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+      {TimeInMs(0), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
+      {TimeInMs(5), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
+      {TimeInMs(10), ceres::examples::Pose2d{3.0, 1.0, M_PI_2}},
+      {TimeInMs(15), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
+      {TimeInMs(20), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
+      {TimeInMs(25), ceres::examples::Pose2d{10.0, -32.0, M_PI_2}},
+      {TimeInMs(30), ceres::examples::Pose2d{-15.0, 12.0, 0.0}},
+      {TimeInMs(35), ceres::examples::Pose2d{-15.0, 12.0, 0.0}}};
+  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+      {{TimeInMs(5),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+        0},
+       {TimeInMs(9),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+        1},
+       {TimeInMs(9),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+        2},
+       {TimeInMs(15),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+        0},
+       {TimeInMs(16),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+        2},
+       {TimeInMs(27),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+        1}};
+  auto [target_constraints, robot_delta_poses] =
+      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+                                         timestamped_target_detections);
+
+  // Check that target constraints got inserted in the
+  // correct spots
+  EXPECT_EQ(target_constraints.size(),
+            timestamped_target_detections.size() - 1);
+  for (auto it = target_constraints.begin(); it < target_constraints.end();
+       it++) {
+    auto timestamped_it = timestamped_target_detections.begin() +
+                          (it - target_constraints.begin());
+    EXPECT_EQ(it->id_begin, timestamped_it->id);
+    EXPECT_EQ(it->id_end, (timestamped_it + 1)->id);
+  }
+
+  // Check that poses were interpolated correctly.
+  // Keep track of the computed robot pose by adding the delta poses
+  auto computed_robot_pose = timestamped_robot_poses[1].pose;
+
+  computed_robot_pose =
+      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[0]);
+  EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
+                       timestamped_robot_poses[2].pose);
+
+  computed_robot_pose =
+      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[1]);
+  EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
+                       timestamped_robot_poses[2].pose);
+  EXPECT_POSE_EQ(robot_delta_poses[1], MakePose(0.0, 0.0, 0.0));
+
+  computed_robot_pose =
+      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[2]);
+  EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[3].pose);
+
+  computed_robot_pose =
+      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[3]);
+  EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[4].pose);
+
+  computed_robot_pose =
+      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[4]);
+  EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[5].pose,
+                       timestamped_robot_poses[6].pose);
+
+  // Check the confidence matrices. Don't check the actual values
+  // in case the constants change, just check the confidence of contraints
+  // relative to each other, as constraints over longer time periods should have
+  // lower confidence.
+  const auto confidence_0ms =
+      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(0));
+  const auto confidence_1ms =
+      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(1));
+  const auto confidence_4ms =
+      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(4));
+  const auto confidence_6ms =
+      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(6));
+  const auto confidence_11ms =
+      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(11));
+
+  // Check relative magnitude of different confidences.
+  // Confidences for 0-5ms, 5-10ms, and 10-15ms periods are equal
+  // because they fit within one control loop iteration.
+  EXPECT_EQ(confidence_0ms, confidence_1ms);
+  EXPECT_EQ(confidence_1ms, confidence_4ms);
+  EXPECT_CONFIDENCE_GT(confidence_4ms, confidence_6ms);
+  EXPECT_CONFIDENCE_GT(confidence_6ms, confidence_11ms);
+
+  // Check that confidences (information) of actual constraints are correct
+  EXPECT_EQ(target_constraints[0].information, confidence_4ms);
+  EXPECT_EQ(target_constraints[1].information, confidence_0ms);
+  EXPECT_EQ(target_constraints[2].information, confidence_6ms);
+  EXPECT_EQ(target_constraints[3].information, confidence_1ms);
+  EXPECT_EQ(target_constraints[4].information, confidence_11ms);
+}
+
+TEST(TargetMapperTest, TwoTargetsOneConstraint) {
+  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+  target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
+  target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};
+
+  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+      {TimeInMs(5), ceres::examples::Pose2d{2.0, 0.0, 0.0}},
+      {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
+      {TimeInMs(15), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
+  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+      {{TimeInMs(5),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{3.0, 0.0, M_PI}),
+        0},
+       {TimeInMs(10),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.0, 0.0, 0.0}),
+        1}};
+  auto target_constraints =
+      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+                                         timestamped_target_detections)
+          .first;
+
+  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+  mapper.Solve();
+
+  ASSERT_EQ(mapper.target_poses().size(), 2);
+  EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
+  EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
+}
+
+TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
+  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+  target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
+  target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, -M_PI_2};
+
+  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+      {TimeInMs(5), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
+      {TimeInMs(10), ceres::examples::Pose2d{3.0, 0.0, 0.0}},
+      {TimeInMs(15), ceres::examples::Pose2d{4.0, 0.0, 0.0}},
+      {TimeInMs(20), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
+  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+      {{TimeInMs(5),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{6.0, 0.0, M_PI}),
+        0},
+       {TimeInMs(10),
+        PoseUtils::Pose2dToAffine3d(
+            ceres::examples::Pose2d{-8.0, 0.0, -M_PI_2}),
+        1},
+       {TimeInMs(15),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{1.0, 0.0, M_PI}),
+        0}};
+  auto target_constraints =
+      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+                                         timestamped_target_detections)
+          .first;
+
+  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+  mapper.Solve();
+
+  ASSERT_EQ(mapper.target_poses().size(), 2);
+  EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
+  EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, -M_PI_2));
+}
+
+TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
+  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+  target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
+  target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};
+
+  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+      {TimeInMs(5), ceres::examples::Pose2d{1.99, 0.0, 0.0}},
+      {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
+      {TimeInMs(15), ceres::examples::Pose2d{-1.01, -0.01, 0.004}}};
+  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+      {{TimeInMs(5),
+        PoseUtils::Pose2dToAffine3d(
+            ceres::examples::Pose2d{3.01, 0.001, M_PI - 0.001}),
+        0},
+       {TimeInMs(10),
+        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.01, 0.0, 0.0}),
+        1}};
+
+  auto target_constraints =
+      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+                                         timestamped_target_detections)
+          .first;
+
+  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+  mapper.Solve();
+
+  ASSERT_EQ(mapper.target_poses().size(), 2);
+  EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
+  EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
+}
+
+TEST(TargetMapperTest, MultiTargetCircleMotion) {
+  // Build set of target locations wrt global origin
+  // For simplicity, do this on a grid of the field
+  double field_half_length = 7.5;  // half length of the field
+  double field_half_width = 5.0;   // half width of the field
+  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+  std::vector<TargetMapper::TargetPose> actual_target_poses;
+  for (int i = 0; i < 3; i++) {
+    for (int j = 0; j < 3; j++) {
+      TargetMapper::TargetId target_id = i * 3 + j;
+      TargetMapper::TargetPose target_pose{
+          target_id, ceres::examples::Pose2d{field_half_length * (1 - i),
+                                             field_half_width * (1 - j), 0.0}};
+      actual_target_poses.emplace_back(target_pose);
+      target_poses[target_id] = target_pose.pose;
+      VLOG(2) << "VERTEX_SE2 " << target_id << " " << target_pose.pose.x << " "
+              << target_pose.pose.y << " " << target_pose.pose.yaw_radians;
+    }
+  }
+
+  // Now, create a bunch of robot poses and target
+  // observations
+  size_t dt = 1;
+
+  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses;
+  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
+
+  constexpr size_t kTotalSteps = 100;
+  for (size_t step_count = 0; step_count < kTotalSteps; step_count++) {
+    size_t t = dt * step_count;
+    // Circle clockwise around the center of the field
+    double robot_theta = t;
+    double robot_x = (field_half_length / 2.0) * cos(robot_theta);
+    double robot_y = (-field_half_width / 2.0) * sin(robot_theta);
+
+    ceres::examples::Pose2d robot_pose{robot_x, robot_y, robot_theta};
+    for (TargetMapper::TargetPose target_pose : actual_target_poses) {
+      TargetMapper::TargetPose target_detection = {
+          .id = target_pose.id,
+          .pose = PoseUtils::ComputeRelativePose(robot_pose, target_pose.pose)};
+      if (TargetIsInView(target_detection)) {
+        // Define random generator with Gaussian
+        // distribution
+        const double mean = 0.0;
+        const double stddev = 1.0;
+        // Can play with this to see how it impacts
+        // randomness
+        constexpr double kNoiseScale = 0.01;
+        std::default_random_engine generator(aos::testing::RandomSeed());
+        std::normal_distribution<double> dist(mean, stddev);
+
+        target_detection.pose.x += dist(generator) * kNoiseScale;
+        target_detection.pose.y += dist(generator) * kNoiseScale;
+        robot_pose.x += dist(generator) * kNoiseScale;
+        robot_pose.y += dist(generator) * kNoiseScale;
+
+        auto time_point =
+            aos::distributed_clock::time_point(std::chrono::milliseconds(t));
+        timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
+            .time = time_point, .pose = robot_pose});
+        timestamped_target_detections.emplace_back(
+            DataAdapter::TimestampedDetection{
+                .time = time_point,
+                .H_robot_target =
+                    PoseUtils::Pose2dToAffine3d(target_detection.pose),
+                .id = target_detection.id});
+      }
+    }
+  }
+
+  {
+    // Add in a robot pose after all target poses
+    auto final_robot_pose =
+        timestamped_robot_poses[timestamped_robot_poses.size() - 1];
+    timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
+        .time = final_robot_pose.time + std::chrono::milliseconds(dt),
+        .pose = final_robot_pose.pose});
+  }
+
+  auto target_constraints =
+      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+                                         timestamped_target_detections)
+          .first;
+  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+  mapper.Solve();
+
+  for (auto [target_pose_id, mapper_target_pose] : mapper.target_poses()) {
+    TargetMapper::TargetPose actual_target_pose =
+        TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
+            .value();
+    EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
+  }
+
+  //
+  // See what happens when we don't start with the
+  // correct values
+  //
+  for (auto [target_id, target_pose] : target_poses) {
+    // Skip first pose, since that needs to be correct
+    // and is fixed in the solver
+    if (target_id != 0) {
+      ceres::examples::Pose2d bad_pose{0.0, 0.0, M_PI / 2.0};
+      target_poses[target_id] = bad_pose;
+    }
+  }
+
+  frc971::vision::TargetMapper mapper_bad_poses(target_poses,
+                                                target_constraints);
+  mapper_bad_poses.Solve();
+
+  for (auto [target_pose_id, mapper_target_pose] :
+       mapper_bad_poses.target_poses()) {
+    TargetMapper::TargetPose actual_target_pose =
+        TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
+            .value();
+    EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
+  }
+}
+
+}  // namespace frc971::vision