diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 508a6a6..13bd6e6 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -1,6 +1,8 @@
 #ifndef FRC971_CONTROL_LOOPS_POSE_H_
 #define FRC971_CONTROL_LOOPS_POSE_H_
 
+#include <vector>
+
 #include "Eigen/Dense"
 #include "aos/util/math.h"
 
@@ -39,6 +41,9 @@
   // The type that contains the translational (x, y, z) component of the Pose.
   typedef Eigen::Matrix<Scalar, 3, 1> Pos;
 
+  // Provide a default constructor that crease a pose at the origin.
+  TypedPose() : TypedPose({0.0, 0.0, 0.0}, 0.0) {}
+
   // Construct a Pose in the absolute frame with a particular position and yaw.
   TypedPose(const Pos &abs_pos, Scalar theta) : pos_(abs_pos), theta_(theta) {}
   // Construct a Pose relative to another Pose (base).
@@ -69,9 +74,12 @@
   // Provide access to the position and yaw relative to the base Pose.
   Pos rel_pos() const { return pos_; }
   Scalar rel_theta() const { return theta_; }
+  const TypedPose<Scalar> *base() const { return base_; }
 
   Pos *mutable_pos() { return &pos_; }
   void set_theta(Scalar theta) { theta_ = theta; }
+  // Swap out the base Pose, keeping the current relative position/angle.
+  void set_base(const TypedPose<Scalar> *new_base) { base_ = new_base; }
 
   // For 2-D calculation, provide the heading, which is distinct from the
   // yaw/theta value. heading is the heading relative to the base Pose if you
@@ -137,6 +145,7 @@
 template <typename Scalar = double>
 class TypedLineSegment {
  public:
+  TypedLineSegment() {}
   TypedLineSegment(const TypedPose<Scalar> &pose1,
                    const TypedPose<Scalar> &pose2)
       : pose1_(pose1), pose2_(pose2) {}
@@ -162,9 +171,18 @@
            (::aos::math::PointsAreCCW<Scalar>(p1, p2, q1) !=
             ::aos::math::PointsAreCCW<Scalar>(p1, p2, q2));
   }
+
+  TypedPose<Scalar> pose1() const { return pose1_; }
+  TypedPose<Scalar> pose2() const { return pose2_; }
+  TypedPose<Scalar> *mutable_pose1() { return &pose1_; }
+  TypedPose<Scalar> *mutable_pose2() { return &pose2_; }
+
+  ::std::vector<TypedPose<Scalar>> PlotPoints() const {
+    return {pose1_, pose2_};
+  }
  private:
-  const TypedPose<Scalar> pose1_;
-  const TypedPose<Scalar> pose2_;
+  TypedPose<Scalar> pose1_;
+  TypedPose<Scalar> pose2_;
 };  // class TypedLineSegment
 
 typedef TypedLineSegment<double> LineSegment;
diff --git a/frc971/control_loops/pose_test.cc b/frc971/control_loops/pose_test.cc
index cb994a2..4fd3f15 100644
--- a/frc971/control_loops/pose_test.cc
+++ b/frc971/control_loops/pose_test.cc
@@ -23,6 +23,8 @@
 
   EXPECT_EQ(1.0, pose.abs_pos().x());
   EXPECT_EQ(1.0, pose.abs_pos().y());
+  EXPECT_EQ(1.0, pose.abs_xy().x());
+  EXPECT_EQ(1.0, pose.abs_xy().y());
   EXPECT_EQ(0.5, pose.abs_pos().z());
 
   EXPECT_EQ(0.5, pose.rel_theta());
@@ -32,6 +34,11 @@
   EXPECT_EQ(3.14, pose.rel_theta());
   pose.mutable_pos()->x() = 9.71;
   EXPECT_EQ(9.71, pose.rel_pos().x());
+
+  EXPECT_EQ(nullptr, pose.base());
+  Pose new_base;
+  pose.set_base(&new_base);
+  EXPECT_EQ(&new_base, pose.base());
 }
 
 // Check that Poses behave as expected when constructed relative to another
@@ -76,6 +83,22 @@
   EXPECT_NEAR(rel1.abs_theta(), abs.rel_theta(), kEps);
 }
 
+// Tests that basic accessors for LineSegment behave as expected.
+TEST(LineSegmentTest, BasicAccessorTest) {
+  LineSegment l;
+  EXPECT_EQ(0.0, l.pose1().rel_theta());
+  l.mutable_pose1()->set_theta(1.234);
+  EXPECT_EQ(1.234, l.pose1().rel_theta());
+  EXPECT_EQ(0.0, l.pose2().rel_theta());
+  l.mutable_pose2()->set_theta(5.678);
+  EXPECT_EQ(5.678, l.pose2().rel_theta());
+
+  const ::std::vector<Pose> plot_pts = l.PlotPoints();
+  ASSERT_EQ(2u, plot_pts.size());
+  EXPECT_EQ(l.pose1().rel_theta(), plot_pts[0].rel_theta());
+  EXPECT_EQ(l.pose2().rel_theta(), plot_pts[1].rel_theta());
+}
+
 // Tests that basic checks for intersection function as expected.
 TEST(LineSegmentTest, TrivialIntersectTest) {
   Pose p1({0, 0, 0}, 0.0), p2({2, 0, 0}, 0.0);
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 9748a41..26b2cb2 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python3
 from __future__ import print_function
 import os
+import sys
 import copy
 import basic_window
 from color import Color, palette
@@ -13,7 +14,7 @@
 from gi.repository import Gdk, Gtk, GLib
 import cairo
 import enum
-import csv  # For writing to csv files
+import json # For writing to json files
 
 from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
 
@@ -124,6 +125,9 @@
         self.x = 0
         self.y = 0
 
+        module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+        self.path_to_export = os.path.join(module_path,
+            'points_for_pathedit.json')
         # update list of control points
         self.point_selected = False
         # self.adding_spline = False
@@ -335,40 +339,6 @@
                         cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
                         display_text(cr, str(i), 0.5, 0.5, 2, 2)
 
-        elif self.mode == Mode.kExporting:
-            set_color(cr, palette["BLACK"])
-            cr.move_to(-300, 170)
-            display_text(cr, "VIEWING", 1, 1, 1, 1)
-            set_color(cr, palette["GREY"])
-
-            if len(self.selected_points) > 0:
-                print("SELECTED_POINTS: " + str(len(self.selected_points)))
-                print("ITEMS:")
-                # for item in self.selected_points:
-                #     print(str(item))
-                for i, point in enumerate(self.selected_points):
-                    # print("I: " + str(i))
-                    draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1] - 15)
-                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
-        elif self.mode == Mode.kImporting:
-            set_color(cr, palette["BLACK"])
-            cr.move_to(-300, 170)
-            display_text(cr, "VIEWING", 1, 1, 1, 1)
-            set_color(cr, palette["GREY"])
-
-            if len(self.selected_points) > 0:
-                print("SELECTED_POINTS: " + str(len(self.selected_points)))
-                print("ITEMS:")
-                for item in self.selected_points:
-                    print(str(item))
-                for i, point in enumerate(self.selected_points):
-                    print("I: " + str(i))
-                    draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1] - 15)
-                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
         elif self.mode == Mode.kConstraint:
             print("Drawn")
             set_color(cr, palette["BLACK"])
@@ -397,26 +367,22 @@
             print("Found q key and exiting.")
             quit_main_loop()
         if keyval == Gdk.KEY_e:
-            self.mode = Mode.kExporting
-            # Will export to csv file
-            with open('points_for_pathedit.csv', mode='w') as points_file:
-                writer = csv.writer(
-                    points_file,
-                    delimiter=',',
-                    quotechar='"',
-                    quoting=csv.QUOTE_MINIMAL)
-                for item in self.selected_points:
-                    writer.writerow([str(item[0]), str(item[1])])
-                    print("Wrote: " + str(item[0]) + " " + str(item[1]))
+            # Will export to json file
+            self.mode = Mode.kEditing
+            print(str(sys.argv))
+            print('out to: ', self.path_to_export)
+            exportList = [l.tolist() for l in self.splines]
+            with open(self.path_to_export, mode='w') as points_file:
+                json.dump(exportList, points_file)
+                print("Wrote: " + str(self.splines))
         if keyval == Gdk.KEY_i:
-            self.mode = Mode.kImporting
-            # import from csv file
+            # import from json file
+            self.mode = Mode.kEditing
             self.selected_points = []
-            with open('points_for_pathedit.csv') as points_file:
-                reader = csv.reader(points_file, delimiter=',')
-                for row in reader:
-                    self.add_point(float(row[0]), float(row[1]))
-                    print("Added: " + row[0] + " " + row[1])
+            self.splines = []
+            with open(self.path_to_export) as points_file:
+                self.splines = json.load(points_file)
+                print("Added: " + str(self.splines))
         if keyval == Gdk.KEY_p:
             self.mode = Mode.kPlacing
             # F0 = A1
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 7135e8b..ff53b7b 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -35,6 +35,7 @@
  public:
   typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
   TypedTarget(const Pose &pose) : pose_(pose) {}
+  TypedTarget() {}
   Pose pose() const { return pose_; }
 
   bool occluded() const { return occluded_; }
@@ -117,6 +118,8 @@
 
     // The target that this view corresponds to.
     const TypedTarget<Scalar> *target;
+    // The Pose the camera was at when viewing the target:
+    Pose camera_pose;
   };
 
   // Important parameters for dealing with camera noise calculations.
@@ -172,11 +175,12 @@
   // separately for simulation.
   ::aos::SizedArray<TargetView, num_targets> target_views() const {
     ::aos::SizedArray<TargetView, num_targets> views;
+    Pose camera_abs_pose = pose_.Rebase(nullptr);
     // Because there are num_targets in targets_ and because AddTargetIfVisible
     // adds at most 1 view to views, we should never exceed the size of
     // SizedArray.
     for (const auto &target : targets_) {
-      AddTargetIfVisible(target, &views);
+      AddTargetIfVisible(target, camera_abs_pose, &views);
     }
     return views;
   }
@@ -189,12 +193,12 @@
   ::std::vector<::std::vector<Pose>> PlotPoints() const {
     ::std::vector<::std::vector<Pose>> list_of_lists;
     for (const auto &view : target_views()) {
-      list_of_lists.push_back({pose_, view.target.pose()});
+      list_of_lists.push_back({pose_, view.target->pose()});
     }
     return list_of_lists;
   }
 
-  const Pose pose() const { return pose_; }
+  const Pose &pose() const { return pose_; }
   Scalar fov() const { return fov_; }
 
  private:
@@ -202,11 +206,12 @@
   // If the specified target is visible from the current camera Pose, adds it to
   // the views array.
   void AddTargetIfVisible(
-      const TypedTarget<Scalar> &target,
+      const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
       ::aos::SizedArray<TargetView, num_targets> *views) const;
 
   // The Pose of this camera.
   const Pose pose_;
+
   // Field of view of the camera, in radians.
   const Scalar fov_;
 
@@ -215,6 +220,8 @@
   const NoiseParameters noise_parameters_;
 
   // A list of all the targets on the field.
+  // TODO(james): Is it worth creating some sort of cache for the targets and
+  // obstacles? e.g., passing around pointer to the targets/obstacles.
   const ::std::array<TypedTarget<Scalar>, num_targets> targets_;
   // Known obstacles on the field which can interfere with our view of the
   // targets. An "obstacle" is a line segment which we cannot see through, as
@@ -225,7 +232,7 @@
 
 template <int num_targets, int num_obstacles, typename Scalar>
 void TypedCamera<num_targets, num_obstacles, Scalar>::AddTargetIfVisible(
-    const TypedTarget<Scalar> &target,
+    const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
     ::aos::SizedArray<TargetView, num_targets> *views) const {
   if (target.occluded()) {
     return;
@@ -233,8 +240,6 @@
 
   // Precompute the current absolute pose of the camera, because we will reuse
   // it a bunch.
-  const Pose camera_abs_pose = pose_.Rebase(nullptr);
-
   const Pose relative_pose = target.pose().Rebase(&camera_abs_pose);
   const Scalar heading = relative_pose.heading();
   const Scalar distance = relative_pose.xy_norm();
@@ -280,6 +285,7 @@
   view.noise = {noise_parameters_.heading_noise, distance_noise, height_noise,
                 skew_noise};
   view.target = &target;
+  view.camera_pose = camera_abs_pose;
   views->push_back(view);
 }
 
diff --git a/y2019/control_loops/drivetrain/camera_test.cc b/y2019/control_loops/drivetrain/camera_test.cc
index 9a73ccf..ce1a2a4 100644
--- a/y2019/control_loops/drivetrain/camera_test.cc
+++ b/y2019/control_loops/drivetrain/camera_test.cc
@@ -93,6 +93,14 @@
   EXPECT_GT(views[0].noise.distance, 0.0);
   EXPECT_GT(views[0].noise.skew, 0.0);
   EXPECT_GT(views[0].noise.height, 0.0);
+
+  // Check that the PlotPoints for debugging are as expected (should be a single
+  // line from the camera to the one visible target).
+  const auto plot_pts = camera_.PlotPoints();
+  ASSERT_EQ(1u, plot_pts.size());
+  ASSERT_EQ(2u, plot_pts[0].size());
+  EXPECT_EQ(camera_.pose().abs_pos(), plot_pts[0][0].abs_pos());
+  EXPECT_EQ(views[0].target->pose().abs_pos(), plot_pts[0][1].abs_pos());
 }
 
 // Check that occluding the middle target makes it invisible.
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
new file mode 100644
index 0000000..6736c24
--- /dev/null
+++ b/y2019/vision/BUILD
@@ -0,0 +1,80 @@
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+
+package(default_visibility = ["//visibility:public"])
+
+VISION_TARGETS = [ "//tools:k8", "//tools:armhf-debian"]
+
+cc_library(
+    name = "target_finder",
+    srcs = ["target_finder.cc", "target_geometry.cc"],
+    hdrs = ["target_finder.h", "target_types.h"],
+    deps = [
+        "@com_google_ceres_solver//:ceres",
+        "//aos/vision/blob:hierarchical_contour_merge",
+        "//aos/vision/blob:region_alloc",
+        "//aos/vision/blob:contour",
+        "//aos/vision/blob:threshold",
+        "//aos/vision/blob:transpose",
+        "//aos/vision/debug:overlay",
+        "//aos/vision/math:vector",
+    ],
+    restricted_to = VISION_TARGETS,
+)
+
+gtk_dependent_cc_binary(
+    name = "debug_viewer",
+    srcs = ["debug_viewer.cc"],
+    deps = [
+        ":target_finder",
+        "//aos/vision/blob:move_scale",
+        "//aos/vision/blob:threshold",
+        "//aos/vision/blob:transpose",
+        "//aos/vision/debug:debug_framework",
+        "//aos/vision/math:vector",
+    ],
+    copts = ["-Wno-unused-variable"],
+    restricted_to = VISION_TARGETS,
+)
+
+cc_binary(
+    name = "target_sender",
+    srcs = ["target_sender.cc"],
+    deps = [
+         ":target_finder",
+         "//y2019/jevois:serial",
+         "//aos/logging",
+         "//aos/logging:implementations",
+         "//aos/vision/blob:find_blob",
+         "//aos/vision/blob:codec",
+         "//aos/vision/events:epoll_events",
+         "//aos/vision/events:socket_types",
+         "//aos/vision/events:udp",
+         "//aos/vision/image:image_stream",
+         "//aos/vision/image:reader",
+         "@com_google_ceres_solver//:ceres",
+    ],
+    restricted_to = VISION_TARGETS,
+)
+
+"""
+cc_binary(
+    name = "calibration",
+    srcs = ["calibration.cc"],
+    deps = [
+         ":target_finder",
+         "//aos/logging",
+         "//aos/logging:implementations",
+         "//aos/vision/blob:find_blob",
+         "//aos/vision/blob:codec",
+         "//aos/vision/events:epoll_events",
+         "//aos/vision/events:socket_types",
+         "//aos/vision/events:udp",
+         "//aos/vision/image:image_stream",
+         "//aos/vision/image:reader",
+         "@com_google_ceres_solver//:ceres",
+    ],
+    restricted_to = VISION_TARGETS,
+)
+"""
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
new file mode 100644
index 0000000..4f43c9a
--- /dev/null
+++ b/y2019/vision/debug_viewer.cc
@@ -0,0 +1,269 @@
+#include <Eigen/Dense>
+#include <iostream>
+
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/move_scale.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/debug_framework.h"
+#include "aos/vision/math/vector.h"
+
+using aos::vision::ImageRange;
+using aos::vision::ImageFormat;
+using aos::vision::RangeImage;
+using aos::vision::AnalysisAllocator;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+using aos::vision::Segment;
+using aos::vision::PixelRef;
+
+namespace y2019 {
+namespace vision {
+
+std::vector<PixelRef> GetNColors(size_t num_colors) {
+  std::vector<PixelRef> colors;
+  for (size_t i = 0; i < num_colors; ++i) {
+    int quadrent = i * 6 / num_colors;
+    uint8_t alpha = (256 * 6 * i - quadrent * num_colors * 256) / num_colors;
+    uint8_t inv_alpha = 255 - alpha;
+    switch (quadrent) {
+      case 0:
+        colors.push_back(PixelRef{255, alpha, 0});
+        break;
+      case 1:
+        colors.push_back(PixelRef{inv_alpha, 255, 0});
+        break;
+      case 2:
+        colors.push_back(PixelRef{0, 255, alpha});
+        break;
+      case 3:
+        colors.push_back(PixelRef{0, inv_alpha, 255});
+        break;
+      case 4:
+        colors.push_back(PixelRef{alpha, 0, 255});
+        break;
+      case 5:
+        colors.push_back(PixelRef{255, 0, inv_alpha});
+        break;
+    }
+  }
+  return colors;
+}
+
+class FilterHarness : public aos::vision::FilterHarness {
+ public:
+  aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
+    return finder_.Threshold(image);
+  }
+
+  void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
+    viewer_ = viewer;
+    viewer_->SetScale(0.75);
+    overlays_.push_back(&overlay_);
+    overlays_.push_back(finder_.GetOverlay());
+    viewer_->view()->SetOverlays(&overlays_);
+  }
+
+  void DrawBlob(const RangeImage &blob, PixelRef color) {
+    if (viewer_) {
+      BlobList list;
+      list.push_back(blob);
+      viewer_->DrawBlobList(list, color);
+    }
+  }
+
+  bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+    imgs_last_ = imgs;
+    fmt_last_ = fmt;
+    // reset for next drawing cycle
+    for (auto &overlay : overlays_) {
+      overlay->Reset();
+    }
+
+    if (draw_select_blob_ || draw_raw_poly_ || draw_components_ ||
+        draw_raw_target_ || draw_raw_IR_ || draw_results_) {
+      printf("_____ New Image _____\n");
+    }
+
+    // Remove bad blobs.
+    finder_.PreFilter(&imgs);
+
+    // Find polygons from blobs.
+    std::vector<std::vector<Segment<2>>> raw_polys;
+    for (const RangeImage &blob : imgs) {
+      std::vector<Segment<2>> polygon =
+          finder_.FillPolygon(blob, draw_raw_poly_);
+      if (polygon.empty()) {
+        DrawBlob(blob, {255, 0, 0});
+      } else {
+        raw_polys.push_back(polygon);
+        if (draw_select_blob_) {
+          DrawBlob(blob, {0, 0, 255});
+        }
+        if (draw_raw_poly_) {
+          std::vector<PixelRef> colors = GetNColors(polygon.size());
+          std::vector<Vector<2>> corners;
+          for (size_t i = 0; i < 4; ++i) {
+            corners.push_back(polygon[i].Intersect(polygon[(i + 1) % 4]));
+          }
+
+          for (size_t i = 0; i < 4; ++i) {
+            overlay_.AddLine(corners[i], corners[(i + 1) % 4], colors[i]);
+          }
+        }
+      }
+    }
+
+    // Calculate each component side of a possible target.
+    std::vector<TargetComponent> target_component_list =
+        finder_.FillTargetComponentList(raw_polys);
+    if (draw_components_) {
+      for (const TargetComponent &comp : target_component_list) {
+        DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
+                      {0, 0, 255});
+      }
+    }
+
+    // Put the compenents together into targets.
+    std::vector<Target> target_list = finder_.FindTargetsFromComponents(
+        target_component_list, draw_raw_target_);
+    if (draw_raw_target_) {
+      for (const Target &target : target_list) {
+        DrawTarget(target);
+      }
+    }
+
+    // Use the solver to generate an intermediate version of our results.
+    std::vector<IntermediateResult> results;
+    for (const Target &target : target_list) {
+      results.emplace_back(finder_.ProcessTargetToResult(target, true));
+      if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
+    }
+
+    // Check that our current results match possible solutions.
+    results = finder_.FilterResults(results);
+    if (draw_results_) {
+      for (const IntermediateResult &res : results) {
+        DrawResult(res, {0, 255, 0});
+        DrawTarget(res, {0, 255, 0});
+      }
+    }
+
+    // If the target list is not empty then we found a target.
+    return !results.empty();
+  }
+
+  std::function<void(uint32_t)> RegisterKeyPress() override {
+    return [this](uint32_t key) {
+      (void)key;
+      if (key == 'z') {
+        draw_results_ = !draw_results_;
+      } else if (key == 'x') {
+        draw_raw_IR_ = !draw_raw_IR_;
+      } else if (key == 'c') {
+        draw_raw_target_ = !draw_raw_target_;
+      } else if (key == 'v') {
+        draw_components_ = !draw_components_;
+      } else if (key == 'b') {
+        draw_raw_poly_ = !draw_raw_poly_;
+      } else if (key == 'n') {
+        draw_select_blob_ = !draw_select_blob_;
+      } else if (key == 'q') {
+        printf("User requested shutdown.\n");
+        exit(0);
+      }
+      HandleBlobs(imgs_last_, fmt_last_);
+      viewer_->Redraw();
+    };
+  }
+
+  void DrawComponent(const TargetComponent &comp, PixelRef top_color,
+                     PixelRef bot_color, PixelRef in_color,
+                     PixelRef out_color) {
+    overlay_.AddLine(comp.top, comp.inside, top_color);
+    overlay_.AddLine(comp.bottom, comp.outside, bot_color);
+
+    overlay_.AddLine(comp.bottom, comp.inside, in_color);
+    overlay_.AddLine(comp.top, comp.outside, out_color);
+  }
+
+  void DrawTarget(const Target &target) {
+    Vector<2> leftTop = (target.left.top + target.left.inside) * 0.5;
+    Vector<2> rightTop = (target.right.top + target.right.inside) * 0.5;
+    overlay_.AddLine(leftTop, rightTop, {255, 215, 0});
+
+    Vector<2> leftBot = (target.left.bottom + target.left.outside) * 0.5;
+    Vector<2> rightBot = (target.right.bottom + target.right.outside) * 0.5;
+    overlay_.AddLine(leftBot, rightBot, {255, 215, 0});
+
+    overlay_.AddLine(leftTop, leftBot, {255, 215, 0});
+    overlay_.AddLine(rightTop, rightBot, {255, 215, 0});
+  }
+
+  void DrawResult(const IntermediateResult &result, PixelRef color) {
+    Target target =
+        Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+    DrawComponent(target.left, color, color, color, color);
+    DrawComponent(target.right, color, color, color, color);
+  }
+
+  void DrawTarget(const IntermediateResult &result, PixelRef color) {
+    Target target =
+        Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+    Segment<2> leftAx((target.left.top + target.left.inside) * 0.5,
+                      (target.left.bottom + target.left.outside) * 0.5);
+    leftAx.Set(leftAx.A() * 0.9 + leftAx.B() * 0.1,
+               leftAx.B() * 0.9 + leftAx.A() * 0.1);
+    overlay_.AddLine(leftAx, color);
+
+    Segment<2> rightAx((target.right.top + target.right.inside) * 0.5,
+                       (target.right.bottom + target.right.outside) * 0.5);
+    rightAx.Set(rightAx.A() * 0.9 + rightAx.B() * 0.1,
+                rightAx.B() * 0.9 + rightAx.A() * 0.1);
+    overlay_.AddLine(rightAx, color);
+
+    overlay_.AddLine(leftAx.A(), rightAx.A(), color);
+    overlay_.AddLine(leftAx.B(), rightAx.B(), color);
+    Vector<3> p1(0.0, 0.0, 100.0);
+
+    Vector<3> p2 =
+        Rotate(intrinsics().mount_angle, result.extrinsics.r1, 0.0, p1);
+    Vector<2> p3(p2.x(), p2.y());
+    overlay_.AddLine(leftAx.A(), p3 + leftAx.A(), {0, 255, 0});
+    overlay_.AddLine(leftAx.B(), p3 + leftAx.B(), {0, 255, 0});
+    overlay_.AddLine(rightAx.A(), p3 + rightAx.A(), {0, 255, 0});
+    overlay_.AddLine(rightAx.B(), p3 + rightAx.B(), {0, 255, 0});
+
+    overlay_.AddLine(p3 + leftAx.A(), p3 + leftAx.B(), {0, 255, 0});
+    overlay_.AddLine(p3 + leftAx.A(), p3 + rightAx.A(), {0, 255, 0});
+    overlay_.AddLine(p3 + rightAx.A(), p3 + rightAx.B(), {0, 255, 0});
+    overlay_.AddLine(p3 + leftAx.B(), p3 + rightAx.B(), {0, 255, 0});
+  }
+
+  const IntrinsicParams &intrinsics() const { return finder_.intrinsics(); }
+
+ private:
+  // implementation of the filter pipeline.
+  TargetFinder finder_;
+  aos::vision::BlobStreamViewer *viewer_ = nullptr;
+  aos::vision::PixelLinesOverlay overlay_;
+  std::vector<aos::vision::OverlayBase *> overlays_;
+  BlobList imgs_last_;
+  ImageFormat fmt_last_;
+  bool draw_select_blob_ = false;
+  bool draw_raw_poly_ = false;
+  bool draw_components_ = false;
+  bool draw_raw_target_ = false;
+  bool draw_raw_IR_ = false;
+  bool draw_results_ = true;
+};
+
+}  // namespace vision
+}  // namespace y2017
+
+int main(int argc, char **argv) {
+  y2019::vision::FilterHarness filter_harness;
+  aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
+                                  aos::vision::CameraParams());
+}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
new file mode 100644
index 0000000..f69e987
--- /dev/null
+++ b/y2019/vision/target_finder.cc
@@ -0,0 +1,379 @@
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+
+using namespace aos::vision;
+
+namespace y2019 {
+namespace vision {
+
+TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+
+aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
+  const uint8_t threshold_value = GetThresholdValue();
+  return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
+    if (px.g > threshold_value && px.b > threshold_value &&
+        px.r > threshold_value) {
+      return true;
+    }
+    return false;
+  });
+}
+
+// Filter blobs on size.
+void TargetFinder::PreFilter(BlobList *imgs) {
+  imgs->erase(
+      std::remove_if(imgs->begin(), imgs->end(),
+                     [](RangeImage &img) {
+                       // We can drop images with a small number of
+                       // pixels, but images
+                       // must be over 20px or the math will have issues.
+                       return (img.npixels() < 100 || img.height() < 25);
+                     }),
+      imgs->end());
+}
+
+// TODO: Try hierarchical merge for this.
+// Convert blobs into polygons.
+std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
+    const RangeImage &blob, bool verbose) {
+  if (verbose) printf("Process Polygon.\n");
+  alloc_.reset();
+  auto *st = RangeImgToContour(blob, &alloc_);
+
+  struct Pt {
+    float x;
+    float y;
+  };
+  std::vector<Pt> pts;
+
+  // Collect all slopes from the contour.
+  auto opt = st->pt;
+  for (auto *node = st; node->next != st;) {
+    node = node->next;
+
+    auto npt = node->pt;
+
+    pts.push_back(
+        {static_cast<float>(npt.x - opt.x), static_cast<float>(npt.y - opt.y)});
+
+    opt = npt;
+  }
+
+  const int n = pts.size();
+  auto get_pt = [&](int i) { return pts[(i + n * 2) % n]; };
+
+  std::vector<Pt> pts_new = pts;
+  auto run_box_filter = [&](int window_size) {
+    for (size_t i = 0; i < pts.size(); ++i) {
+      Pt a{0.0, 0.0};
+      for (int j = -window_size; j <= window_size; ++j) {
+        Pt p = get_pt(j + i);
+        a.x += p.x;
+        a.y += p.y;
+      }
+      a.x /= (window_size * 2 + 1);
+      a.y /= (window_size * 2 + 1);
+
+      float scale = 1.0 + (i / float(pts.size() * 10));
+      a.x *= scale;
+      a.y *= scale;
+      pts_new[i] = a;
+    }
+    pts = pts_new;
+  };
+  // Three box filter makith a guassian?
+  // Run gaussian filter over the slopes.
+  run_box_filter(2);
+  run_box_filter(2);
+  run_box_filter(2);
+
+  // Heuristic which says if a particular slope is part of a corner.
+  auto is_corner = [&](size_t i) {
+    Pt a = get_pt(i - 3);
+    Pt b = get_pt(i + 3);
+    double dx = (a.x - b.x);
+    double dy = (a.y - b.y);
+    return dx * dx + dy * dy > 0.25;
+  };
+
+  bool prev_v = is_corner(-1);
+
+  // Find all centers of corners.
+  // Because they round, multiple points may be a corner.
+  std::vector<size_t> edges;
+  size_t kBad = pts.size() + 10;
+  size_t prev_up = kBad;
+  size_t wrapped_n = prev_up;
+
+  for (size_t i = 0; i < pts.size(); ++i) {
+    bool v = is_corner(i);
+    if (prev_v && !v) {
+      if (prev_up == kBad) {
+        wrapped_n = i;
+      } else {
+        edges.push_back((prev_up + i - 1) / 2);
+      }
+    }
+    if (v && !prev_v) {
+      prev_up = i;
+    }
+    prev_v = v;
+  }
+
+  if (wrapped_n != kBad) {
+    edges.push_back(((prev_up + pts.size() + wrapped_n - 1) / 2) % pts.size());
+  }
+
+  if (verbose) printf("Edge Count (%zu).\n", edges.size());
+
+  // Get all CountourNodes from the contour.
+  using aos::vision::PixelRef;
+  std::vector<ContourNode *> segments;
+  {
+    std::vector<ContourNode *> segments_all;
+
+    for (ContourNode *node = st; node->next != st;) {
+      node = node->next;
+      segments_all.push_back(node);
+    }
+    for (size_t i : edges) {
+      segments.push_back(segments_all[i]);
+    }
+  }
+  if (verbose) printf("Segment Count (%zu).\n", segments.size());
+
+  // Run best-fits over each line segment.
+  std::vector<Segment<2>> seg_list;
+  if (segments.size() == 4) {
+    for (size_t i = 0; i < segments.size(); ++i) {
+      auto *ed = segments[(i + 1) % segments.size()];
+      auto *st = segments[i];
+      float mx = 0.0;
+      float my = 0.0;
+      int n = 0;
+      for (auto *node = st; node != ed; node = node->next) {
+        mx += node->pt.x;
+        my += node->pt.y;
+        ++n;
+        // (x - [x] / N) ** 2 = [x * x] - 2 * [x] * [x] / N + [x] * [x] / N / N;
+      }
+      mx /= n;
+      my /= n;
+
+      float xx = 0.0;
+      float xy = 0.0;
+      float yy = 0.0;
+      for (auto *node = st; node != ed; node = node->next) {
+        float x = node->pt.x - mx;
+        float y = node->pt.y - my;
+        xx += x * x;
+        xy += x * y;
+        yy += y * y;
+      }
+
+      // TODO: Extract common to hierarchical merge.
+      float neg_b_over_2 = (xx + yy) / 2.0;
+      float c = (xx * yy - xy * xy);
+
+      float sqr = sqrt(neg_b_over_2 * neg_b_over_2 - c);
+
+      {
+        float lam = neg_b_over_2 + sqr;
+        float x = xy;
+        float y = lam - xx;
+
+        float norm = sqrt(x * x + y * y);
+        x /= norm;
+        y /= norm;
+
+        seg_list.push_back(
+            Segment<2>(Vector<2>(mx, my), Vector<2>(mx + x, my + y)));
+      }
+
+      /* Characteristic polynomial
+      1 lam^2 - (xx + yy) lam + (xx * yy - xy * xy) = 0
+
+      [a b]
+      [c d]
+
+      // covariance matrix.
+      [xx xy] [nx]
+      [xy yy] [ny]
+      */
+    }
+  }
+  if (verbose) printf("Poly Count (%zu).\n", seg_list.size());
+  return seg_list;
+}
+
+// Convert segments into target components (left or right)
+std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
+    const std::vector<std::vector<Segment<2>>> &seg_list) {
+  std::vector<TargetComponent> list;
+  TargetComponent new_target;
+  for (const auto &poly : seg_list) {
+    // Reject missized pollygons for now. Maybe rectify them here in the future;
+    if (poly.size() != 4) continue;
+    std::vector<Vector<2>> corners;
+    for (size_t i = 0; i < 4; ++i) {
+      corners.push_back(poly[i].Intersect(poly[(i + 1) % 4]));
+    }
+
+    // Select the closest two points. Short side of the rectangle.
+    double min_dist = -1;
+    std::pair<size_t, size_t> closest;
+    for (size_t i = 0; i < 4; ++i) {
+      size_t next = (i + 1) % 4;
+      double nd = corners[i].SquaredDistanceTo(corners[next]);
+      if (min_dist == -1 || nd < min_dist) {
+        min_dist = nd;
+        closest.first = i;
+        closest.second = next;
+      }
+    }
+
+    // Verify our top is above the bottom.
+    size_t bot_index = closest.first;
+    size_t top_index = (closest.first + 2) % 4;
+    if (corners[top_index].y() < corners[bot_index].y()) {
+      closest.first = top_index;
+      closest.second = (top_index + 1) % 4;
+    }
+
+    // Find the major axis.
+    size_t far_first = (closest.first + 2) % 4;
+    size_t far_second = (closest.second + 2) % 4;
+    Segment<2> major_axis(
+        (corners[closest.first] + corners[closest.second]) * 0.5,
+        (corners[far_first] + corners[far_second]) * 0.5);
+    if (major_axis.AsVector().AngleToZero() > M_PI / 180.0 * 120.0 ||
+        major_axis.AsVector().AngleToZero() < M_PI / 180.0 * 60.0) {
+      // Target is angled way too much, drop it.
+      continue;
+    }
+
+    // organize the top points.
+    Vector<2> topA = corners[closest.first] - major_axis.B();
+    new_target.major_axis = major_axis;
+    if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+      // We have a left target since we are leaning positive.
+      new_target.is_right = false;
+      if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+        // And our A point is left of the major axis.
+        new_target.inside = corners[closest.second];
+        new_target.top = corners[closest.first];
+      } else {
+        // our A point is to the right of the major axis.
+        new_target.inside = corners[closest.first];
+        new_target.top = corners[closest.second];
+      }
+    } else {
+      // We have a right target since we are leaning negative.
+      new_target.is_right = true;
+      if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+        // And our A point is left of the major axis.
+        new_target.inside = corners[closest.first];
+        new_target.top = corners[closest.second];
+      } else {
+        // our A point is to the right of the major axis.
+        new_target.inside = corners[closest.second];
+        new_target.top = corners[closest.first];
+      }
+    }
+
+    // organize the top points.
+    Vector<2> botA = corners[far_first] - major_axis.A();
+    if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+      // We have a right target since we are leaning positive.
+      if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+        // And our A point is left of the major axis.
+        new_target.outside = corners[far_second];
+        new_target.bottom = corners[far_first];
+      } else {
+        // our A point is to the right of the major axis.
+        new_target.outside = corners[far_first];
+        new_target.bottom = corners[far_second];
+      }
+    } else {
+      // We have a left target since we are leaning negative.
+      if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+        // And our A point is left of the major axis.
+        new_target.outside = corners[far_first];
+        new_target.bottom = corners[far_second];
+      } else {
+        // our A point is to the right of the major axis.
+        new_target.outside = corners[far_second];
+        new_target.bottom = corners[far_first];
+      }
+    }
+
+    // This piece of the target should be ready now.
+    list.emplace_back(new_target);
+  }
+
+  return list;
+}
+
+// Match components into targets.
+std::vector<Target> TargetFinder::FindTargetsFromComponents(
+    const std::vector<TargetComponent> component_list, bool verbose) {
+  std::vector<Target> target_list;
+  using namespace aos::vision;
+  if (component_list.size() < 2) {
+    // We don't enough parts for a target.
+    return target_list;
+  }
+
+  for (size_t i = 0; i < component_list.size(); i++) {
+    const TargetComponent &a = component_list[i];
+    for (size_t j = 0; j < i; j++) {
+      bool target_valid = false;
+      Target new_target;
+      const TargetComponent &b = component_list[j];
+
+      // Reject targets that are too far off vertically.
+      Vector<2> a_center = a.major_axis.Center();
+      if (a_center.y() > b.bottom.y() || a_center.y() < b.top.y()) {
+        continue;
+      }
+      Vector<2> b_center = b.major_axis.Center();
+      if (b_center.y() > a.bottom.y() || b_center.y() < a.top.y()) {
+        continue;
+      }
+
+      if (a.is_right && !b.is_right) {
+        if (a.top.x() > b.top.x()) {
+          new_target.right = a;
+          new_target.left = b;
+          target_valid = true;
+        }
+      } else if (!a.is_right && b.is_right) {
+        if (b.top.x() > a.top.x()) {
+          new_target.right = b;
+          new_target.left = a;
+          target_valid = true;
+        }
+      }
+      if (target_valid) {
+        target_list.emplace_back(new_target);
+      }
+    }
+  }
+  if (verbose) printf("Possible Target: %zu.\n", target_list.size());
+  return target_list;
+}
+
+std::vector<IntermediateResult> TargetFinder::FilterResults(
+    const std::vector<IntermediateResult> &results) {
+  std::vector<IntermediateResult> filtered;
+  for (const IntermediateResult &res : results) {
+    if (res.solver_error < 75.0) {
+      filtered.emplace_back(res);
+    }
+  }
+  return filtered;
+}
+
+}  // namespace vision
+}  // namespace y2019
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
new file mode 100644
index 0000000..633733a
--- /dev/null
+++ b/y2019/vision/target_finder.h
@@ -0,0 +1,80 @@
+#ifndef _Y2019_VISION_TARGET_FINDER_H_
+#define _Y2019_VISION_TARGET_FINDER_H_
+
+#include "aos/vision/blob/region_alloc.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/vector.h"
+#include "y2019/vision/target_types.h"
+
+namespace y2019 {
+namespace vision {
+
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+
+class TargetFinder {
+ public:
+  TargetFinder();
+  // Turn a raw image into blob range image.
+  aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
+
+  // Value against which we threshold.
+  uint8_t GetThresholdValue() { return 120; }
+
+  // filter out obvious or durranged blobs.
+  void PreFilter(BlobList *imgs);
+
+  // Turn a blob into a polgygon.
+  std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+                                                   bool verbose);
+
+  // Turn a bloblist into components of a target.
+  std::vector<TargetComponent> FillTargetComponentList(
+      const std::vector<std::vector<aos::vision::Segment<2>>> &seg_list);
+
+  // Piece the compenents together into a target.
+  std::vector<Target> FindTargetsFromComponents(
+      const std::vector<TargetComponent> component_list, bool verbose);
+
+  // Given a target solve for the transformation of the template target.
+  IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
+
+  std::vector<IntermediateResult> FilterResults(
+      const std::vector<IntermediateResult> &results);
+
+  // Get the local overlay for debug if we are doing that.
+  aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+
+  // Convert target location into meters and radians.
+  void GetAngleDist(const aos::vision::Vector<2> &target, double down_angle,
+                    double *dist, double *angle);
+
+  // Return the template target in a normalized space.
+  const Target &GetTemplateTarget() { return target_template_; }
+
+  const IntrinsicParams &intrinsics() const { return intrinsics_; }
+
+ private:
+  // Find a loosly connected target.
+  double DetectConnectedTarget(const RangeImage &img);
+
+  aos::vision::PixelLinesOverlay overlay_;
+
+  aos::vision::AnalysisAllocator alloc_;
+
+  // The template for the default target in the standard space.
+  Target target_template_;
+
+  IntrinsicParams intrinsics_;
+
+  ExtrinsicParams default_extrinsics_;
+};
+
+}  // namespace vision
+}  // namespace y2019
+
+#endif
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
new file mode 100644
index 0000000..77adc83
--- /dev/null
+++ b/y2019/vision/target_geometry.cc
@@ -0,0 +1,186 @@
+#include "y2019/vision/target_finder.h"
+
+#include "ceres/ceres.h"
+
+#include <math.h>
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+namespace y2019 {
+namespace vision {
+
+static constexpr double kInchesToMeters = 0.0254;
+
+using namespace aos::vision;
+using aos::vision::Vector;
+
+Target Target::MakeTemplate() {
+  Target out;
+  // This is how off-vertical the tape is.
+  const double theta = 14.5 * M_PI / 180.0;
+
+  const double tape_offset = 4 * kInchesToMeters;
+  const double tape_width = 2 * kInchesToMeters;
+  const double tape_length = 5.5 * kInchesToMeters;
+
+  const double s = sin(theta);
+  const double c = cos(theta);
+  out.right.top = Vector<2>(tape_offset, 0.0);
+  out.right.inside = Vector<2>(tape_offset + tape_width * c, tape_width * s);
+  out.right.bottom = Vector<2>(tape_offset + tape_width * c + tape_length * s,
+                               tape_width * s - tape_length * c);
+  out.right.outside =
+      Vector<2>(tape_offset + tape_length * s, -tape_length * c);
+
+  out.right.is_right = true;
+  out.left.top = Vector<2>(-out.right.top.x(), out.right.top.y());
+  out.left.inside = Vector<2>(-out.right.inside.x(), out.right.inside.y());
+  out.left.bottom = Vector<2>(-out.right.bottom.x(), out.right.bottom.y());
+  out.left.outside = Vector<2>(-out.right.outside.x(), out.right.outside.y());
+  return out;
+}
+
+std::array<Vector<2>, 8> Target::toPointList() const {
+  return std::array<Vector<2>, 8>{{right.top, right.inside, right.bottom,
+                                   right.outside, left.top, left.inside,
+                                   left.bottom, left.outside}};
+}
+
+Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
+                  const ExtrinsicParams &extrinsics) {
+  double y = extrinsics.y;
+  double z = extrinsics.z;
+  double r1 = extrinsics.r1;
+  double r2 = extrinsics.r2;
+  double rup = intrinsics.mount_angle;
+  double fl = intrinsics.focal_length;
+
+  ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
+
+  {
+    double theta = r1;
+    double s = sin(theta);
+    double c = cos(theta);
+    pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+           c).finished() *
+          pts.transpose();
+  }
+
+  pts(2) += z;
+
+  {
+    double theta = r2;
+    double s = sin(theta);
+    double c = cos(theta);
+    pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+           c).finished() *
+          pts.transpose();
+  }
+
+  // TODO: Apply 15 degree downward rotation.
+  {
+    double theta = rup;
+    double s = sin(theta);
+    double c = cos(theta);
+
+    pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
+           c).finished() *
+          pts.transpose();
+  }
+
+  // TODO: Final image projection.
+  ::Eigen::Matrix<double, 1, 3> res = pts;
+
+  float scale = fl / res.z();
+  return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
+}
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+               const ExtrinsicParams &extrinsics) {
+  auto project = [&](Vector<2> pt) {
+    return Project(pt, intrinsics, extrinsics);
+  };
+  Target new_targ;
+  new_targ.right.is_right = true;
+  new_targ.right.top = project(target.right.top);
+  new_targ.right.inside = project(target.right.inside);
+  new_targ.right.bottom = project(target.right.bottom);
+  new_targ.right.outside = project(target.right.outside);
+
+  new_targ.left.top = project(target.left.top);
+  new_targ.left.inside = project(target.left.inside);
+  new_targ.left.bottom = project(target.left.bottom);
+  new_targ.left.outside = project(target.left.outside);
+
+  return new_targ;
+}
+
+// Used at runtime on a single image given camera parameters.
+struct RuntimeCostFunctor {
+  RuntimeCostFunctor(Vector<2> result, Vector<2> template_pt,
+                     IntrinsicParams intrinsics)
+      : result(result), template_pt(template_pt), intrinsics(intrinsics) {}
+
+  bool operator()(const double *const x, double *residual) const {
+    auto extrinsics = ExtrinsicParams::get(x);
+    auto pt = result - Project(template_pt, intrinsics, extrinsics);
+    residual[0] = pt.x();
+    residual[1] = pt.y();
+    return true;
+  }
+
+  Vector<2> result;
+  Vector<2> template_pt;
+  IntrinsicParams intrinsics;
+};
+
+IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
+                                                       bool verbose) {
+  // Memory for the ceres solver.
+  double params[ExtrinsicParams::kNumParams];
+  default_extrinsics_.set(&params[0]);
+
+  Problem problem;
+
+  auto target_value = target.toPointList();
+  auto template_value = target_template_.toPointList();
+
+  for (size_t i = 0; i < 8; ++i) {
+    auto a = template_value[i];
+    auto b = target_value[i];
+
+    problem.AddResidualBlock(
+        new NumericDiffCostFunction<RuntimeCostFunctor, CENTRAL, 2, 4>(
+            new RuntimeCostFunctor(b, a, intrinsics_)),
+        NULL, &params[0]);
+  }
+
+  Solver::Options options;
+  options.minimizer_progress_to_stdout = false;
+  Solver::Summary summary;
+  Solve(options, &problem, &summary);
+
+  IntermediateResult IR;
+  IR.extrinsics = ExtrinsicParams::get(&params[0]);
+  IR.solver_error = summary.final_cost;
+
+  if (verbose) {
+    std::cout << summary.BriefReport() << "\n";
+    std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
+    std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
+    std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
+    std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
+    std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+    std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+    std::cout << "error = " << summary.final_cost << ";\n";
+  }
+  return IR;
+}
+
+}  // namespace vision
+}  // namespace y2019
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
new file mode 100644
index 0000000..0d53e2c
--- /dev/null
+++ b/y2019/vision/target_sender.cc
@@ -0,0 +1,136 @@
+#include <fstream>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/reader.h"
+
+#include "y2019/jevois/serial.h"
+#include "y2019/vision/target_finder.h"
+
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using ::y2019::jevois::open_via_terminos;
+using aos::vision::Segment;
+
+class CameraStream : public ::aos::vision::ImageStreamEvent {
+ public:
+  CameraStream(::aos::vision::CameraParams params, const ::std::string &fname)
+      : ImageStreamEvent(fname, params) {}
+
+  void ProcessImage(DataRef data, monotonic_clock::time_point monotonic_now) {
+    LOG(INFO, "got frame: %d\n", (int)data.size());
+
+    static unsigned i = 0;
+
+    /*
+          std::ofstream ofs(std::string("/jevois/data/debug_viewer_jpeg_") +
+                                std::to_string(i) + ".yuyv",
+                            std::ofstream::out);
+          ofs << data;
+          ofs.close();
+          */
+    if (on_frame) on_frame(data, monotonic_now);
+    ++i;
+
+    if (i == 200) exit(-1);
+  }
+
+  std::function<void(DataRef, monotonic_clock::time_point)> on_frame;
+};
+
+int open_terminos(const char *tty_name) { return open_via_terminos(tty_name); }
+
+std::string GetFileContents(const std::string &filename) {
+  std::ifstream in(filename, std::ios::in | std::ios::binary);
+  if (in) {
+    std::string contents;
+    in.seekg(0, std::ios::end);
+    contents.resize(in.tellg());
+    in.seekg(0, std::ios::beg);
+    in.read(&contents[0], contents.size());
+    in.close();
+    return (contents);
+  }
+  fprintf(stderr, "Could not read file: %s\n", filename.c_str());
+  exit(-1);
+}
+
+int main(int argc, char **argv) {
+  (void)argc;
+  (void)argv;
+  using namespace y2019::vision;
+  // gflags::ParseCommandLineFlags(&argc, &argv, false);
+  ::aos::logging::Init();
+  ::aos::logging::AddImplementation(
+      new ::aos::logging::StreamLogImplementation(stderr));
+
+  int itsDev = open_terminos("/dev/ttyS0");
+  dup2(itsDev, 1);
+  dup2(itsDev, 2);
+
+  TargetFinder finder_;
+
+  // Check that our current results match possible solutions.
+  aos::vision::CameraParams params0;
+  params0.set_exposure(0);
+  params0.set_brightness(40);
+  params0.set_width(640);
+  // params0.set_fps(10);
+  params0.set_height(480);
+
+  ::std::unique_ptr<CameraStream> camera0(
+      new CameraStream(params0, "/dev/video0"));
+  camera0->on_frame = [&](DataRef data,
+                          monotonic_clock::time_point /*monotonic_now*/) {
+    aos::vision::ImageFormat fmt{640, 480};
+    aos::vision::BlobList imgs = aos::vision::FindBlobs(
+        aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
+    finder_.PreFilter(&imgs);
+
+    bool verbose = false;
+    std::vector<std::vector<Segment<2>>> raw_polys;
+    for (const RangeImage &blob : imgs) {
+      std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+      if (polygon.empty()) {
+      } else {
+        raw_polys.push_back(polygon);
+      }
+    }
+
+    // Calculate each component side of a possible target.
+    std::vector<TargetComponent> target_component_list =
+        finder_.FillTargetComponentList(raw_polys);
+
+    // Put the compenents together into targets.
+    std::vector<Target> target_list =
+        finder_.FindTargetsFromComponents(target_component_list, verbose);
+
+    // Use the solver to generate an intermediate version of our results.
+    std::vector<IntermediateResult> results;
+    for (const Target &target : target_list) {
+      results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
+    }
+
+    results = finder_.FilterResults(results);
+  };
+
+  aos::events::EpollLoop loop;
+
+  for (int i = 0; i < 100; ++i) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    camera0->ReadEvent();
+  }
+
+  // TODO: Fix event loop on jevois:
+  // loop.Add(camera0.get());
+  // loop.Run();
+}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
new file mode 100644
index 0000000..557b04a
--- /dev/null
+++ b/y2019/vision/target_types.h
@@ -0,0 +1,128 @@
+#ifndef _Y2019_VISION_TARGET_TYPES_H_
+#define _Y2019_VISION_TARGET_TYPES_H_
+
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace y2019 {
+namespace vision {
+
+// This polynomial exists in transpose space.
+struct TargetComponent {
+  aos::vision::Vector<2> GetByIndex(size_t i) const {
+    switch (i) {
+      case 0:
+        return top;
+      case 1:
+        return inside;
+      case 2:
+        return bottom;
+      case 3:
+        return outside;
+      default:
+        return aos::vision::Vector<2>();
+    }
+  }
+  bool is_right;
+  aos::vision::Vector<2> top;
+  aos::vision::Vector<2> inside;
+  aos::vision::Vector<2> outside;
+  aos::vision::Vector<2> bottom;
+
+  aos::vision::Segment<2> major_axis;
+};
+
+// Convert back to screen space for final result.
+struct Target {
+  TargetComponent left;
+  TargetComponent right;
+
+  static Target MakeTemplate();
+  // Get the points in some order (will match against the template).
+  std::array<aos::vision::Vector<2>, 8> toPointList() const;
+};
+
+struct IntrinsicParams {
+  static constexpr size_t kNumParams = 2;
+
+  double mount_angle = 10.0481 / 180.0 * M_PI;
+  double focal_length = 729.445;
+
+  void set(double *data) {
+    data[0] = mount_angle;
+    data[1] = focal_length;
+  }
+  static IntrinsicParams get(const double *data) {
+    IntrinsicParams out;
+    out.mount_angle = data[0];
+    out.focal_length = data[1];
+    return out;
+  }
+};
+
+struct ExtrinsicParams {
+  static constexpr size_t kNumParams = 4;
+
+  double y = 18.0 * 0.0254;
+  double z = 23.0 * 0.0254;
+  double r1 = 1.0 / 180 * M_PI;
+  double r2 = -1.0 / 180 * M_PI;
+
+  void set(double *data) {
+    data[0] = y;
+    data[1] = z;
+    data[2] = r1;
+    data[3] = r2;
+  }
+  static ExtrinsicParams get(const double *data) {
+    ExtrinsicParams out;
+    out.y = data[0];
+    out.z = data[1];
+    out.r1 = data[2];
+    out.r2 = data[3];
+    return out;
+  }
+};
+// Projects a point from idealized template space to camera space.
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+                               const IntrinsicParams &intrinsics,
+                               const ExtrinsicParams &extrinsics);
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+               const ExtrinsicParams &extrinsics);
+
+// An intermediate for of the results.
+// These are actual awnsers, but in a form from the solver that is not ready for
+// the wire.
+struct IntermediateResult {
+  ExtrinsicParams extrinsics;
+
+  // Error from solver calulations.
+  double solver_error;
+};
+
+// Final foramtting ready for output on the wire.
+struct TargetResult {
+  // Distance to the target in meters. Specifically, the distance from the
+  // center of the camera's image plane to the center of the target.
+  float distance;
+  // Height of the target in meters. Specifically, the distance from the floor
+  // to the center of the target.
+  float height;
+
+  // Heading of the center of the target in radians. Zero is straight out
+  // perpendicular to the camera's image plane. Images to the left (looking at a
+  // camera image) are at a positive angle.
+  float heading;
+
+  // The angle between the target and the camera's image plane. This is
+  // projected so both are assumed to be perpendicular to the floor. Parallel
+  // targets have a skew of zero. Targets rotated such that their left edge
+  // (looking at a camera image) is closer are at a positive angle.
+  float skew;
+};
+
+}  // namespace vision
+}  // namespace y2019
+
+#endif