diff --git a/WORKSPACE b/WORKSPACE
index d76d359..913d969 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -1562,6 +1562,20 @@
     url = "https://software.frc971.org/Build-Dependencies/2023_arducam_apriltag_test_images.tar.gz",
 )
 
+http_file(
+    name = "orin_image_apriltag",
+    downloaded_file_path = "orin_image_apriltag.bfbs",
+    sha256 = "c86604fd0b1301b301e299b1bba2573af8c586413934a386a2bd28fd9b037b84",
+    url = "https://software.frc971.org/Build-Dependencies/orin_image_apriltag.bfbs",
+)
+
+http_file(
+    name = "orin_large_image_apriltag",
+    downloaded_file_path = "orin_large_gs_apriltag.bfbs",
+    sha256 = "d933adac0d6c205c574791060be73701ead05977ff5dd9f6f4eadb45817c3ccb",
+    url = "https://software.frc971.org/Build-Dependencies/orin_large_gs_apriltag.bfbs",
+)
+
 http_archive(
     name = "libedgetpu",
     build_file = "//third_party:libedgetpu/libedgetpu.BUILD",
diff --git a/frc971/orin/BUILD b/frc971/orin/BUILD
index 61d9b8e..6075a22 100644
--- a/frc971/orin/BUILD
+++ b/frc971/orin/BUILD
@@ -17,11 +17,13 @@
 )
 
 cc_library(
-    name = "cuda",
+    name = "apriltag",
     srcs = [
         "apriltag.cc",
+        "apriltag_detect.cc",
         "cuda.cc",
         "labeling_allegretti_2019_BKE.cc",
+        "line_fit_filter.cc",
         "points.cc",
         "threshold.cc",
     ],
@@ -30,6 +32,7 @@
         "cuda.h",
         "gpu_image.h",
         "labeling_allegretti_2019_BKE.h",
+        "line_fit_filter.h",
         "points.h",
         "threshold.h",
         "transform_output_iterator.h",
@@ -42,6 +45,7 @@
     features = ["cuda"],
     deps = [
         "//aos/time",
+        "//third_party:cudart",
         "//third_party/apriltag",
         "@com_github_google_glog//:glog",
         "@com_github_nvidia_cccl//:cccl",
@@ -56,10 +60,12 @@
     ],
     data = [
         "@apriltag_test_bfbs_images",
+        "@orin_image_apriltag//file",
+        "@orin_large_image_apriltag//file",
     ],
     features = ["cuda"],
     deps = [
-        ":cuda",
+        ":apriltag",
         ":ycbcr",
         "//aos:flatbuffer_merge",
         "//aos:json_to_flatbuffer",
@@ -67,7 +73,6 @@
         "//aos/testing:path",
         "//aos/testing:random_seed",
         "//frc971/vision:vision_fbs",
-        "//third_party:cudart",
         "//third_party:opencv",
         "//third_party/apriltag",
         "//y2023/vision:ThresholdHalide",
@@ -82,10 +87,9 @@
     ],
     features = ["cuda"],
     deps = [
-        ":cuda",
+        ":apriltag",
         "//aos/testing:googletest",
         "//aos/testing:random_seed",
-        "//third_party:cudart",
     ],
 )
 
@@ -96,10 +100,9 @@
     ],
     features = ["cuda"],
     deps = [
-        ":cuda",
+        ":apriltag",
         "//aos/testing:googletest",
         "//aos/testing:random_seed",
-        "//third_party:cudart",
     ],
 )
 
diff --git a/frc971/orin/apriltag.cc b/frc971/orin/apriltag.cc
index ff8ccec..263543d 100644
--- a/frc971/orin/apriltag.cc
+++ b/frc971/orin/apriltag.cc
@@ -15,6 +15,7 @@
 #include <vector>
 
 #include "glog/logging.h"
+#include "third_party/apriltag/common/g2d.h"
 
 #include "aos/time/time.h"
 #include "frc971/orin/labeling_allegretti_2019_BKE.h"
@@ -23,6 +24,7 @@
 
 namespace frc971 {
 namespace apriltag {
+namespace {
 
 typedef std::chrono::duration<float, std::milli> float_milli;
 
@@ -98,12 +100,26 @@
   return temp_storage_bytes;
 }
 
+// Computes and returns the scratch space needed for DeviceScan::InclusiveScan
+// of the provided value V with the provided number of elements.
+template <typename K, typename V>
+static size_t DeviceScanInclusiveScanByKeyScratchSpace(size_t elements) {
+  size_t temp_storage_bytes = 0;
+  CHECK_CUDA(cub::DeviceScan::InclusiveScanByKey(
+      nullptr, temp_storage_bytes, (K *)(nullptr), (V *)(nullptr),
+      (V *)(nullptr), CustomFirst<V>(), elements));
+  return temp_storage_bytes;
+}
+
+}  // namespace
+
 GpuDetector::GpuDetector(size_t width, size_t height,
-                         const apriltag_detector_t *tag_detector)
+                         apriltag_detector_t *tag_detector)
     : width_(width),
       height_(height),
       tag_detector_(tag_detector),
       color_image_host_(width * height * 2),
+      gray_image_host_(width * height),
       color_image_device_(width * height * 2),
       gray_image_device_(width * height),
       decimated_image_device_(width / 2 * height / 2),
@@ -116,10 +132,17 @@
       compressed_union_marker_pair_device_(union_marker_pair_device_.size()),
       sorted_union_marker_pair_device_(union_marker_pair_device_.size()),
       extents_device_(union_marker_pair_device_.size()),
-      reduced_dot_blobs_pair_device_(kMaxBlobs),
       selected_extents_device_(kMaxBlobs),
       selected_blobs_device_(union_marker_pair_device_.size()),
       sorted_selected_blobs_device_(selected_blobs_device_.size()),
+      line_fit_points_device_(selected_blobs_device_.size()),
+      errs_device_(line_fit_points_device_.size()),
+      filtered_errs_device_(line_fit_points_device_.size()),
+      filtered_is_local_peak_device_(line_fit_points_device_.size()),
+      compressed_peaks_device_(line_fit_points_device_.size()),
+      sorted_compressed_peaks_device_(line_fit_points_device_.size()),
+      peak_extents_device_(kMaxBlobs),
+      fit_quads_device_(kMaxBlobs),
       radix_sort_tmpstorage_device_(RadixSortScratchSpace<QuadBoundaryPoint>(
           sorted_union_marker_pair_device_.size())),
       temp_storage_compressed_union_marker_pair_device_(
@@ -138,7 +161,13 @@
               num_selected_blobs_device_.get())),
       temp_storage_selected_extents_scan_device_(
           DeviceScanInclusiveScanScratchSpace<
-              cub::KeyValuePair<long, MinMaxExtents>>(kMaxBlobs)) {
+              cub::KeyValuePair<long, MinMaxExtents>>(kMaxBlobs)),
+      temp_storage_line_fit_scan_device_(
+          DeviceScanInclusiveScanByKeyScratchSpace<uint32_t, LineFitPoint>(
+              sorted_selected_blobs_device_.size())) {
+  fit_quads_host_.reserve(kMaxBlobs);
+  quad_corners_host_.reserve(kMaxBlobs);
+
   CHECK_EQ(tag_detector_->quad_decimate, 2);
   CHECK(!tag_detector_->qtp.deglitch);
 
@@ -155,9 +184,27 @@
   if (min_tag_width_ < 3) {
     min_tag_width_ = 3;
   }
+
+  poly0_ = g2d_polygon_create_zeros(4);
+  poly1_ = g2d_polygon_create_zeros(4);
+
+  detections_ = zarray_create(sizeof(apriltag_detection_t *));
+  zarray_ensure_capacity(detections_, kMaxBlobs);
 }
 
-GpuDetector::~GpuDetector() {}
+GpuDetector::~GpuDetector() {
+  for (int i = 0; i < zarray_size(detections_); ++i) {
+    apriltag_detection_t *det;
+    zarray_get(detections_, i, &det);
+    apriltag_detection_destroy(det);
+  }
+
+  zarray_destroy(detections_);
+  zarray_destroy(poly1_);
+  zarray_destroy(poly0_);
+}
+
+namespace {
 
 // Computes a massive image of 4x QuadBoundaryPoint per pixel with a
 // QuadBoundaryPoint for each pixel pair which crosses a blob boundary.
@@ -306,6 +353,14 @@
   }
 };
 
+// Masks out just the blob ID pair, rep01.
+struct MaskBlobIndex {
+  __host__ __device__ __forceinline__ uint32_t
+  operator()(const IndexPoint &a) const {
+    return a.blob_index();
+  }
+};
+
 // Rewrites a QuadBoundaryPoint to an IndexPoint, adding the angle to the
 // center.
 class RewriteToIndexPoint {
@@ -354,6 +409,11 @@
     result.min_x = result.max_x = pt.value.x();
     result.starting_offset = pt.key;
     result.count = 1;
+    result.pxgx_plus_pygy_sum =
+        static_cast<int64_t>(pt.value.x()) * pt.value.gx() +
+        static_cast<int64_t>(pt.value.y()) * pt.value.gy();
+    result.gx_sum = pt.value.gx();
+    result.gy_sum = pt.value.gy();
     return result;
   }
 };
@@ -372,6 +432,9 @@
     result.starting_offset = std::min(a.starting_offset, b.starting_offset);
     // We want to count everything.
     result.count = a.count + b.count;
+    result.pxgx_plus_pygy_sum = a.pxgx_plus_pygy_sum + b.pxgx_plus_pygy_sum;
+    result.gx_sum = a.gx_sum + b.gx_sum;
+    result.gy_sum = a.gy_sum + b.gy_sum;
     return result;
   }
 };
@@ -426,15 +489,29 @@
   size_t max_cluster_pixels_;
 };
 
+class NonzeroBlobs {
+ public:
+  __host__ __device__
+  NonzeroBlobs(const cub::KeyValuePair<long, MinMaxExtents> *extents_device)
+      : extents_device_(extents_device) {}
+
+  __host__ __device__ __forceinline__ bool operator()(
+      const IndexPoint &a) const {
+    return extents_device_[a.blob_index()].value.count > 0;
+  }
+
+ private:
+  const cub::KeyValuePair<long, MinMaxExtents> *extents_device_;
+};
+
 // Selects blobs which are big enough, not too big, and have the right color in
 // the middle.
 class SelectBlobs {
  public:
-  SelectBlobs(const MinMaxExtents *extents_device, const float *dot_products,
-              size_t tag_width, bool reversed_border, bool normal_border,
+  SelectBlobs(const MinMaxExtents *extents_device, size_t tag_width,
+              bool reversed_border, bool normal_border,
               size_t min_cluster_pixels, size_t max_cluster_pixels)
       : extents_device_(extents_device),
-        dot_products_(dot_products),
         tag_width_(tag_width),
         reversed_border_(reversed_border),
         normal_border_(normal_border),
@@ -444,7 +521,7 @@
   // Returns true if the blob passes the size and dot product checks and is
   // worth further consideration.
   __host__ __device__ __forceinline__ bool operator()(
-      const size_t blob_index, MinMaxExtents extents) const {
+      MinMaxExtents extents) const {
     if (extents.count < min_cluster_pixels_) {
       return false;
     }
@@ -459,7 +536,7 @@
     }
 
     // And the right side must be inside.
-    const bool quad_reversed_border = dot_products_[blob_index] < 0;
+    const bool quad_reversed_border = extents.dot() < 0.0;
     if (!reversed_border_ && quad_reversed_border) {
       return false;
     }
@@ -472,13 +549,12 @@
 
   __host__ __device__ __forceinline__ bool operator()(
       const IndexPoint &a) const {
-    bool result = (*this)(a.blob_index(), extents_device_[a.blob_index()]);
+    bool result = (*this)(extents_device_[a.blob_index()]);
 
     return result;
   }
 
   const MinMaxExtents *extents_device_;
-  const float *dot_products_;
   size_t tag_width_;
 
   bool reversed_border_;
@@ -492,16 +568,15 @@
 // update zero sized blobs.
 struct TransformZeroFilteredBlobSizes {
  public:
-  TransformZeroFilteredBlobSizes(const float *dot_products, size_t tag_width,
-                                 bool reversed_border, bool normal_border,
-                                 size_t min_cluster_pixels,
+  TransformZeroFilteredBlobSizes(size_t tag_width, bool reversed_border,
+                                 bool normal_border, size_t min_cluster_pixels,
                                  size_t max_cluster_pixels)
-      : select_blobs_(nullptr, dot_products, tag_width, reversed_border,
-                      normal_border, min_cluster_pixels, max_cluster_pixels) {}
+      : select_blobs_(nullptr, tag_width, reversed_border, normal_border,
+                      min_cluster_pixels, max_cluster_pixels) {}
 
   __host__ __device__ __forceinline__ cub::KeyValuePair<long, MinMaxExtents>
   operator()(cub::KeyValuePair<long, MinMaxExtents> pt) const {
-    pt.value.count *= select_blobs_(pt.key, pt.value);
+    pt.value.count *= select_blobs_(pt.value);
     pt.value.starting_offset = 0;
     return pt;
   }
@@ -534,13 +609,112 @@
       result.key = a.key;
     }
 
-    result.value.starting_offset = b.value.starting_offset + b.value.count;
+    result.value.starting_offset = a.value.starting_offset +
+                                   b.value.starting_offset + a.value.count +
+                                   b.value.count - result.value.count;
+
     return result;
   }
 };
 
+struct TransformLineFitPoint {
+  __host__ __device__ __forceinline__ LineFitPoint
+  operator()(IndexPoint p) const {
+    LineFitPoint result;
+
+    // we now undo our fixed-point arithmetic.
+    // adjust for pixel center bias
+    constexpr int delta = 1;
+    int32_t ix2 = p.x() + delta;
+    int32_t iy2 = p.y() + delta;
+    int32_t ix = ix2 / 2;
+    int32_t iy = iy2 / 2;
+
+    int32_t W = 1;
+
+    if (ix > 0 && ix + 1 < decimated_width && iy > 0 &&
+        iy + 1 < decimated_height) {
+      int32_t grad_x = decimated_image_device_[iy * decimated_width + ix + 1] -
+                       decimated_image_device_[iy * decimated_width + ix - 1];
+
+      int32_t grad_y =
+          decimated_image_device_[(iy + 1) * decimated_width + ix] -
+          decimated_image_device_[(iy - 1) * decimated_width + ix];
+
+      // XXX Tunable. How to shape the gradient magnitude?
+      W = hypotf(grad_x, grad_y) + 1;
+    }
+
+    result.Mx = W * ix2;
+    result.My = W * iy2;
+    result.Mxx = W * ix2 * ix2;
+    result.Mxy = W * ix2 * iy2;
+    result.Myy = W * iy2 * iy2;
+    result.W = W;
+    result.blob_index = p.blob_index();
+    return result;
+  }
+
+  const uint8_t *decimated_image_device_;
+  int decimated_width;
+  int decimated_height;
+};
+
+struct SumLineFitPoints {
+  __host__ __device__ __forceinline__ LineFitPoint
+  operator()(const LineFitPoint &a, const LineFitPoint &b) const {
+    LineFitPoint result;
+    result.Mx = a.Mx + b.Mx;
+    result.My = a.My + b.My;
+    result.Mxx = a.Mxx + b.Mxx;
+    result.Mxy = a.Mxy + b.Mxy;
+    result.Myy = a.Myy + b.Myy;
+    result.W = a.W + b.W;
+    result.blob_index = a.blob_index;
+    return result;
+  }
+};
+
+struct ValidPeaks {
+  __host__ __device__ __forceinline__ bool operator()(const Peak &a) const {
+    return a.blob_index != Peak::kNoPeak();
+  }
+};
+
+struct TransformToPeakExtents {
+  __host__ __device__ __forceinline__ PeakExtents
+  operator()(const cub::KeyValuePair<long, Peak> &a) const {
+    PeakExtents result;
+    result.blob_index = a.value.blob_index;
+    result.starting_offset = a.key;
+    result.count = 1;
+    return result;
+  }
+};
+
+struct MaskPeakExtentsByBlobId {
+  __host__ __device__ __forceinline__ uint32_t operator()(const Peak &a) const {
+    return a.blob_index;
+  }
+};
+
+struct MergePeakExtents {
+  __host__ __device__ __forceinline__ PeakExtents
+  operator()(const PeakExtents &a, const PeakExtents &b) const {
+    PeakExtents result;
+    result.blob_index = a.blob_index;
+    result.starting_offset = std::min(a.starting_offset, b.starting_offset);
+    result.count = a.count + b.count;
+    return result;
+  }
+};
+
+}  // namespace
+
 void GpuDetector::Detect(const uint8_t *image) {
   color_image_host_.MemcpyFrom(image);
+  const aos::monotonic_clock::time_point start_time =
+      aos::monotonic_clock::now();
   start_.Record(&stream_);
   color_image_device_.MemcpyAsyncFrom(&color_image_host_, &stream_);
   after_image_memcpy_to_device_.Record(&stream_);
@@ -553,6 +727,10 @@
       height_, tag_detector_->qtp.min_white_black_diff, &stream_);
   after_threshold_.Record(&stream_);
 
+  gray_image_device_.MemcpyAsyncTo(&gray_image_host_, &stream_);
+
+  after_memcpy_gray_.Record(&stream_);
+
   union_markers_size_device_.MemsetAsync(0u, &stream_);
   after_memset_.Record(&stream_);
 
@@ -589,7 +767,7 @@
         thresholded_image_device_.get(), union_markers_device_.get(),
         union_markers_size_device_.get(), union_marker_pair_device_.get(),
         decimated_width, decimated_height);
-    MaybeCheckAndSynchronize();
+    MaybeCheckAndSynchronize("BlobDiff");
   }
 
   // TODO(austin): Can I do the first step of the zero removal in the BlobDiff
@@ -610,7 +788,7 @@
         num_compressed_union_marker_pair_device_.get(),
         union_marker_pair_device_.size(), nz, stream_.get()));
 
-    MaybeCheckAndSynchronize();
+    MaybeCheckAndSynchronize("cub::DeviceSelect::If");
   }
 
   after_compact_.Record(&stream_);
@@ -633,14 +811,14 @@
         QuadBoundaryPoint::kRepEndBit, QuadBoundaryPoint::kBitsInKey,
         stream_.get()));
 
-    MaybeCheckAndSynchronize();
+    MaybeCheckAndSynchronize("cub::DeviceRadixSort::SortKeys");
   }
 
   after_sort_.Record(&stream_);
 
   size_t num_quads_host = 0;
   {
-    // Our next step is to compute the extents of each blob so we can filter
+    // Our next step is to compute the extents and dot product so we can filter
     // blobs.
     cub::ArgIndexInputIterator<QuadBoundaryPoint *> value_index_input_iterator(
         sorted_union_marker_pair_device_.get());
@@ -672,7 +850,6 @@
 
     num_quads_device_.MemcpyTo(&num_quads_host);
   }
-  CHECK_LE(num_quads_host, reduced_dot_blobs_pair_device_.size());
 
   // Longest april tag will be the full perimeter of the image.  Each point
   // results in 2 neighbor points, 1 straight, and one at 45 degrees.  But, we
@@ -684,46 +861,6 @@
   const size_t max_april_tag_perimeter = 2 * (width_ + height_);
 
   {
-    // Now that we have the extents, compute the dot product used to detect
-    // which blobs are inside out or right side out.
-
-    // Don't care about these quantities since the previous ReduceByKey will
-    // ahve computed them too.
-    cub::DiscardOutputIterator<size_t> length_discard_iterator;
-    cub::DiscardOutputIterator<uint64_t> key_discard_iterator;
-
-    size_t dot_reduce_temp_storage_bytes =
-        temp_storage_dot_product_device_.size();
-
-    // Provide a mask to detect keys by rep01()
-    MaskRep01 mask;
-    cub::TransformInputIterator<uint64_t, MaskRep01, QuadBoundaryPoint *>
-        key_iterator(sorted_union_marker_pair_device_.get(), mask);
-
-    // Create a transform iterator which calculates the dot product for each
-    // point individually.
-    cub::ArgIndexInputIterator<QuadBoundaryPoint *> value_index_input_iterator(
-        sorted_union_marker_pair_device_.get());
-    ComputeDotProductTransform dot_operator(
-        extents_device_.get(), num_quads_host, min_tag_width_,
-        tag_detector_->qtp.min_cluster_pixels, max_april_tag_perimeter);
-    cub::TransformInputIterator<float, ComputeDotProductTransform,
-                                cub::ArgIndexInputIterator<QuadBoundaryPoint *>>
-        value_iterator(value_index_input_iterator, dot_operator);
-
-    // And now sum per key.
-    CHECK_CUDA(cub::DeviceReduce::ReduceByKey(
-        temp_storage_dot_product_device_.get(), dot_reduce_temp_storage_bytes,
-        key_iterator, key_discard_iterator, value_iterator,
-        reduced_dot_blobs_pair_device_.get(), length_discard_iterator,
-        cub::Sum(), num_compressed_union_marker_pair_host, stream_.get()));
-
-    MaybeCheckAndSynchronize();
-  }
-
-  after_dot_.Record(&stream_);
-
-  {
     // Now that we have the dot products, we need to rewrite the extents for the
     // post-thresholded world so we can find the start and end address of blobs
     // for fitting lines.
@@ -733,9 +870,8 @@
     cub::ArgIndexInputIterator<MinMaxExtents *> value_index_input_iterator(
         extents_device_.get());
     TransformZeroFilteredBlobSizes rewrite(
-        reduced_dot_blobs_pair_device_.get(), min_tag_width_, reversed_border_,
-        normal_border_, tag_detector_->qtp.min_cluster_pixels,
-        max_april_tag_perimeter);
+        min_tag_width_, reversed_border_, normal_border_,
+        tag_detector_->qtp.min_cluster_pixels, max_april_tag_perimeter);
     cub::TransformInputIterator<cub::KeyValuePair<long, MinMaxExtents>,
                                 TransformZeroFilteredBlobSizes,
                                 cub::ArgIndexInputIterator<MinMaxExtents *>>
@@ -755,7 +891,7 @@
         input_iterator, selected_extents_device_.get(), sum_points,
         num_quads_host));
 
-    MaybeCheckAndSynchronize();
+    MaybeCheckAndSynchronize("cub::DeviceScan::InclusiveScan");
   }
 
   after_transform_extents_.Record(&stream_);
@@ -776,10 +912,7 @@
     TransformOutputIterator<IndexPoint, IndexPoint, AddThetaToIndexPoint>
         output_iterator(selected_blobs_device_.get(), add_theta);
 
-    SelectBlobs select_blobs(
-        extents_device_.get(), reduced_dot_blobs_pair_device_.get(),
-        min_tag_width_, reversed_border_, normal_border_,
-        tag_detector_->qtp.min_cluster_pixels, max_april_tag_perimeter);
+    NonzeroBlobs select_blobs(selected_extents_device_.get());
 
     size_t temp_storage_bytes =
         temp_storage_compressed_filtered_blobs_device_.size();
@@ -789,13 +922,15 @@
         temp_storage_bytes, input_iterator, output_iterator,
         num_selected_blobs_device_.get(), num_compressed_union_marker_pair_host,
         select_blobs, stream_.get()));
-    MaybeCheckAndSynchronize();
 
-    num_selected_blobs_device_.MemcpyTo(&num_selected_blobs_host);
+    MaybeCheckAndSynchronize("cub::DeviceSelect::If");
+
+    num_selected_blobs_device_.MemcpyAsyncTo(&num_selected_blobs_host,
+                                             &stream_);
+    after_filter_.Record(&stream_);
+    after_filter_.Synchronize();
   }
 
-  after_filter_.Record(&stream_);
-
   {
     // Sort based on the angle.
     size_t temp_storage_bytes = radix_sort_tmpstorage_device_.size();
@@ -807,53 +942,213 @@
         num_selected_blobs_host, decomposer, IndexPoint::kRepEndBit,
         IndexPoint::kBitsInKey, stream_.get()));
 
-    MaybeCheckAndSynchronize();
+    MaybeCheckAndSynchronize("cub::DeviceRadixSort::SortKeys");
   }
 
   after_filtered_sort_.Record(&stream_);
 
-  // Report out how long things took.
-  LOG(INFO) << "Found " << num_compressed_union_marker_pair_host << " items";
-  LOG(INFO) << "Selected " << num_selected_blobs_host << " blobs";
+  {
+    // Now that we have the dot products, we need to rewrite the extents for the
+    // post-thresholded world so we can find the start and end address of blobs
+    // for fitting lines.
+    //
+    // Clear the size of non-passing extents and the starting offset of all
+    // extents.
+    TransformLineFitPoint rewrite(decimated_image_device_.get(), width_ / 2,
+                                  height_ / 2);
+    cub::TransformInputIterator<LineFitPoint, TransformLineFitPoint,
+                                IndexPoint *>
+        input_iterator(sorted_selected_blobs_device_.get(), rewrite);
 
+    MaskBlobIndex mask;
+    cub::TransformInputIterator<uint32_t, MaskBlobIndex, IndexPoint *>
+        key_iterator(sorted_selected_blobs_device_.get(), mask);
+
+    // Sum the counts of everything before us, and update the offset.
+    SumLineFitPoints sum_points;
+
+    // Rewrite the extents to have the starting offset and count match the
+    // post-selected values.
+    size_t temp_storage_bytes = temp_storage_line_fit_scan_device_.size();
+
+    CHECK_CUDA(cub::DeviceScan::InclusiveScanByKey(
+        temp_storage_line_fit_scan_device_.get(), temp_storage_bytes,
+        key_iterator, input_iterator, line_fit_points_device_.get(), sum_points,
+        num_selected_blobs_host));
+
+    MaybeCheckAndSynchronize("cub::DeviceScan::InclusiveScanByKey");
+  }
+  after_line_fit_.Record(&stream_);
+
+  {
+    FitLines(line_fit_points_device_.get(), num_selected_blobs_host,
+             selected_extents_device_.get(), num_quads_host, errs_device_.get(),
+             filtered_errs_device_.get(), filtered_is_local_peak_device_.get(),
+             &stream_);
+  }
+  after_line_filter_.Record(&stream_);
+
+  int num_compressed_peaks_host;
+  {
+    // Remove empty points which aren't to be considered before sorting to speed
+    // things up.
+    size_t temp_storage_bytes =
+        temp_storage_compressed_union_marker_pair_device_.size();
+    ValidPeaks peak_filter;
+    CHECK_CUDA(cub::DeviceSelect::If(
+        temp_storage_compressed_union_marker_pair_device_.get(),
+        temp_storage_bytes, filtered_is_local_peak_device_.get(),
+        compressed_peaks_device_.get(), num_compressed_peaks_device_.get(),
+        num_selected_blobs_host, peak_filter, stream_.get()));
+
+    after_peak_compression_.Record(&stream_);
+    MaybeCheckAndSynchronize("cub::DeviceSelect::If");
+    num_compressed_peaks_device_.MemcpyAsyncTo(&num_compressed_peaks_host,
+                                               &stream_);
+    after_peak_count_memcpy_.Record(&stream_);
+    after_peak_count_memcpy_.Synchronize();
+  }
+
+  {
+    // Sort based on the angle.
+    size_t temp_storage_bytes = radix_sort_tmpstorage_device_.size();
+    PeakDecomposer decomposer;
+
+    CHECK_CUDA(cub::DeviceRadixSort::SortKeys(
+        radix_sort_tmpstorage_device_.get(), temp_storage_bytes,
+        compressed_peaks_device_.get(), sorted_compressed_peaks_device_.get(),
+        num_compressed_peaks_host, decomposer, 0, PeakDecomposer::kBitsInKey,
+        stream_.get()));
+
+    MaybeCheckAndSynchronize("cub::DeviceRadixSort::SortKeys");
+  }
+
+  after_peak_sort_.Record(&stream_);
+
+  int num_quad_peaked_quads_host;
+  // Now that we have the peaks sorted, recompute the extents so we can easily
+  // pick out the number and top 10 peaks for line fitting.
+  {
+    // Our next step is to compute the extents of each blob so we can filter
+    // blobs.
+    cub::ArgIndexInputIterator<Peak *> value_index_input_iterator(
+        sorted_compressed_peaks_device_.get());
+    TransformToPeakExtents transform_extents;
+    cub::TransformInputIterator<PeakExtents, TransformToPeakExtents,
+                                cub::ArgIndexInputIterator<Peak *>>
+        value_input_iterator(value_index_input_iterator, transform_extents);
+
+    // Don't care about the output keys...
+    cub::DiscardOutputIterator<uint32_t> key_discard_iterator;
+
+    // Provide a mask to detect keys by rep01()
+    MaskPeakExtentsByBlobId mask;
+    cub::TransformInputIterator<uint32_t, MaskPeakExtentsByBlobId, Peak *>
+        key_input_iterator(sorted_compressed_peaks_device_.get(), mask);
+
+    // Reduction operator.
+    MergePeakExtents reduce;
+
+    size_t temp_storage_bytes =
+        temp_storage_bounds_reduce_by_key_device_.size();
+    cub::DeviceReduce::ReduceByKey(
+        temp_storage_bounds_reduce_by_key_device_.get(), temp_storage_bytes,
+        key_input_iterator, key_discard_iterator, value_input_iterator,
+        peak_extents_device_.get(), num_quad_peaked_quads_device_.get(), reduce,
+        num_compressed_peaks_host, stream_.get());
+    MaybeCheckAndSynchronize("cub::DeviceReduce::ReduceByKey");
+
+    after_filtered_peak_reduce_.Record(&stream_);
+
+    num_quad_peaked_quads_device_.MemcpyAsyncTo(&num_quad_peaked_quads_host,
+                                                &stream_);
+    MaybeCheckAndSynchronize("num_quad_peaked_quads_device_.MemcpyTo");
+    after_filtered_peak_host_memcpy_.Record(&stream_);
+    after_filtered_peak_host_memcpy_.Synchronize();
+  }
+
+  {
+    apriltag::FitQuads(
+        sorted_compressed_peaks_device_.get(), num_compressed_peaks_host,
+        peak_extents_device_.get(), num_quad_peaked_quads_host,
+        line_fit_points_device_.get(), tag_detector_->qtp.max_nmaxima,
+        selected_extents_device_.get(), tag_detector_->qtp.max_line_fit_mse,
+        tag_detector_->qtp.cos_critical_rad, fit_quads_device_.get(), &stream_);
+    MaybeCheckAndSynchronize("FitQuads");
+  }
+  after_quad_fit_.Record(&stream_);
+
+  {
+    fit_quads_host_.resize(num_quad_peaked_quads_host);
+    fit_quads_device_.MemcpyAsyncTo(fit_quads_host_.data(),
+                                    num_quad_peaked_quads_host, &stream_);
+    after_quad_fit_memcpy_.Record(&stream_);
+    after_quad_fit_memcpy_.Synchronize();
+  }
+
+  const aos::monotonic_clock::time_point before_fit_quads =
+      aos::monotonic_clock::now();
+  UpdateFitQuads();
+  AdjustPixelCenters();
+
+  DecodeTags();
+
+  const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
+
+  // TODO(austin): Bring it back to the CPU and see how good we did.
+
+  // Report out how long things took.
+
+  VLOG(1) << "Found " << num_compressed_union_marker_pair_host << " items";
+  VLOG(1) << "Selected " << num_selected_blobs_host << " right side out points";
+  VLOG(1) << "Found compressed runs: " << num_quads_host;
+  VLOG(1) << "Peaks " << num_compressed_peaks_host << " peaks";
+  VLOG(1) << "Peak Selected blobs " << num_quad_peaked_quads_host << " quads";
   CudaEvent *previous_event = &start_;
   for (auto name_event : std::vector<std::tuple<std::string_view, CudaEvent &>>{
            {"Memcpy", after_image_memcpy_to_device_},
            {"Threshold", after_threshold_},
+           {"Memcpy Gray", after_memcpy_gray_},
            {"Memset", after_memset_},
            {"Unionfinding", after_unionfinding_},
            {"Diff", after_diff_},
            {"Compact", after_compact_},
            {"Sort", after_sort_},
            {"Bounds", after_bounds_},
-           {"Dot product", after_dot_},
            {"Transform Extents", after_transform_extents_},
            {"Filter by dot product", after_filter_},
            {"Filtered sort", after_filtered_sort_},
+           {"Line Fit", after_line_fit_},
+           {"Error Filter", after_line_filter_},
+           {"Compress Peaks", after_peak_compression_},
+           {"Memcpy Peaks", after_peak_count_memcpy_},
+           {"Sort Peaks", after_peak_sort_},
+           {"Peak Extents", after_filtered_peak_reduce_},
+           {"Memcpy num Extents", after_filtered_peak_host_memcpy_},
+           {"FitQuads", after_quad_fit_},
        }) {
     std::get<1>(name_event).Synchronize();
-    LOG(INFO) << std::get<0>(name_event) << " "
-              << float_milli(
-                     std::get<1>(name_event).ElapsedTime(*previous_event))
-                     .count()
-              << "ms";
+    VLOG(1) << "    " << std::get<0>(name_event) << " "
+            << float_milli(std::get<1>(name_event).ElapsedTime(*previous_event))
+                   .count()
+            << "ms";
     previous_event = &std::get<1>(name_event);
   }
+  VLOG(1) << "  FitQuads " << float_milli(end_time - before_fit_quads).count()
+          << "ms on host";
 
-  LOG(INFO) << "Overall "
-            << float_milli(previous_event->ElapsedTime(start_)).count() << "ms";
-
+  VLOG(1) << "Overall "
+          << float_milli(previous_event->ElapsedTime(start_)).count() << "ms, "
+          << float_milli(end_time - start_time).count() << "ms on host";
   // Average.  Skip the first one as the kernel is warming up and is slower.
   if (!first_) {
     ++execution_count_;
     execution_duration_ += previous_event->ElapsedTime(start_);
-    LOG(INFO) << "Average overall "
-              << float_milli(execution_duration_ / execution_count_).count()
-              << "ms";
+    VLOG(1) << "Average overall "
+            << float_milli(execution_duration_ / execution_count_).count()
+            << "ms";
   }
 
-  LOG(INFO) << "Found compressed runs: " << num_quads_host;
-
   first_ = false;
 }
 
diff --git a/frc971/orin/apriltag.h b/frc971/orin/apriltag.h
index 068c0c7..f87e107 100644
--- a/frc971/orin/apriltag.h
+++ b/frc971/orin/apriltag.h
@@ -9,37 +9,12 @@
 #include "device_launch_parameters.h"
 #include "frc971/orin/cuda.h"
 #include "frc971/orin/gpu_image.h"
+#include "frc971/orin/line_fit_filter.h"
 #include "frc971/orin/points.h"
 
 namespace frc971 {
 namespace apriltag {
 
-// Class to hold the extents of a blob of points.
-struct MinMaxExtents {
-  // Min and max coordinates (in non-decimated coordinates)
-  uint16_t min_x;
-  uint16_t min_y;
-  uint16_t max_x;
-  uint16_t max_y;
-
-  // The starting offset of this blob of points in the vector holding all the
-  // points.
-  uint32_t starting_offset;
-
-  // The number of points in the blob.
-  uint32_t count;
-
-  // Center location of the blob using the aprilrobotics algorithm.
-  __host__ __device__ float cx() const {
-    return (min_x + max_x) * 0.5f + 0.05118;
-  }
-  __host__ __device__ float cy() const {
-    return (min_y + max_y) * 0.5f + -0.028581;
-  }
-};
-
-static_assert(sizeof(MinMaxExtents) == 16, "MinMaxExtents didn't pack right.");
-
 // Class to find the blob index of a point in a point vector.
 class BlobExtentsIndexFinder {
  public:
@@ -58,7 +33,6 @@
       }
 
       size_t average = min + (max - min) / 2;
-
       if (average < num_extents_ && extents_device_[average].starting_offset <=
                                         static_cast<size_t>(point_index)) {
         min = average;
@@ -80,6 +54,12 @@
   // TODO(austin): Cache the last one?
 };
 
+struct QuadCorners {
+  float corners[4][2];
+  bool reversed_border;
+  uint32_t blob_index;
+};
+
 // GPU based april tag detector.
 class GpuDetector {
  public:
@@ -88,35 +68,40 @@
 
   // Constructs a detector, reserving space for detecting tags of the provided
   // with and height, using the provided detector options.
-  GpuDetector(size_t width, size_t height,
-              const apriltag_detector_t *tag_detector);
+  GpuDetector(size_t width, size_t height, apriltag_detector_t *tag_detector);
 
   virtual ~GpuDetector();
 
   // Detects april tags in the provided image.
   void Detect(const uint8_t *image);
 
+  const std::vector<QuadCorners> &FitQuads() const;
+
+  const zarray_t *Detections() const { return detections_; }
+
   // Debug methods to expose internal state for testing.
-  void CopyGrayTo(uint8_t *output) { gray_image_device_.MemcpyTo(output); }
-  void CopyDecimatedTo(uint8_t *output) {
+  void CopyGrayTo(uint8_t *output) const {
+    gray_image_device_.MemcpyTo(output);
+  }
+  void CopyDecimatedTo(uint8_t *output) const {
     decimated_image_device_.MemcpyTo(output);
   }
-  void CopyThresholdedTo(uint8_t *output) {
+  void CopyThresholdedTo(uint8_t *output) const {
     thresholded_image_device_.MemcpyTo(output);
   }
-  void CopyUnionMarkersTo(uint32_t *output) {
+  void CopyUnionMarkersTo(uint32_t *output) const {
     union_markers_device_.MemcpyTo(output);
   }
 
-  void CopyUnionMarkerPairTo(QuadBoundaryPoint *output) {
+  void CopyUnionMarkerPairTo(QuadBoundaryPoint *output) const {
     union_marker_pair_device_.MemcpyTo(output);
   }
 
-  void CopyCompressedUnionMarkerPairTo(QuadBoundaryPoint *output) {
+  void CopyCompressedUnionMarkerPairTo(QuadBoundaryPoint *output) const {
     compressed_union_marker_pair_device_.MemcpyTo(output);
   }
 
-  std::vector<QuadBoundaryPoint> CopySortedUnionMarkerPair() {
+  std::vector<QuadBoundaryPoint> CopySortedUnionMarkerPair() const {
     std::vector<QuadBoundaryPoint> result;
     int size = NumCompressedUnionMarkerPairs();
     result.resize(size);
@@ -124,39 +109,75 @@
     return result;
   }
 
-  int NumCompressedUnionMarkerPairs() {
+  int NumCompressedUnionMarkerPairs() const {
     return num_compressed_union_marker_pair_device_.Copy()[0];
   }
 
-  void CopyUnionMarkersSizeTo(uint32_t *output) {
+  void CopyUnionMarkersSizeTo(uint32_t *output) const {
     union_markers_size_device_.MemcpyTo(output);
   }
 
   int NumQuads() const { return num_quads_device_.Copy()[0]; }
 
-  std::vector<MinMaxExtents> CopyExtents() {
+  std::vector<MinMaxExtents> CopyExtents() const {
     return extents_device_.Copy(NumQuads());
   }
 
-  std::vector<float> CopyReducedDotBlobs() {
-    return reduced_dot_blobs_pair_device_.Copy(NumQuads());
-  }
-
-  std::vector<cub::KeyValuePair<long, MinMaxExtents>> CopySelectedExtents() {
+  std::vector<cub::KeyValuePair<long, MinMaxExtents>> CopySelectedExtents()
+      const {
     return selected_extents_device_.Copy(NumQuads());
   }
 
   int NumSelectedPairs() const { return num_selected_blobs_device_.Copy()[0]; }
 
-  std::vector<IndexPoint> CopySelectedBlobs() {
+  std::vector<IndexPoint> CopySelectedBlobs() const {
     return selected_blobs_device_.Copy(NumSelectedPairs());
   }
 
-  std::vector<IndexPoint> CopySortedSelectedBlobs() {
+  std::vector<IndexPoint> CopySortedSelectedBlobs() const {
     return sorted_selected_blobs_device_.Copy(NumSelectedPairs());
   }
 
+  std::vector<LineFitPoint> CopyLineFitPoints() const {
+    return line_fit_points_device_.Copy(NumSelectedPairs());
+  }
+
+  std::vector<double> CopyErrors() const {
+    return errs_device_.Copy(NumSelectedPairs());
+  }
+
+  std::vector<double> CopyFilteredErrors() const {
+    return filtered_errs_device_.Copy(NumSelectedPairs());
+  }
+  std::vector<Peak> CopyPeaks() const {
+    return filtered_is_local_peak_device_.Copy(NumSelectedPairs());
+  }
+
+  int NumCompressedPeaks() const {
+    return num_compressed_peaks_device_.Copy()[0];
+  }
+
+  std::vector<Peak> CopyCompressedPeaks() const {
+    return compressed_peaks_device_.Copy(NumCompressedPeaks());
+  }
+
+  int NumFitQuads() const { return num_quad_peaked_quads_device_.Copy()[0]; }
+
+  std::vector<FitQuad> CopyFitQuads() const {
+    return fit_quads_device_.Copy(NumFitQuads());
+  }
+
+  void AdjustCenter(float corners[4][2]) const;
+
  private:
+  void UpdateFitQuads();
+
+  void AdjustPixelCenters();
+
+  void DecodeTags();
+
+  static void QuadDecodeTask(void *_u);
+
   // Creates a GPU image wrapped around the provided memory.
   template <typename T>
   GpuImage<T> ToGpuImage(GpuMemory<T> &memory) {
@@ -184,7 +205,7 @@
   const size_t height_;
 
   // Detector parameters.
-  const apriltag_detector_t *tag_detector_;
+  apriltag_detector_t *tag_detector_;
 
   // Stream to operate on.
   CudaStream stream_;
@@ -193,19 +214,29 @@
   CudaEvent start_;
   CudaEvent after_image_memcpy_to_device_;
   CudaEvent after_threshold_;
+  CudaEvent after_memcpy_gray_;
   CudaEvent after_memset_;
   CudaEvent after_unionfinding_;
   CudaEvent after_diff_;
   CudaEvent after_compact_;
   CudaEvent after_sort_;
   CudaEvent after_bounds_;
-  CudaEvent after_dot_;
   CudaEvent after_transform_extents_;
   CudaEvent after_filter_;
   CudaEvent after_filtered_sort_;
+  CudaEvent after_line_fit_;
+  CudaEvent after_line_filter_;
+  CudaEvent after_peak_compression_;
+  CudaEvent after_peak_count_memcpy_;
+  CudaEvent after_peak_sort_;
+  CudaEvent after_filtered_peak_reduce_;
+  CudaEvent after_filtered_peak_host_memcpy_;
+  CudaEvent after_quad_fit_;
+  CudaEvent after_quad_fit_memcpy_;
 
   // TODO(austin): Remove this...
   HostMemory<uint8_t> color_image_host_;
+  HostMemory<uint8_t> gray_image_host_;
 
   // Starting color image.
   GpuMemory<uint8_t> color_image_device_;
@@ -238,9 +269,6 @@
   GpuMemory<size_t> num_quads_device_{/* allocate 1 integer...*/ 1};
   // Bounds per blob, one blob per ID.
   GpuMemory<MinMaxExtents> extents_device_;
-  // Sum of the dot products between the vector from center to each point, with
-  // the gradient at each point.
-  GpuMemory<float> reduced_dot_blobs_pair_device_;
   // Extents of all the blobs under consideration.
   GpuMemory<cub::KeyValuePair<long, MinMaxExtents>> selected_extents_device_;
 
@@ -252,6 +280,24 @@
   // Sorted list of those points.
   GpuMemory<IndexPoint> sorted_selected_blobs_device_;
 
+  // TODO(austin): Can we bound this better?  This is a lot of memory.
+  GpuMemory<LineFitPoint> line_fit_points_device_;
+
+  GpuMemory<double> errs_device_;
+  GpuMemory<double> filtered_errs_device_;
+  GpuMemory<Peak> filtered_is_local_peak_device_;
+  GpuMemory<int> num_compressed_peaks_device_{/* allocate 1 integer...*/ 1};
+  GpuMemory<Peak> compressed_peaks_device_;
+  GpuMemory<Peak> sorted_compressed_peaks_device_;
+
+  GpuMemory<int> num_quad_peaked_quads_device_{/* allocate 1 integer...*/ 1};
+  GpuMemory<PeakExtents> peak_extents_device_;
+
+  GpuMemory<FitQuad> fit_quads_device_;
+
+  std::vector<FitQuad> fit_quads_host_;
+  std::vector<QuadCorners> quad_corners_host_;
+
   // Temporary storage for each of the steps.
   // TODO(austin): Can we combine these and just use the max?
   GpuMemory<uint32_t> radix_sort_tmpstorage_device_;
@@ -260,6 +306,7 @@
   GpuMemory<uint8_t> temp_storage_dot_product_device_;
   GpuMemory<uint8_t> temp_storage_compressed_filtered_blobs_device_;
   GpuMemory<uint8_t> temp_storage_selected_extents_scan_device_;
+  GpuMemory<uint8_t> temp_storage_line_fit_scan_device_;
 
   // Cumulative duration of april tag detection.
   std::chrono::nanoseconds execution_duration_{0};
@@ -272,6 +319,11 @@
   bool normal_border_ = false;
   bool reversed_border_ = false;
   int min_tag_width_ = 1000000;
+
+  zarray_t *poly0_;
+  zarray_t *poly1_;
+
+  zarray_t *detections_ = nullptr;
 };
 
 }  // namespace apriltag
diff --git a/frc971/orin/apriltag_detect.cc b/frc971/orin/apriltag_detect.cc
new file mode 100644
index 0000000..eecc056
--- /dev/null
+++ b/frc971/orin/apriltag_detect.cc
@@ -0,0 +1,523 @@
+#include <thrust/iterator/constant_iterator.h>
+#include <thrust/iterator/transform_iterator.h>
+
+#include <cub/device/device_copy.cuh>
+#include <cub/device/device_radix_sort.cuh>
+#include <cub/device/device_reduce.cuh>
+#include <cub/device/device_run_length_encode.cuh>
+#include <cub/device/device_scan.cuh>
+#include <cub/device/device_segmented_sort.cuh>
+#include <cub/device/device_select.cuh>
+#include <cub/iterator/discard_output_iterator.cuh>
+#include <cub/iterator/transform_input_iterator.cuh>
+#include <vector>
+
+#include "glog/logging.h"
+
+#include "aos/time/time.h"
+#include "frc971/orin/apriltag.h"
+#include "frc971/orin/labeling_allegretti_2019_BKE.h"
+#include "frc971/orin/threshold.h"
+
+DEFINE_int32(debug_blob_index, 4096, "Blob to print out for");
+
+extern "C" {
+
+void quad_decode_index(apriltag_detector_t *td, struct quad *quad_original,
+                       image_u8_t *im, image_u8_t *im_samples,
+                       zarray_t *detections);
+
+void reconcile_detections(zarray_t *detections, zarray_t *poly0,
+                          zarray_t *poly1);
+};
+
+namespace frc971 {
+namespace apriltag {
+namespace {
+
+void HostFitLine(LineFitMoments moments, double *lineparam01,
+                 double *lineparam23, double *err, double *mse) {
+  int64_t Cxx = moments.Mxx * moments.W - static_cast<int64_t>(moments.Mx) *
+                                              static_cast<int64_t>(moments.Mx);
+  int64_t Cxy = moments.Mxy * moments.W - static_cast<int64_t>(moments.Mx) *
+                                              static_cast<int64_t>(moments.My);
+  int64_t Cyy = moments.Myy * moments.W - static_cast<int64_t>(moments.My) *
+                                              static_cast<int64_t>(moments.My);
+
+  // Pose it as an eigenvalue problem.
+  const float hypot_cached = std::hypotf((Cxx - Cyy), 2 * Cxy);
+  const float eight_w_squared = static_cast<float>(
+      static_cast<int64_t>(moments.W) * static_cast<int64_t>(moments.W) * 8.0);
+  const float eig_small = (Cxx + Cyy - hypot_cached) / eight_w_squared;
+
+  if (lineparam01) {
+    lineparam01[0] =
+        static_cast<float>(moments.Mx) / static_cast<float>(moments.W * 2);
+    lineparam01[1] =
+        static_cast<float>(moments.My) / static_cast<float>(moments.W * 2);
+  }
+  if (lineparam23) {
+    // These don't match the originals at all, but the math should come out
+    // right.  n{xy}{12} end up being multiplied by 8 W^2, and we compare the
+    // square but use hypot on nx, ny directly.  (and let the W^2 term come out
+    // as common to both the hypot and nxy terms.)
+    const float nx1 = static_cast<float>(Cxx - Cyy) - hypot_cached;
+    const float ny1 = Cxy * 2;
+    const float M1 = nx1 * nx1 + ny1 * ny1;
+    const float nx2 = Cxy * 2;
+    const float ny2 = static_cast<float>(Cyy - Cxx) - hypot_cached;
+    const float M2 = nx2 * nx2 + ny2 * ny2;
+
+    float nx, ny;
+    if (M1 > M2) {
+      nx = nx1;
+      ny = ny1;
+    } else {
+      nx = nx2;
+      ny = ny2;
+    }
+
+    float length = std::hypotf(nx, ny);
+    lineparam23[0] = nx / length;
+    lineparam23[1] = ny / length;
+  }
+
+  // sum of squared errors
+  *err = moments.N * eig_small;
+
+  // mean squared error
+  *mse = eig_small;
+}
+
+}  // namespace
+
+const std::vector<QuadCorners> &GpuDetector::FitQuads() const {
+  return quad_corners_host_;
+}
+
+void GpuDetector::UpdateFitQuads() {
+  quad_corners_host_.resize(0);
+  VLOG(1) << "Considering " << fit_quads_host_.size();
+  for (const FitQuad &quad : fit_quads_host_) {
+    bool print = quad.blob_index == FLAGS_debug_blob_index;
+    if (!quad.valid) {
+      continue;
+    }
+    QuadCorners corners;
+    corners.blob_index = quad.blob_index;
+    CHECK(normal_border_ != reversed_border_);
+    corners.reversed_border = reversed_border_;
+
+    double lines[4][4];
+    for (int i = 0; i < 4; i++) {
+      double err;
+      double mse;
+      HostFitLine(quad.moments[i], lines[i], lines[i] + 2, &err, &mse);
+      if (print) {
+        LOG(INFO) << "Blob " << corners.blob_index << " mse -> " << mse
+                  << " err " << err << " index " << quad.indices[i] << ", "
+                  << quad.indices[(i + 1) % 4];
+        LOG(INFO) << "   " << quad.moments[i];
+      }
+    }
+
+    bool bad_determinant = false;
+    for (int i = 0; i < 4; i++) {
+      // solve for the intersection of lines (i) and (i+1)&3.
+      // p0 + lambda0*u0 = p1 + lambda1*u1, where u0 and u1
+      // are the line directions.
+      //
+      // lambda0*u0 - lambda1*u1 = (p1 - p0)
+      //
+      // rearrange (solve for lambdas)
+      //
+      // [u0_x   -u1_x ] [lambda0] = [ p1_x - p0_x ]
+      // [u0_y   -u1_y ] [lambda1]   [ p1_y - p0_y ]
+      //
+      // remember that lines[i][0,1] = p, lines[i][2,3] = NORMAL vector.
+      // We want the unit vector, so we need the perpendiculars. Thus, below
+      // we have swapped the x and y components and flipped the y components.
+
+      double A00 = lines[i][3], A01 = -lines[(i + 1) & 3][3];
+      double A10 = -lines[i][2], A11 = lines[(i + 1) & 3][2];
+      double B0 = -lines[i][0] + lines[(i + 1) & 3][0];
+      double B1 = -lines[i][1] + lines[(i + 1) & 3][1];
+
+      double det = A00 * A11 - A10 * A01;
+
+      // inverse.
+      double W00 = A11 / det, W01 = -A01 / det;
+      if (fabs(det) < 0.001) {
+        bad_determinant = true;
+        break;
+      }
+
+      // solve
+      double L0 = W00 * B0 + W01 * B1;
+
+      // compute intersection
+      corners.corners[i][0] = lines[i][0] + L0 * A00;
+      corners.corners[i][1] = lines[i][1] + L0 * A10;
+      if (print) {
+        LOG(INFO) << "Calculated corner[" << i << "] -> ("
+                  << std::setprecision(20) << corners.corners[i][0] << ", "
+                  << std::setprecision(20) << corners.corners[i][1] << ")";
+      }
+    }
+    if (bad_determinant) {
+      continue;
+    }
+
+    {
+      float area = 0;
+
+      // get area of triangle formed by points 0, 1, 2, 0
+      float length[3], p;
+      for (int i = 0; i < 3; i++) {
+        int idxa = i;            // 0, 1, 2,
+        int idxb = (i + 1) % 3;  // 1, 2, 0
+        length[i] =
+            hypotf((corners.corners[idxb][0] - corners.corners[idxa][0]),
+                   (corners.corners[idxb][1] - corners.corners[idxa][1]));
+      }
+      p = (length[0] + length[1] + length[2]) / 2;
+
+      area += sqrtf(p * (p - length[0]) * (p - length[1]) * (p - length[2]));
+
+      // get area of triangle formed by points 2, 3, 0, 2
+      for (int i = 0; i < 3; i++) {
+        int idxs[] = {2, 3, 0, 2};
+        int idxa = idxs[i];
+        int idxb = idxs[i + 1];
+        length[i] =
+            hypotf((corners.corners[idxb][0] - corners.corners[idxa][0]),
+                   (corners.corners[idxb][1] - corners.corners[idxa][1]));
+      }
+      p = (length[0] + length[1] + length[2]) / 2;
+
+      area += sqrtf(p * (p - length[0]) * (p - length[1]) * (p - length[2]));
+
+      if (area < 0.95 * min_tag_width_ * min_tag_width_) {
+        if (print) {
+          LOG(INFO) << "Area of " << area << " smaller than "
+                    << 0.95 * min_tag_width_ * min_tag_width_;
+        }
+        continue;
+      }
+    }
+
+    // reject quads whose cumulative angle change isn't equal to 2PI
+    {
+      bool reject_corner = false;
+      for (int i = 0; i < 4; i++) {
+        int i0 = i, i1 = (i + 1) & 3, i2 = (i + 2) & 3;
+
+        float dx1 = corners.corners[i1][0] - corners.corners[i0][0];
+        float dy1 = corners.corners[i1][1] - corners.corners[i0][1];
+        float dx2 = corners.corners[i2][0] - corners.corners[i1][0];
+        float dy2 = corners.corners[i2][1] - corners.corners[i1][1];
+        float cos_dtheta =
+            (dx1 * dx2 + dy1 * dy2) /
+            sqrtf((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2));
+
+        if (print) {
+          LOG(INFO) << "Cosdtheta -> for " << i0 << " " << i1 << " " << i2
+                    << " -> " << cos_dtheta << " threshold "
+                    << tag_detector_->qtp.cos_critical_rad;
+        }
+
+        if (std::abs(cos_dtheta) > tag_detector_->qtp.cos_critical_rad ||
+            dx1 * dy2 < dy1 * dx2) {
+          reject_corner = true;
+          break;
+        }
+      }
+      if (reject_corner) {
+        continue;
+      }
+    }
+    quad_corners_host_.push_back(corners);
+  }
+}
+
+void GpuDetector::AdjustCenter(float corners[4][2]) const {
+  const float quad_decimate = tag_detector_->quad_decimate;
+  if (tag_detector_->quad_decimate > 1) {
+    if (tag_detector_->quad_decimate == 1.5) {
+      for (int j = 0; j < 4; j++) {
+        corners[j][0] *= quad_decimate;
+        corners[j][1] *= quad_decimate;
+      }
+    } else {
+      for (int j = 0; j < 4; j++) {
+        corners[j][0] = (corners[j][0] - 0.5f) * quad_decimate + 0.5f;
+        corners[j][1] = (corners[j][1] - 0.5f) * quad_decimate + 0.5f;
+      }
+    }
+  }
+}
+
+void GpuDetector::AdjustPixelCenters() {
+  const float quad_decimate = tag_detector_->quad_decimate;
+
+  if (quad_decimate > 1) {
+    if (tag_detector_->quad_decimate == 1.5) {
+      for (QuadCorners &quad : quad_corners_host_) {
+        for (int j = 0; j < 4; j++) {
+          quad.corners[j][0] *= quad_decimate;
+          quad.corners[j][1] *= quad_decimate;
+        }
+      }
+    } else {
+      for (QuadCorners &quad : quad_corners_host_) {
+        for (int j = 0; j < 4; j++) {
+          quad.corners[j][0] =
+              (quad.corners[j][0] - 0.5f) * quad_decimate + 0.5f;
+          quad.corners[j][1] =
+              (quad.corners[j][1] - 0.5f) * quad_decimate + 0.5f;
+        }
+      }
+    }
+  }
+}
+
+static inline int detection_compare_function(const void *_a, const void *_b) {
+  apriltag_detection_t *a = *(apriltag_detection_t **)_a;
+  apriltag_detection_t *b = *(apriltag_detection_t **)_b;
+  return a->id - b->id;
+}
+
+struct QuadDecodeTaskStruct {
+  int i0, i1;
+  QuadCorners *quads;
+  apriltag_detector_t *td;
+
+  image_u8_t *im;
+  zarray_t *detections;
+
+  image_u8_t *im_samples;
+};
+
+// Mostly stolen from aprilrobotics, but modified to implement the dewarp.
+void RefineEdges(apriltag_detector_t *td, image_u8_t *im_orig,
+                 struct quad *quad) {
+  double lines[4][4];  // for each line, [Ex Ey nx ny]
+
+  for (int edge = 0; edge < 4; edge++) {
+    int a = edge, b = (edge + 1) & 3;  // indices of the end points.
+
+    // compute the normal to the current line estimate
+    float nx = quad->p[b][1] - quad->p[a][1];
+    float ny = -quad->p[b][0] + quad->p[a][0];
+    float mag = sqrtf(nx * nx + ny * ny);
+    nx /= mag;
+    ny /= mag;
+
+    if (quad->reversed_border) {
+      nx = -nx;
+      ny = -ny;
+    }
+
+    // we will now fit a NEW line by sampling points near
+    // our original line that have large gradients. On really big tags,
+    // we're willing to sample more to get an even better estimate.
+    int nsamples = std::max<int>(16, mag / 8);  // XXX tunable
+
+    // stats for fitting a line...
+    double Mx = 0, My = 0, Mxx = 0, Mxy = 0, Myy = 0, N = 0;
+
+    for (int s = 0; s < nsamples; s++) {
+      // compute a point along the line... Note, we're avoiding
+      // sampling *right* at the corners, since those points are
+      // the least reliable.
+      double alpha = (1.0 + s) / (nsamples + 1);
+      double x0 = alpha * quad->p[a][0] + (1 - alpha) * quad->p[b][0];
+      double y0 = alpha * quad->p[a][1] + (1 - alpha) * quad->p[b][1];
+
+      // search along the normal to this line, looking at the
+      // gradients along the way. We're looking for a strong
+      // response.
+      double Mn = 0;
+      double Mcount = 0;
+
+      // XXX tunable: how far to search?  We want to search far
+      // enough that we find the best edge, but not so far that
+      // we hit other edges that aren't part of the tag. We
+      // shouldn't ever have to search more than quad_decimate,
+      // since otherwise we would (ideally) have started our
+      // search on another pixel in the first place. Likewise,
+      // for very small tags, we don't want the range to be too
+      // big.
+      double range = td->quad_decimate + 1;
+
+      // XXX tunable step size.
+      for (double n = -range; n <= range; n += 0.25) {
+        // Because of the guaranteed winding order of the
+        // points in the quad, we will start inside the white
+        // portion of the quad and work our way outward.
+        //
+        // sample to points (x1,y1) and (x2,y2) XXX tunable:
+        // how far +/- to look? Small values compute the
+        // gradient more precisely, but are more sensitive to
+        // noise.
+        double grange = 1;
+        int x1 = x0 + (n + grange) * nx;
+        int y1 = y0 + (n + grange) * ny;
+        if (x1 < 0 || x1 >= im_orig->width || y1 < 0 || y1 >= im_orig->height)
+          continue;
+
+        int x2 = x0 + (n - grange) * nx;
+        int y2 = y0 + (n - grange) * ny;
+        if (x2 < 0 || x2 >= im_orig->width || y2 < 0 || y2 >= im_orig->height)
+          continue;
+
+        int g1 = im_orig->buf[y1 * im_orig->stride + x1];
+        int g2 = im_orig->buf[y2 * im_orig->stride + x2];
+
+        if (g1 < g2)  // reject points whose gradient is "backwards". They can
+                      // only hurt us.
+          continue;
+
+        double weight =
+            (g2 - g1) *
+            (g2 - g1);  // XXX tunable. What shape for weight=f(g2-g1)?
+
+        // compute weighted average of the gradient at this point.
+        Mn += weight * n;
+        Mcount += weight;
+      }
+
+      // what was the average point along the line?
+      if (Mcount == 0) continue;
+
+      double n0 = Mn / Mcount;
+
+      // where is the point along the line?
+      double bestx = x0 + n0 * nx;
+      double besty = y0 + n0 * ny;
+
+      // update our line fit statistics
+      Mx += bestx;
+      My += besty;
+      Mxx += bestx * bestx;
+      Mxy += bestx * besty;
+      Myy += besty * besty;
+      N++;
+    }
+
+    // fit a line
+    double Ex = Mx / N, Ey = My / N;
+    double Cxx = Mxx / N - Ex * Ex;
+    double Cxy = Mxy / N - Ex * Ey;
+    double Cyy = Myy / N - Ey * Ey;
+
+    // TODO: Can replace this with same code as in fit_line.
+    double normal_theta = .5 * atan2f(-2 * Cxy, (Cyy - Cxx));
+    nx = cosf(normal_theta);
+    ny = sinf(normal_theta);
+    lines[edge][0] = Ex;
+    lines[edge][1] = Ey;
+    lines[edge][2] = nx;
+    lines[edge][3] = ny;
+  }
+
+  // now refit the corners of the quad
+  for (int i = 0; i < 4; i++) {
+    // solve for the intersection of lines (i) and (i+1)&3.
+    double A00 = lines[i][3], A01 = -lines[(i + 1) & 3][3];
+    double A10 = -lines[i][2], A11 = lines[(i + 1) & 3][2];
+    double B0 = -lines[i][0] + lines[(i + 1) & 3][0];
+    double B1 = -lines[i][1] + lines[(i + 1) & 3][1];
+
+    double det = A00 * A11 - A10 * A01;
+
+    // inverse.
+    if (fabs(det) > 0.001) {
+      // solve
+      double W00 = A11 / det, W01 = -A01 / det;
+
+      double L0 = W00 * B0 + W01 * B1;
+
+      // Compute intersection. Note that line i represents the line from corner
+      // i to (i+1)&3, so the intersection of line i with line (i+1)&3
+      // represents corner (i+1)&3.
+      quad->p[(i + 1) & 3][0] = lines[i][0] + L0 * A00;
+      quad->p[(i + 1) & 3][1] = lines[i][1] + L0 * A10;
+    } else {
+      // this is a bad sign. We'll just keep the corner we had.
+      //            debug_print("bad det: %15f %15f %15f %15f %15f\n", A00, A11,
+      //            A10, A01, det);
+    }
+  }
+}
+
+void GpuDetector::QuadDecodeTask(void *_u) {
+  QuadDecodeTaskStruct *task = reinterpret_cast<QuadDecodeTaskStruct *>(_u);
+  apriltag_detector_t *td = task->td;
+  image_u8_t *im = task->im;
+
+  for (int quadidx = task->i0; quadidx < task->i1; quadidx++) {
+    struct quad quad_original;
+    std::memcpy(quad_original.p, task->quads[quadidx].corners,
+                sizeof(task->quads[quadidx].corners));
+
+    quad_original.reversed_border = task->quads[quadidx].reversed_border;
+    quad_original.H = nullptr;
+    quad_original.Hinv = nullptr;
+
+    if (td->refine_edges) {
+      RefineEdges(td, im, &quad_original);
+    }
+
+    quad_decode_index(td, &quad_original, im, task->im_samples,
+                      task->detections);
+  }
+}
+
+void GpuDetector::DecodeTags() {
+  size_t chunksize =
+      1 + quad_corners_host_.size() /
+              (APRILTAG_TASKS_PER_THREAD_TARGET * tag_detector_->nthreads);
+
+  std::vector<QuadDecodeTaskStruct> tasks(
+      (quad_corners_host_.size() / chunksize + 1));
+
+  for (int i = 0; i < zarray_size(detections_); ++i) {
+    apriltag_detection_t *det;
+    zarray_get(detections_, i, &det);
+    apriltag_detection_destroy(det);
+  }
+
+  zarray_truncate(detections_, 0);
+
+  image_u8_t im_orig{
+      .width = static_cast<int32_t>(width_),
+      .height = static_cast<int32_t>(height_),
+      .stride = static_cast<int32_t>(width_),
+      .buf = gray_image_host_.get(),
+  };
+
+  int ntasks = 0;
+  for (size_t i = 0; i < quad_corners_host_.size(); i += chunksize) {
+    tasks[ntasks].i0 = i;
+    tasks[ntasks].i1 = std::min(quad_corners_host_.size(), i + chunksize);
+    tasks[ntasks].quads = quad_corners_host_.data();
+    tasks[ntasks].td = tag_detector_;
+    tasks[ntasks].im = &im_orig;
+    tasks[ntasks].detections = detections_;
+
+    tasks[ntasks].im_samples = nullptr;
+
+    workerpool_add_task(tag_detector_->wp, QuadDecodeTask, &tasks[ntasks]);
+    ntasks++;
+  }
+
+  workerpool_run(tag_detector_->wp);
+
+  reconcile_detections(detections_, poly0_, poly1_);
+
+  zarray_sort(detections_, detection_compare_function);
+}
+
+}  // namespace apriltag
+}  // namespace frc971
diff --git a/frc971/orin/cuda.cc b/frc971/orin/cuda.cc
index 1173f73..89b1529 100644
--- a/frc971/orin/cuda.cc
+++ b/frc971/orin/cuda.cc
@@ -10,14 +10,20 @@
 namespace frc971 {
 namespace apriltag {
 
-void CheckAndSynchronize() {
-  CHECK_CUDA(cudaDeviceSynchronize());
-  CHECK_CUDA(cudaGetLastError());
+size_t overall_memory = 0;
+
+void CheckAndSynchronize(std::string_view message) {
+  CHECK_CUDA(cudaDeviceSynchronize()) << message;
+  CHECK_CUDA(cudaGetLastError()) << message;
 }
 
 void MaybeCheckAndSynchronize() {
   if (FLAGS_sync) CheckAndSynchronize();
 }
 
+void MaybeCheckAndSynchronize(std::string_view message) {
+  if (FLAGS_sync) CheckAndSynchronize(message);
+}
+
 }  // namespace apriltag
 }  // namespace frc971
diff --git a/frc971/orin/cuda.h b/frc971/orin/cuda.h
index e386aa6..2cc8f44 100644
--- a/frc971/orin/cuda.h
+++ b/frc971/orin/cuda.h
@@ -139,12 +139,15 @@
 
   // Copies data to host memory from this memory asynchronously on the provided
   // stream.
-  void MemcpyAsyncTo(T *host_memory, CudaStream *stream) const {
+  void MemcpyAsyncTo(T *host_memory, size_t size, CudaStream *stream) const {
     CHECK_CUDA(cudaMemcpyAsync(reinterpret_cast<void *>(host_memory),
                                reinterpret_cast<void *>(memory_),
-                               sizeof(T) * size_, cudaMemcpyDeviceToHost,
+                               sizeof(T) * size, cudaMemcpyDeviceToHost,
                                stream->get()));
   }
+  void MemcpyAsyncTo(T *host_memory, CudaStream *stream) const {
+    MemcpyAsyncTo(host_memory, size_, stream);
+  }
   void MemcpyAsyncTo(HostMemory<T> *host_memory, CudaStream *stream) const {
     MemcpyAsyncTo(host_memory->get(), stream);
   }
@@ -195,11 +198,12 @@
 };
 
 // Synchronizes and CHECKs for success the last CUDA operation.
-void CheckAndSynchronize();
+void CheckAndSynchronize(std::string_view message = "");
 
 // Synchronizes and CHECKS iff --sync is passed on the command line.  Makes it
 // so we can leave debugging in the code.
 void MaybeCheckAndSynchronize();
+void MaybeCheckAndSynchronize(std::string_view message);
 
 }  // namespace apriltag
 }  // namespace frc971
diff --git a/frc971/orin/cuda_april_tag_test.cc b/frc971/orin/cuda_april_tag_test.cc
index 776caf5..9e5eb5f 100644
--- a/frc971/orin/cuda_april_tag_test.cc
+++ b/frc971/orin/cuda_april_tag_test.cc
@@ -25,6 +25,8 @@
 
 DEFINE_bool(debug, false, "If true, write debug images.");
 
+DECLARE_int32(debug_blob_index);
+
 // Get access to the intermediates of aprilrobotics.
 extern "C" {
 
@@ -42,8 +44,45 @@
 
   float slope;
 };
+
+struct line_fit_pt {
+  double Mx, My;
+  double Mxx, Myy, Mxy;
+  double W;  // total weight
+};
+
+struct line_fit_pt *compute_lfps(int sz, zarray_t *cluster, image_u8_t *im);
+
+void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1,
+              double *lineparm, double *err, double *mse);
+
+int fit_quad(apriltag_detector_t *td, image_u8_t *im, zarray_t *cluster,
+             struct quad *quad, int tag_width, bool normal_border,
+             bool reversed_border);
+
 }  // extern C
 
+std::ostream &operator<<(std::ostream &os, const line_fit_pt &point) {
+  os << "{Mx:" << std::setprecision(20) << point.Mx
+     << ", My:" << std::setprecision(20) << point.My
+     << ", Mxx:" << std::setprecision(20) << point.Mxx
+     << ", Myy:" << std::setprecision(20) << point.Myy
+     << ", Mxy:" << std::setprecision(20) << point.Mxy << ", W:" << point.W
+     << "}";
+  return os;
+}
+
+namespace frc971::apriltag {
+std::ostream &operator<<(std::ostream &os,
+                         const frc971::apriltag::LineFitPoint &point) {
+  os << "{Mx:" << point.Mx / 2.0 << ", My:" << point.My / 2.0
+     << ", Mxx:" << point.Mxx / 4.0 << ", Mxy:" << point.Mxy / 4.0
+     << ", Myy:" << point.Myy / 4.0 << ", W:" << point.W << "}";
+  return os;
+}
+
+}  // namespace frc971::apriltag
+
 // Converts a cv::Mat to an aprilrobotics image.
 image_u8_t ToImageu8t(const cv::Mat &img) {
   return image_u8_t{
@@ -196,7 +235,7 @@
 
   apriltag_detector_add_family_bits(tag_detector, tag_family, 1);
 
-  tag_detector->nthreads = 1;
+  tag_detector->nthreads = 6;
   tag_detector->wp = workerpool_create(tag_detector->nthreads);
   tag_detector->qtp.min_white_black_diff = 5;
   tag_detector->debug = FLAGS_debug;
@@ -260,6 +299,9 @@
   }
 
   ~CudaAprilTagDetector() {
+    if (aprilrobotics_detections_ != nullptr) {
+      apriltag_detections_destroy(aprilrobotics_detections_);
+    }
     apriltag_detector_destroy(tag_detector_);
     free(tag_family_);
   }
@@ -286,11 +328,17 @@
     num_compressed_union_marker_pair_ =
         gpu_detector_.NumCompressedUnionMarkerPairs();
     extents_cuda_ = gpu_detector_.CopyExtents();
-    reduced_dot_blobs_pair_cuda_ = gpu_detector_.CopyReducedDotBlobs();
+    selected_extents_cuda_ = gpu_detector_.CopySelectedExtents();
     selected_blobs_cuda_ = gpu_detector_.CopySelectedBlobs();
     sorted_selected_blobs_cuda_ = gpu_detector_.CopySortedSelectedBlobs();
+    line_fit_points_cuda_ = gpu_detector_.CopyLineFitPoints();
+    errors_device_ = gpu_detector_.CopyErrors();
+    filtered_errors_device_ = gpu_detector_.CopyFilteredErrors();
+    peaks_device_ = gpu_detector_.CopyPeaks();
     num_quads_ = gpu_detector_.NumQuads();
 
+    fit_quads_ = gpu_detector_.FitQuads();
+
     LOG(INFO) << "num_compressed_union_marker_pair "
               << sorted_union_marker_pair_.size();
   }
@@ -307,18 +355,21 @@
     };
 
     LOG(INFO) << "Starting CPU detect.";
-    zarray_t *detections = apriltag_detector_detect(tag_detector_, &im);
+    if (aprilrobotics_detections_ != nullptr) {
+      apriltag_detections_destroy(aprilrobotics_detections_);
+    }
+    aprilrobotics_detections_ = apriltag_detector_detect(tag_detector_, &im);
 
     timeprofile_display(tag_detector_->tp);
 
-    for (int i = 0; i < zarray_size(detections); i++) {
+    for (int i = 0; i < zarray_size(aprilrobotics_detections_); i++) {
       apriltag_detection_t *det;
-      zarray_get(detections, i, &det);
+      zarray_get(aprilrobotics_detections_, i, &det);
 
       if (det->decision_margin > FLAGS_min_decision_margin) {
-        VLOG(1) << "Found tag number " << det->id
-                << " hamming: " << det->hamming
-                << " margin: " << det->decision_margin;
+        LOG(INFO) << "Found tag number " << det->id
+                  << " hamming: " << det->hamming
+                  << " margin: " << det->decision_margin;
       }
     }
   }
@@ -725,12 +776,14 @@
           [](std::tuple<float, uint64_t> a, std::tuple<float, uint64_t> b) {
             return std::get<1>(a) < std::get<1>(b);
           });
+      size_t before_size = pts.size();
       pts.erase(std::unique(pts.begin(), pts.end(),
                             [](std::tuple<float, uint64_t> a,
                                std::tuple<float, uint64_t> b) {
                               return std::get<1>(a) == std::get<1>(b);
                             }),
                 pts.end());
+      CHECK_EQ(pts.size(), before_size);
 
       std::sort(
           pts.begin(), pts.end(),
@@ -927,8 +980,7 @@
     const std::vector<std::vector<QuadBoundaryPoint>>
         cuda_point_sorted_grouped_points = SortPoints(cuda_grouped_points);
     const std::vector<std::vector<uint64_t>> april_sorted_grouped_points =
-        SortAprilroboticsPoints(
-            SortAprilroboticsPointSizes(april_grouped_points));
+        SortAprilroboticsPoints(april_grouped_points);
 
     CHECK_EQ(april_sorted_grouped_points.size(),
              cuda_point_sorted_grouped_points.size());
@@ -952,8 +1004,7 @@
   // Tests that the extents and dot products match the cuda versions.
   std::pair<size_t, std::vector<IndexPoint>> CheckCudaExtents(
       const std::vector<std::vector<QuadBoundaryPoint>> &cuda_grouped_points,
-      const std::vector<MinMaxExtents> &extents_cuda,
-      const std::vector<float> &reduced_dot_blobs_pair_cuda) const {
+      const std::vector<MinMaxExtents> &extents_cuda) const {
     std::vector<IndexPoint> selected_blobs;
     const size_t max_april_tag_perimeter = 2 * (width_ + height_);
     size_t selected_quads = 0;
@@ -962,7 +1013,6 @@
       size_t i = 0;
       size_t starting_offset = 0;
       CHECK_EQ(cuda_grouped_points.size(), extents_cuda.size());
-      CHECK_EQ(extents_cuda.size(), reduced_dot_blobs_pair_cuda.size());
       for (const std::vector<QuadBoundaryPoint> &points : cuda_grouped_points) {
         size_t min_x, min_y, max_x, max_y;
         min_x = max_x = points[0].x();
@@ -994,33 +1044,27 @@
         bool good_blob_size =
             points.size() >= 24 && points.size() <= max_april_tag_perimeter &&
             points.size() >=
-                static_cast<size_t>(tag_detector_->qtp.min_cluster_pixels);
+                static_cast<size_t>(tag_detector_->qtp.min_cluster_pixels) &&
+            static_cast<int>((max_x - min_x) * (max_y - min_y)) >=
+                min_tag_width_;
 
-        if (good_blob_size) {
-          for (size_t j = 0; j < points.size(); ++j) {
-            dot += (static_cast<float>(points[j].x()) - cx) * points[j].gx() +
-                   (static_cast<float>(points[j].y()) - cy) * points[j].gy();
+        for (size_t j = 0; j < points.size(); ++j) {
+          dot += (static_cast<float>(points[j].x()) - cx) * points[j].gx() +
+                 (static_cast<float>(points[j].y()) - cy) * points[j].gy();
 
-            // Make sure our blob finder agrees.
-            CHECK_EQ(i, finder.FindBlobIndex(starting_offset + j));
-          }
-          // Test that the summed dot product is right.
-          CHECK_LT(
-              std::abs(reduced_dot_blobs_pair_cuda[i] - dot) / std::abs(dot),
-              1e-4)
-              << ": for point " << i << ", cuda -> "
-              << reduced_dot_blobs_pair_cuda[i] << ", C++ -> " << dot;
-        } else {
-          CHECK_EQ(reduced_dot_blobs_pair_cuda[i], 0.0f)
-              << ": for point " << i << ", cuda -> "
-              << reduced_dot_blobs_pair_cuda[i] << ", C++ -> " << dot;
+          // Make sure our blob finder agrees.
+          CHECK_EQ(i, finder.FindBlobIndex(starting_offset + j));
         }
 
+        // Test that the summed dot product is right.
+        CHECK_LT(std::abs(extents.dot() - dot) / std::abs(dot), 1e-2)
+            << ": for point " << i << ", cuda -> " << extents.dot()
+            << ", C++ -> " << dot;
+
         const bool quad_reversed_border = dot < 0;
 
-        VLOG(1) << "For point " << i << ", cuda -> "
-                << reduced_dot_blobs_pair_cuda[i] << ", C++ -> " << dot
-                << " size " << points.size() << " border "
+        VLOG(1) << "For point " << i << ", cuda -> " << extents.dot()
+                << ", C++ -> " << dot << " size " << points.size() << " border "
                 << (!(!reversed_border_ && quad_reversed_border) &&
                     !(!normal_border_ && !quad_reversed_border))
                 << " area: "
@@ -1167,13 +1211,14 @@
           ++missmatched_points;
           ++missmatched_runs;
           // We shouldn't see a lot of points in a row which don't match.
-          VLOG(1) << "Missmatched point in blob "
+          VLOG(0) << "Missmatched point in blob "
                   << cuda_grouped_blob[0].blob_index() << ", point " << j
                   << " (" << cuda_grouped_blob[j].x() << ", "
-                  << cuda_grouped_blob[j].y() << ") vs ("
+                  << cuda_grouped_blob[j].y() << "), theta "
+                  << cuda_grouped_blob[j].theta() << " vs ("
                   << slope_sorted_points[j].x() << ", "
-                  << slope_sorted_points[j].y() << ")"
-                  << " in size " << cuda_grouped_points[i].size();
+                  << slope_sorted_points[j].y() << "), in size "
+                  << cuda_grouped_points[i].size();
           CHECK_LE(missmatched_runs, 4u);
         } else {
           missmatched_runs = 0;
@@ -1183,7 +1228,540 @@
 
     // Or a lot of points overall.  The slope algo has duplicate points, and
     // is occasionally wrong.
-    CHECK_LE(missmatched_points, 50u);
+    CHECK_LE(missmatched_points, 25u);
+  }
+
+  void CheckCudaFilteredExtents(
+      const std::vector<cub::KeyValuePair<long, MinMaxExtents>>
+          &selected_extents_cuda,
+      const std::vector<IndexPoint> &sorted_selected_blobs_cuda) {
+    size_t start = 0;
+    size_t sorted_point_index = 0;
+    for (size_t i = 0; i < selected_extents_cuda.size(); ++i) {
+      VLOG(1) << "Extent " << i << " started at "
+              << selected_extents_cuda[i].value.starting_offset
+              << " with count " << selected_extents_cuda[i].value.count;
+      CHECK_EQ(selected_extents_cuda[i].value.starting_offset, start)
+          << " for extent " << i;
+      size_t found_blobs = 0;
+      CHECK_EQ(selected_extents_cuda[i].value.starting_offset,
+               sorted_point_index);
+      while (sorted_selected_blobs_cuda[sorted_point_index].blob_index() == i) {
+        ++found_blobs;
+        ++sorted_point_index;
+      }
+      CHECK_EQ(found_blobs, selected_extents_cuda[i].value.count);
+      start += selected_extents_cuda[i].value.count;
+    }
+  }
+
+  void CheckErrors(const std::vector<IndexPoint> &sorted_selected_blobs_cuda,
+                   const std::vector<LineFitPoint> &line_fit_points_cuda,
+                   const std::vector<double> &errors_device,
+                   const std::vector<double> &filtered_errors_device,
+                   const std::vector<Peak> &peaks_device) {
+    std::vector<std::vector<IndexPoint>> grouped_points =
+        CudaGroupedPoints(sorted_selected_blobs_cuda);
+    image_u8_t quad_im = ToImageu8t(decimated_cuda_);
+    size_t accumulated_size = 0u;
+    size_t blob_index = 0u;
+    size_t bad_errors = 0u;
+    size_t summed_cuda_pts = 0;
+    for (const std::vector<IndexPoint> &group : grouped_points) {
+      std::vector<double> errs;
+      errs.resize(group.size(), 0.0);
+      const size_t ksz = std::min<size_t>(20u, group.size() / 12);
+
+      zarray_t *cluster = zarray_create(sizeof(struct pt));
+
+      for (size_t i = 0; i < group.size(); i++) {
+        pt p{
+            .x = static_cast<uint16_t>(group[i].x()),
+            .y = static_cast<uint16_t>(group[i].y()),
+        };
+        zarray_add(cluster, &p);
+      }
+
+      CHECK_EQ(static_cast<size_t>(zarray_size(cluster)), group.size());
+
+      struct line_fit_pt *lfps = compute_lfps(group.size(), cluster, &quad_im);
+
+      VLOG(1) << "Inspecting blob of size " << group.size() << " global start "
+              << accumulated_size;
+      for (size_t i = 0; i < group.size(); i++) {
+        if (group[i].blob_index() == (size_t)FLAGS_debug_blob_index) {
+          LOG(INFO) << "For idx " << i << " global " << accumulated_size + i
+                    << "(" << group[i].x() << ", " << group[i].y()
+                    << "), cuda: " << line_fit_points_cuda[accumulated_size + i]
+                    << " aprilrobotics " << lfps[i];
+        }
+        CHECK_EQ(line_fit_points_cuda[accumulated_size + i].Mx / 2.0,
+                 lfps[i].Mx)
+            << ": for blob index " << group[i].blob_index();
+        CHECK_EQ(line_fit_points_cuda[accumulated_size + i].My / 2.0,
+                 lfps[i].My)
+            << ": for blob index " << group[i].blob_index();
+        CHECK_EQ(line_fit_points_cuda[accumulated_size + i].Mxx / 4.0,
+                 lfps[i].Mxx)
+            << ": for blob index " << group[i].blob_index();
+        CHECK_EQ(line_fit_points_cuda[accumulated_size + i].Mxy / 4.0,
+                 lfps[i].Mxy)
+            << ": for blob index " << group[i].blob_index();
+        CHECK_EQ(line_fit_points_cuda[accumulated_size + i].Myy / 4.0,
+                 lfps[i].Myy)
+            << ": for blob index " << group[i].blob_index();
+        CHECK_EQ(line_fit_points_cuda[accumulated_size + i].W, lfps[i].W)
+            << ": for blob index " << group[i].blob_index();
+      }
+
+      for (size_t i = 0; i < group.size(); i++) {
+        if (group[i].blob_index() == (size_t)FLAGS_debug_blob_index) {
+          LOG(INFO) << "  Cuda error[" << i << "] -> "
+                    << errors_device[accumulated_size + i] << ", filtered "
+                    << filtered_errors_device[i + accumulated_size];
+        }
+      }
+
+      for (size_t i = 0; i < group.size(); i++) {
+        const size_t i0 = (i + group.size() - ksz) % group.size();
+        const size_t i1 = (i + ksz) % group.size();
+        const size_t gi0 = accumulated_size + i0;
+        const size_t gi1 = accumulated_size + i1;
+
+        fit_line(lfps, group.size(), i0, i1, NULL, &errs[i], NULL);
+
+        if (std::abs(errs[i] - errors_device[accumulated_size + i]) >=
+            0.001 * std::max<double>(1.0, errs[i])) {
+          VLOG(1) << "i0   aprilrobotics " << lfps[i0];
+          VLOG(1) << "i0            cuda " << line_fit_points_cuda[gi0];
+          VLOG(1) << " dMx: "
+                  << (lfps[i0].Mx * 2 - line_fit_points_cuda[gi0].Mx);
+          VLOG(1) << " dMy: "
+                  << (lfps[i0].My * 2 - line_fit_points_cuda[gi0].My);
+          VLOG(1) << " dMxx: "
+                  << (lfps[i0].Mxx * 4 - line_fit_points_cuda[gi0].Mxx);
+          VLOG(1) << " dMxy: "
+                  << (lfps[i0].Mxy * 4 - line_fit_points_cuda[gi0].Mxy);
+          VLOG(1) << " dMyy: "
+                  << (lfps[i0].Myy * 4 - line_fit_points_cuda[gi0].Myy);
+          VLOG(1) << " dW: " << (lfps[i0].W - line_fit_points_cuda[gi0].W);
+          VLOG(1) << "i1   aprilrobotics " << lfps[i1];
+          VLOG(1) << "i1            cuda " << line_fit_points_cuda[gi1];
+          VLOG(1) << " dMx: "
+                  << (lfps[i1].Mx * 2 - line_fit_points_cuda[gi1].Mx);
+          VLOG(1) << " dMy: "
+                  << (lfps[i1].My * 2 - line_fit_points_cuda[gi1].My);
+          VLOG(1) << " dMxx: "
+                  << (lfps[i1].Mxx * 4 - line_fit_points_cuda[gi1].Mxx);
+          VLOG(1) << " dMxy: "
+                  << (lfps[i1].Mxy * 4 - line_fit_points_cuda[gi1].Mxy);
+          VLOG(1) << " dMyy: "
+                  << (lfps[i1].Myy * 4 - line_fit_points_cuda[gi1].Myy);
+          VLOG(1) << " dW: " << (lfps[i1].W - line_fit_points_cuda[gi1].W);
+          VLOG(1) << "derror: " << errs[i] << " - "
+                  << errors_device[accumulated_size + i] << " = "
+                  << errs[i] - errors_device[accumulated_size + i];
+
+          ++bad_errors;
+        }
+      }
+
+      std::vector<float> filter_coefficients;
+      {
+        // Stolen directly from aprilrobotics to test.
+
+        double sigma = 1;  // was 3
+
+        // cutoff = exp(-j*j/(2*sigma*sigma));
+        // log(cutoff) = -j*j / (2*sigma*sigma)
+        // log(cutoff)*2*sigma*sigma = -j*j;
+
+        // how big a filter should we use? We make our kernel big
+        // enough such that we represent any values larger than
+        // 'cutoff'.
+
+        // XXX Tunable (though not super useful to change)
+        double cutoff = 0.05;
+        int fsz = sqrt(-log(cutoff) * 2 * sigma * sigma) + 1;
+        fsz = 2 * fsz + 1;
+
+        // For default values of cutoff = 0.05, sigma = 3,
+        // we have fsz = 17.
+
+        for (int i = 0; i < fsz; i++) {
+          int j = i - fsz / 2;
+          filter_coefficients.push_back(exp(-j * j / (2 * sigma * sigma)));
+          CHECK_EQ(filter_coefficients[i], FilterCoefficients()[i]);
+        }
+      }
+
+      std::vector<double> filtered_errors;
+      filtered_errors.resize(group.size(), 0.0);
+
+      for (size_t i = 0; i < group.size(); i++) {
+        double accumulated_cuda = 0;
+        double accumulated = 0;
+        for (int filter_coefficient_index = 0;
+             filter_coefficient_index <
+             static_cast<int>(filter_coefficients.size());
+             filter_coefficient_index++) {
+          const double e =
+              errs[(i + filter_coefficient_index -
+                    filter_coefficients.size() / 2 + group.size()) %
+                   group.size()];
+          accumulated += e * filter_coefficients[filter_coefficient_index];
+
+          const double e_cuda =
+              errors_device[(i + filter_coefficient_index -
+                             filter_coefficients.size() / 2 + group.size()) %
+                                group.size() +
+                            accumulated_size];
+          accumulated_cuda +=
+              e_cuda * filter_coefficients[filter_coefficient_index];
+        }
+        filtered_errors[i] = accumulated;
+
+        // Make sure the filter math works.  This can be a lot tighter since the
+        // error calc appears to be less accurate.
+        CHECK_LT(std::abs(accumulated_cuda -
+                          filtered_errors_device[accumulated_size + i]),
+                 1e-3)
+            << " Failed to match error at blob " << blob_index << " index " << i
+            << " global index " << accumulated_size + i
+            << " for a blob of size " << group.size() << " expected "
+            << filtered_errors[i] << " got "
+            << filtered_errors_device[accumulated_size + i];
+
+        // Make sure it hasn't walked too far off from the actual error.
+        /*
+        CHECK_LT(std::abs(filtered_errors[i] -
+                          filtered_errors_device[accumulated_size + i]),
+                 1e-1)
+            << " Failed to match error at blob " << blob_index << " index " << i
+            << " global index " << accumulated_size + i
+            << " for a blob of size " << group.size() << " expected "
+            << filtered_errors[i] << " got "
+            << filtered_errors_device[accumulated_size + i];
+            */
+      }
+
+      // Now, check the peaks.
+      size_t num_peaks_cuda = 0;
+      size_t num_peaks_april = 0;
+      for (size_t i = 0; i < group.size(); i++) {
+        double before_aprilrobotics =
+            filtered_errors[(i + group.size() - 1) % group.size()];
+        double after_aprilrobotics = filtered_errors[(i + 1) % group.size()];
+        double us_aprilrobotics = filtered_errors[i];
+
+        double before_cuda =
+            filtered_errors_device[(i + group.size() - 1) % group.size() +
+                                   accumulated_size];
+        double after_cuda =
+            filtered_errors_device[(i + 1) % group.size() + accumulated_size];
+        double us_cuda = filtered_errors_device[i + accumulated_size];
+
+        const bool is_peak_aprilrobotics =
+            us_aprilrobotics > before_aprilrobotics &&
+            us_aprilrobotics > after_aprilrobotics;
+        const bool is_peak_cuda = us_cuda > before_cuda && us_cuda > after_cuda;
+
+        if (is_peak_aprilrobotics) {
+          ++num_peaks_april;
+        }
+        // We don't agree...  But it is on little stupid stuff.
+        /*CHECK_EQ(is_peak_aprilrobotics, peaks_device[accumulated_size + i])
+            << " Failed to match peak at blob " << blob_index << " index " << i
+            << " global index " << accumulated_size + i
+            << " for a blob of size " << group.size() << " compared "
+            << before_aprilrobotics << " vs " << us_aprilrobotics << " vs "
+            << after_aprilrobotics;*/
+        CHECK_EQ(is_peak_cuda, peaks_device[accumulated_size + i].blob_index !=
+                                   Peak::kNoPeak())
+            << " Failed to match peak at blob " << blob_index << " index " << i
+            << " global index " << accumulated_size + i
+            << " for a blob of size " << group.size() << " compared "
+            << before_cuda << " vs " << us_cuda << " vs " << after_cuda;
+        CHECK_EQ(peaks_device[accumulated_size + i].filtered_point_index,
+                 accumulated_size + i);
+        CHECK_NEAR(peaks_device[accumulated_size + i].error,
+                   -filtered_errors_device[accumulated_size + i], 1e-3);
+        if (is_peak_cuda) {
+          CHECK_EQ(peaks_device[accumulated_size + i].blob_index,
+                   group[0].blob_index())
+              << " Failed to match peak at blob " << blob_index << " index "
+              << i << " global index " << accumulated_size + i
+              << " for a blob of size " << group.size() << " compared "
+              << before_cuda << " vs " << us_cuda << " vs " << after_cuda;
+        }
+        if (is_peak_cuda) {
+          ++num_peaks_cuda;
+          ++summed_cuda_pts;
+        }
+      }
+
+      (void)num_peaks_cuda;
+      (void)num_peaks_april;
+      // CHECK_EQ(num_peaks_cuda, num_peaks_april);
+
+      zarray_destroy(cluster);
+      free(lfps);
+      accumulated_size += group.size();
+      ++blob_index;
+    }
+    LOG(INFO) << "Overall, found " << summed_cuda_pts << " peaks with "
+              << bad_errors << " bad errors.";
+    // CHECK_LT(bad_errors, 60u);
+  }
+
+  // Orders the clusters reported by AprilRobotics to match the inbound cuda
+  // cluster order.
+  zarray_t *OrderAprilroboticsLikeCuda(
+      image_u8_t *thresholded_im, unionfind_t *uf,
+      const std::vector<std::vector<QuadBoundaryPoint>> &cuda_grouped_points)
+      const {
+    // Step 1, label both the cuda and april robotics points with their original
+    // index. Step 2, sort both, retaining the indices. Step 3, reorder the
+    // april robotics points to match the cuda points using the indices.
+    zarray_t *clusters =
+        gradient_clusters(tag_detector_, thresholded_im, thresholded_im->width,
+                          thresholded_im->height, thresholded_im->stride, uf);
+
+    // Reorder a copy of april robotics clusters to our format.
+    struct AprilClusterIndexPair {
+      size_t index;
+      std::vector<uint64_t> points;
+    };
+    std::vector<AprilClusterIndexPair> april_points;
+
+    for (int i = 0; i < zarray_size(clusters); i++) {
+      zarray_t *cluster;
+      zarray_get(clusters, i, &cluster);
+
+      AprilClusterIndexPair points;
+      points.index = i;
+
+      for (int j = 0; j < zarray_size(cluster); j++) {
+        struct pt *p;
+        zarray_get_volatile(cluster, j, &p);
+
+        points.points.push_back(p->x + p->y * width_);
+      }
+
+      std::sort(points.points.begin(), points.points.end());
+      points.points.erase(
+          std::unique(points.points.begin(), points.points.end()),
+          points.points.end());
+      april_points.emplace_back(std::move(points));
+    }
+
+    // And sort the blobs now.
+    std::sort(april_points.begin(), april_points.end(), [](auto &x, auto &y) {
+      if (x.points.size() == y.points.size()) {
+        for (size_t j = 0; j < x.points.size(); ++j) {
+          if (x.points[j] == y.points[j]) {
+            continue;
+          }
+          return x.points[j] < y.points[j];
+        }
+        LOG(FATAL) << "Equal";
+      }
+      return x.points.size() < y.points.size();
+    });
+
+    struct CudaClusterIndexPair {
+      size_t index;
+      std::vector<QuadBoundaryPoint> points;
+    };
+
+    // Do the same for CUDA.
+    std::vector<CudaClusterIndexPair> cuda_points;
+    for (size_t i = 0; i < cuda_grouped_points.size(); ++i) {
+      CudaClusterIndexPair points;
+      points.points = cuda_grouped_points[i];
+      points.index = i;
+
+      std::sort(points.points.begin(), points.points.end(),
+                [this](const QuadBoundaryPoint &a, const QuadBoundaryPoint &b) {
+                  CHECK_EQ(a.rep01(), b.rep01());
+                  return a.x() + a.y() * width_ < b.x() + b.y() * width_;
+                });
+      cuda_points.emplace_back(std::move(points));
+    }
+
+    // And sort the blobs.
+    std::sort(cuda_points.begin(), cuda_points.end(), [this](auto &x, auto &y) {
+      if (x.points.size() == y.points.size()) {
+        for (size_t j = 0; j < x.points.size(); ++j) {
+          if (x.points[j].x() + x.points[j].y() * width_ ==
+              y.points[j].x() + y.points[j].y() * width_) {
+            continue;
+          }
+          return x.points[j].x() + x.points[j].y() * width_ <
+                 y.points[j].x() + y.points[j].y() * width_;
+        }
+        LOG(FATAL) << "Equal";
+      }
+      return x.points.size() < y.points.size();
+    });
+
+    // OK, we now have the cuda points in the same order as the aprilrobotics
+    // points.  First CHECK that they match, just in case...
+    std::vector<zarray_t *> sorted_result;
+    sorted_result.resize(cuda_points.size());
+
+    CHECK_EQ(april_points.size(), cuda_points.size());
+    for (size_t i = 0; i < april_points.size(); ++i) {
+      CHECK_EQ(april_points[i].points.size(), cuda_points[i].points.size());
+      for (size_t j = 0; j < april_points[i].points.size(); ++j) {
+        CHECK_EQ(april_points[i].points[j],
+                 cuda_points[i].points[j].x() +
+                     cuda_points[i].points[j].y() * width_)
+            << ": " << i << " " << j;
+      }
+
+      // And, while we are iterating, make a vector with the clusters in the
+      // right order.
+      zarray_t *cluster;
+      zarray_get(clusters, april_points[i].index, &cluster);
+      sorted_result[cuda_points[i].index] = cluster;
+    }
+
+    // Finally, put it back in the clusters.
+    for (size_t i = 0; i < april_points.size(); ++i) {
+      zarray_set(clusters, i, &sorted_result[i], nullptr);
+    }
+
+    return clusters;
+  }
+
+  void CheckQuads(const zarray_t *clusters,
+                  std::vector<IndexPoint> sorted_selected_blobs_cuda,
+                  const std::vector<QuadCorners> &fit_quads) {
+    std::vector<std::vector<IndexPoint>> sorted_blobs;
+    for (auto blob : CudaGroupedPoints(sorted_selected_blobs_cuda)) {
+      while (blob[0].blob_index() > sorted_blobs.size()) {
+        sorted_blobs.emplace_back();
+      }
+      sorted_blobs.emplace_back(std::move(blob));
+    }
+
+    image_u8_t quad_im = ToImageu8t(decimated_cuda_);
+
+    std::vector<bool> aprilrobotics_valid;
+    std::vector<struct quad> aprilrobotics_quad;
+    aprilrobotics_valid.resize(zarray_size(clusters));
+    aprilrobotics_quad.resize(zarray_size(clusters));
+
+    for (int i = 0; i < zarray_size(clusters); ++i) {
+      struct quad quad_result;
+
+      zarray_t *cluster;
+      zarray_get(clusters, i, &cluster);
+
+      if (i == FLAGS_debug_blob_index) {
+        LOG(INFO) << "cuda points for blob " << i << " are";
+        for (size_t j = 0; j < sorted_blobs[i].size(); ++j) {
+          LOG(INFO) << "  blob[" << j << "]: (" << sorted_blobs[i][j].x()
+                    << ", " << sorted_blobs[i][j].y() << ")";
+        }
+      }
+      VLOG(1) << "Going to fit quad " << i << " of size "
+              << zarray_size(cluster);
+      int valid_blob =
+          fit_quad(tag_detector_, &quad_im, cluster, &quad_result,
+                   min_tag_width_, normal_border_, reversed_border_);
+
+      auto quad_iterator = std::find_if(fit_quads.begin(), fit_quads.end(),
+                                        [&](const QuadCorners corner) {
+                                          return (int)corner.blob_index == i;
+                                        });
+
+      CHECK_EQ(quad_iterator != fit_quads.end(), valid_blob != 0)
+          << ": Missmatch on quad " << i;
+
+      if (!valid_blob) {
+        continue;
+      }
+
+      // Apply the center adjustment algorithm to the ground truth so we compare
+      // apples to apples.
+      gpu_detector_.AdjustCenter(quad_result.p);
+
+      QuadCorners cuda_corner = *quad_iterator;
+
+      for (size_t point = 0; point < 4; ++point) {
+        CHECK_NEAR(quad_result.p[point][0], cuda_corner.corners[point][0],
+                   1e-3);
+        CHECK_NEAR(quad_result.p[point][1], cuda_corner.corners[point][1],
+                   1e-3);
+      }
+    }
+  }
+
+  void CheckDetections(zarray_t *aprilrobotics_detections,
+                       const zarray_t *gpu_detections) {
+    CHECK_EQ(zarray_size(aprilrobotics_detections),
+             zarray_size(gpu_detections));
+    LOG(INFO) << "Found " << zarray_size(gpu_detections) << " tags";
+
+    for (int i = 0; i < zarray_size(aprilrobotics_detections); ++i) {
+      const apriltag_detection_t *aprilrobotics_detection;
+      const apriltag_detection_t *gpu_detection;
+
+      zarray_get(aprilrobotics_detections, i, &aprilrobotics_detection);
+      zarray_get(gpu_detections, i, &gpu_detection);
+
+      bool valid = gpu_detection->decision_margin > FLAGS_min_decision_margin;
+
+      LOG(INFO) << "Found GPU " << (valid ? "valid" : "invalid")
+                << " tag number " << gpu_detection->id
+                << " hamming: " << gpu_detection->hamming
+                << " margin: " << gpu_detection->decision_margin;
+      LOG(INFO) << "Found CPU " << (valid ? "valid" : "invalid")
+                << " tag number " << aprilrobotics_detection->id
+                << " hamming: " << aprilrobotics_detection->hamming
+                << " margin: " << aprilrobotics_detection->decision_margin;
+    }
+
+    for (int i = 0; i < zarray_size(aprilrobotics_detections); ++i) {
+      const apriltag_detection_t *aprilrobotics_detection;
+      const apriltag_detection_t *gpu_detection;
+
+      zarray_get(aprilrobotics_detections, i, &aprilrobotics_detection);
+      zarray_get(gpu_detections, i, &gpu_detection);
+
+      const bool valid =
+          gpu_detection->decision_margin > FLAGS_min_decision_margin;
+
+      // TODO(austin): Crank down the thresholds and figure out why these
+      // deviate.  It should be the same function for both at this point.
+      const double threshold = valid ? 2e-3 : 1e-1;
+
+      CHECK_EQ(aprilrobotics_detection->id, gpu_detection->id);
+      CHECK_EQ(aprilrobotics_detection->hamming, gpu_detection->hamming);
+      EXPECT_NEAR(aprilrobotics_detection->c[0], gpu_detection->c[0],
+                  threshold);
+      EXPECT_NEAR(aprilrobotics_detection->c[1], gpu_detection->c[1],
+                  threshold);
+
+      for (int j = 0; j < 4; ++j) {
+        for (int k = 0; k < 2; ++k) {
+          EXPECT_NEAR(aprilrobotics_detection->p[j][k], gpu_detection->p[j][k],
+                      threshold);
+        }
+      }
+
+      CHECK_EQ(aprilrobotics_detection->H->nrows, gpu_detection->H->nrows);
+      CHECK_EQ(aprilrobotics_detection->H->ncols, gpu_detection->H->ncols);
+
+      for (size_t j = 0; j < gpu_detection->H->ncols; ++j) {
+        for (size_t k = 0; k < gpu_detection->H->nrows; ++k) {
+          EXPECT_NEAR(matd_get(gpu_detection->H, k, j),
+                      matd_get(aprilrobotics_detection->H, k, j), threshold);
+        }
+      }
+    }
   }
 
   // Checks that the GPU and CPU algorithms match.
@@ -1250,8 +1828,8 @@
     // Test that the extents are reasonable.
     size_t selected_quads;
     std::vector<IndexPoint> selected_blobs;
-    std::tie(selected_quads, selected_blobs) = CheckCudaExtents(
-        cuda_grouped_points, extents_cuda_, reduced_dot_blobs_pair_cuda_);
+    std::tie(selected_quads, selected_blobs) =
+        CheckCudaExtents(cuda_grouped_points, extents_cuda_);
 
     // And that the filtered list is reasonable.
     CheckFilteredCudaPoints(selected_blobs, selected_blobs_cuda_);
@@ -1261,10 +1839,30 @@
                                   selected_quads, selected_blobs,
                                   sorted_selected_blobs_cuda_);
 
-    // TODO(austin): Check the selected extents is right.
+    CheckCudaFilteredExtents(selected_extents_cuda_,
+                             sorted_selected_blobs_cuda_);
+
+    CheckErrors(sorted_selected_blobs_cuda_, line_fit_points_cuda_,
+                errors_device_, filtered_errors_device_, peaks_device_);
+
+    zarray_t *april_clusters =
+        OrderAprilroboticsLikeCuda(thresholded_im, uf, cuda_grouped_points);
+
+    CheckQuads(april_clusters, sorted_selected_blobs_cuda_, fit_quads_);
+
+    const zarray_t *gpu_detections = gpu_detector_.Detections();
+    CheckDetections(aprilrobotics_detections_, gpu_detections);
+
     LOG(INFO) << "Found slope sorted count: "
               << sorted_union_marker_pair_.size();
 
+    for (int i = 0; i < zarray_size(april_clusters); i++) {
+      zarray_t *cluster;
+      zarray_get(april_clusters, i, &cluster);
+      zarray_destroy(cluster);
+    }
+
+    zarray_destroy(april_clusters);
     unionfind_destroy(uf);
     image_u8_destroy(quad_im);
     image_u8_destroy(thresholded_im);
@@ -1442,14 +2040,22 @@
   std::vector<QuadBoundaryPoint> sorted_union_marker_pair_;
   std::vector<uint32_t> quad_length_;
   std::vector<MinMaxExtents> extents_cuda_;
-  std::vector<float> reduced_dot_blobs_pair_cuda_;
+  std::vector<cub::KeyValuePair<long, MinMaxExtents>> selected_extents_cuda_;
   std::vector<IndexPoint> selected_blobs_cuda_;
   std::vector<IndexPoint> sorted_selected_blobs_cuda_;
+  std::vector<LineFitPoint> line_fit_points_cuda_;
+
+  std::vector<double> errors_device_;
+  std::vector<double> filtered_errors_device_;
+  std::vector<Peak> peaks_device_;
   int num_compressed_union_marker_pair_ = 0;
   int num_quads_ = 0;
+  std::vector<QuadCorners> fit_quads_;
 
   GpuDetector gpu_detector_;
 
+  zarray_t *aprilrobotics_detections_ = nullptr;
+
   size_t width_;
   size_t height_;
 
@@ -1463,7 +2069,7 @@
   aos::FlatbufferVector<frc971::vision::CameraImage> ReadImage(
       std::string_view path) {
     return aos::FileToFlatbuffer<frc971::vision::CameraImage>(
-        "../apriltag_test_bfbs_images/" + std::string(path));
+        "../" + std::string(path));
   }
 
   cv::Mat ToMat(const frc971::vision::CameraImage *image) {
@@ -1474,7 +2080,7 @@
 };
 
 TEST_F(AprilDetectionTest, ImageRepeat) {
-  auto image = ReadImage("bfbs-capture-2013-01-18_08-54-09.501047728.bfbs");
+  auto image = ReadImage("orin_image_apriltag/file/orin_image_apriltag.bfbs");
 
   LOG(INFO) << "Image is: " << image.message().cols() << " x "
             << image.message().rows();
@@ -1484,14 +2090,14 @@
 
   const cv::Mat color_image = ToMat(&image.message());
 
-  for (size_t i = 0; i < 10; ++i) {
+  for (size_t i = 0; i < 4; ++i) {
     LOG(INFO) << "Attempt " << i;
     cuda_detector.DetectGPU(color_image.clone());
     cuda_detector.DetectCPU(color_image.clone());
-    cuda_detector.Check(color_image.clone());
-    if (FLAGS_debug) {
-      cuda_detector.WriteDebug(color_image);
-    }
+  }
+  cuda_detector.Check(color_image.clone());
+  if (FLAGS_debug) {
+    cuda_detector.WriteDebug(color_image);
   }
 }
 
@@ -1522,16 +2128,91 @@
 // Tests that both algorithms agree on a bunch of pixels.
 INSTANTIATE_TEST_SUITE_P(
     CapturedImages, SingleAprilDetectionTest,
-    ::testing::Values("bfbs-capture-2013-01-18_08-54-16.869057537.bfbs",
-                      "bfbs-capture-2013-01-18_08-54-09.501047728.bfbs",
-                      "bfbs-capture-2013-01-18_08-51-24.861065764.bfbs",
-                      "bfbs-capture-2013-01-18_08-52-01.846912552.bfbs",
-                      "bfbs-capture-2013-01-18_08-52-33.462848049.bfbs",
-                      "bfbs-capture-2013-01-18_08-54-24.931661979.bfbs",
-                      "bfbs-capture-2013-01-18_09-29-16.806073486.bfbs",
-                      "bfbs-capture-2013-01-18_09-33-00.993756514.bfbs",
-                      "bfbs-capture-2013-01-18_08-57-00.171120695.bfbs",
-                      "bfbs-capture-2013-01-18_08-57-17.858752817.bfbs",
-                      "bfbs-capture-2013-01-18_08-57-08.096597542.bfbs"));
+    ::testing::Values(
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-54-16.869057537.bfbs",
+        "orin_image_apriltag/file/orin_image_apriltag.bfbs",
+        "orin_large_image_apriltag/file/orin_large_gs_apriltag.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-54-09.501047728.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-51-24.861065764.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-52-01.846912552.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-52-33.462848049.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-54-24.931661979.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_09-29-16.806073486.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_09-33-00.993756514.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-57-00.171120695.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-57-17.858752817.bfbs",
+        "apriltag_test_bfbs_images/"
+        "bfbs-capture-2013-01-18_08-57-08.096597542.bfbs"));
+
+// Tests that the filter coefficients produced by FilterCoefficients() match the
+// implementation in AprilRobotics.
+TEST(FilterTest, Matches) {
+  constexpr double sigma = 1;  // was 3
+
+  // cutoff = exp(-j*j/(2*sigma*sigma));
+  // log(cutoff) = -j*j / (2*sigma*sigma)
+  // log(cutoff)*2*sigma*sigma = -j*j;
+
+  // how big a filter should we use? We make our kernel big
+  // enough such that we represent any values larger than
+  // 'cutoff'.
+
+  // XXX Tunable (though not super useful to change)
+  constexpr double cutoff = 0.05;
+  int fsz = sqrt(-log(cutoff) * 2 * sigma * sigma) + 1;
+  fsz = 2 * fsz + 1;
+
+  // For default values of cutoff = 0.05, sigma = 3,
+  // we have fsz = 17.
+  std::vector<float> f;
+
+  for (int i = 0; i < fsz; i++) {
+    const int j = i - fsz / 2;
+    f.emplace_back(exp(-j * j / (2 * sigma * sigma)));
+  }
+
+  constexpr std::array<float, 7> coefficients = FilterCoefficients();
+  ASSERT_EQ(coefficients.size(), f.size());
+
+  for (size_t i = 0; i < f.size(); ++i) {
+    EXPECT_EQ(coefficients[i], f[i]);
+  }
+}
+
+// Tests that Unrank returns the right value for each input.
+TEST(FilterTest, Unrank) {
+  const int nmaxima = 10;
+  int overall_count = 0;
+  for (int m0 = 0; m0 < nmaxima - 3; m0++) {
+    for (int m1 = m0 + 1; m1 < nmaxima - 2; m1++) {
+      for (int m2 = m1 + 1; m2 < nmaxima - 1; m2++) {
+        for (int m3 = m2 + 1; m3 < nmaxima; m3++) {
+          const std::tuple<int, int, int, int> unranked = Unrank(overall_count);
+
+          VLOG(1) << overall_count << " -> [" << m0 << ", " << m1 << ", " << m2
+                  << ", " << m3 << "] -> [" << std::get<0>(unranked) << ", "
+                  << std::get<1>(unranked) << ", " << std::get<2>(unranked)
+                  << ", " << std::get<3>(unranked) << "]";
+          ASSERT_EQ(m0, std::get<0>(unranked));
+          ASSERT_EQ(m1, std::get<1>(unranked));
+          ASSERT_EQ(m2, std::get<2>(unranked));
+          ASSERT_EQ(m3, std::get<3>(unranked));
+          ++overall_count;
+        }
+      }
+    }
+  }
+  EXPECT_EQ(overall_count, MaxRankedIndex());
+}
 
 }  // namespace frc971::apriltag::testing
diff --git a/frc971/orin/line_fit_filter.cc b/frc971/orin/line_fit_filter.cc
new file mode 100644
index 0000000..954976a
--- /dev/null
+++ b/frc971/orin/line_fit_filter.cc
@@ -0,0 +1,1235 @@
+#include "frc971/orin/line_fit_filter.h"
+
+#include <cub/block/block_reduce.cuh>
+#include <cub/warp/warp_merge_sort.cuh>
+#include <iomanip>
+
+#include "frc971/orin/cuda.h"
+
+// #define DEBUG_BLOB_NUMBER 401
+
+namespace frc971 {
+namespace apriltag {
+
+static_assert(sizeof(LineFitPoint) == 40, "Size of LineFitPoint changed");
+static_assert(sizeof(int4) == 16, "Size of int4 changed");
+
+constexpr size_t kPointsPerBlock = 256;
+// Number of errors to calculate before and after for the err filter and peak
+// finder.
+constexpr size_t kBeforeBuffer = 4;
+constexpr size_t kAfterBuffer = 4;
+constexpr size_t kErrorsBuffer = kBeforeBuffer + kAfterBuffer;
+
+__device__ double FitLineError(size_t N, int64_t Mx, int64_t My, int64_t Mxx,
+                               int64_t Myy, int64_t Mxy, int64_t W) {
+  int64_t Cxx = Mxx * W - Mx * Mx;
+  int64_t Cxy = Mxy * W - Mx * My;
+  int64_t Cyy = Myy * W - My * My;
+
+  // Pose it as an eigenvalue problem.
+  //
+  // TODO(austin): Are floats good enough?  Hard to tell what the "right answer"
+  // actually is...
+  float eig_small = ((Cxx + Cyy) - std::hypotf((Cxx - Cyy), 2 * Cxy)) /
+                    static_cast<float>(W * W * 8.0);
+  return N * eig_small;
+}
+
+struct TempStorage {
+  // Block of points that we pre-load.
+  LineFitPoint tmp_storage[kPointsPerBlock];
+
+  // The errors loaded.  The first kAfterBuffer errors are the indices (could be
+  // duplicated) immediately before the first blob.  The last kAfterBuffer
+  // errors are the indices immediately following the last blob.
+  double tmp_errs[kPointsPerBlock - 2 * kAfterBuffer];
+
+  // The errors which are from the start of the first blob before the beginning
+  // of the buffer.
+  double tmp_errs_before[kAfterBuffer];
+  // The errors which are from the end of the last blob after the end of the
+  // buffer.
+  double tmp_errs_after[kAfterBuffer];
+
+  // The filtered errors in the primary block.
+  double tmp_filtered_errs[kPointsPerBlock - 4 * kAfterBuffer + 2];
+
+  // The filtered error corresponding to the first error in tmp_errs_before.
+  double tmp_filtered_err_before;
+  // The filtered error corresponding to the last error in tmp_errs_after.
+  double tmp_filtered_err_after;
+
+  static_assert(sizeof(tmp_storage) == sizeof(LineFitPoint) * kPointsPerBlock,
+                "Storage changed size.");
+};
+
+template <size_t kThreads>
+class ErrorCalculator {
+ public:
+  __host__ __device__ ErrorCalculator(
+      const LineFitPoint *line_fit_points_device, size_t points,
+      const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+      TempStorage *storage)
+      : line_fit_points_device_(line_fit_points_device),
+        points_(points),
+        selected_extents_device_(selected_extents_device),
+        storage_(storage) {}
+
+  __host__ __device__ void Load() {
+    constexpr size_t kLoadSize = sizeof(int4);
+    static_assert(kLoadSize == 16, "kload");
+
+    // Global index in 128 bit loads.
+    const uint32_t global_index =
+        global_block_index_cache_start_ * sizeof(LineFitPoint) / kLoadSize +
+        threadIdx.x;
+
+    for (uint32_t i = 0; i < kPointsPerBlock * sizeof(LineFitPoint) / kLoadSize;
+         i += kThreads) {
+      // Load things in 128 bit increments strided across our threads in such a
+      // way our loads will coalesce nicely.
+      //
+      // Maybe overload a little bit past the end, but we over-allocate
+      // things on the host too.  Otherwise the end of the message can be
+      // corrupted.  For arrays which are multiples of kPointsPerBlock long,
+      // this will have no issues.
+      if (i + global_index <= points_ * sizeof(LineFitPoint) / kLoadSize) {
+        *(reinterpret_cast<int4 *>(storage_->tmp_storage) + i + threadIdx.x) =
+            *(reinterpret_cast<const int4 *>(line_fit_points_device_) +
+              global_index + i);
+      }
+    }
+  }
+
+  // Loads the blob extents for the provided buffer index. This index is from
+  // the front of the range we are responsible for computing.
+  __host__ __device__ void LoadExtents(size_t index) {
+    const size_t current_blob_index =
+        storage_->tmp_storage[index + (blockIdx.x == 0 ? 0 : kErrorsBuffer)]
+            .blob_index;
+    if (current_blob_index != blob_index_) {
+      starting_offset_ =
+          selected_extents_device_[current_blob_index].value.starting_offset;
+      count_ = selected_extents_device_[current_blob_index].value.count;
+      blob_index_ = current_blob_index;
+      ksz_ = std::min<int>(20, count_ / 12);
+    }
+  }
+
+  // Loads the error from our shared memory.  This follows the same model as
+  // BlobIndex and CalculateError below.  Call LoadExtents first to get the
+  // right index, and proceed from there.
+  __host__ __device__ double LoadError(ssize_t index) const {
+    // Index into the current blob.  Should be between 0 and count_.
+    const uint32_t positive_blob_index = (index + count_) % count_;
+
+    // This one is a normal one, it's global index is inside our range or
+    // contiguously just before our range.  Just do it.
+    //
+    // (note: kBeforeBuffer should be - on the right side of the >=, but due to
+    // unsigned arithmatic, it needs to be on the left.)
+    if (positive_blob_index + starting_offset_ + kBeforeBuffer >=
+        global_block_index_start()) {
+      // It is either inside our block, or just after our block.  That is all
+      // contiguous in memory and easy to calculate, so just return it.
+      if (positive_blob_index + starting_offset_ <
+          global_block_index_end() + kAfterBuffer) {
+        return storage_->tmp_errs[positive_blob_index + starting_offset_ +
+                                  kBeforeBuffer - global_block_index_start()];
+      }
+
+      // OK, this is past the back, but at the back end of the blob at the back
+      // end. These bytes are packed onto the back 4 objects of the buffer.
+      const size_t buffer_end = starting_offset_ + count_;
+      const size_t distance_from_end =
+          buffer_end - (positive_blob_index + starting_offset_);
+      return storage_->tmp_errs_after[4 - distance_from_end];
+    } else {
+      // We must be inside the before bits.
+      return storage_->tmp_errs_before[positive_blob_index];
+    }
+  }
+
+  // Loads the filtered error from the shared memory.  This follows the same
+  // model as BlobIndex and CalculateError below.  Call LoadExtents first to get
+  // the right index, and proceed from there.
+  __host__ __device__ double LoadFilteredError(ssize_t index) const {
+    const uint32_t positive_blob_index =
+        (index + global_block_index_start_ + count_ - starting_offset_) %
+        count_;
+    // This one is a normal one, it's global index is inside our range or
+    // contiguously just before our range.  Just do it.
+    if (positive_blob_index + starting_offset_ + 1 >=
+        global_block_index_start()) {
+      // It is either inside our block, or just after our block.  That is all
+      // contiguous in memory and easy to calculate, so just return it.
+      if (positive_blob_index + starting_offset_ <
+          global_block_index_end() + 1) {
+        return storage_
+            ->tmp_filtered_errs[positive_blob_index + starting_offset_ + 1 -
+                                global_block_index_start()];
+      }
+
+      // OK, this is past the back, but at the back end of the blob at the back
+      // end.  Return the on error we've got there.
+      return storage_->tmp_filtered_err_after;
+    } else {
+      // We must be inside the before bits.
+      return storage_->tmp_filtered_err_before;
+    }
+  }
+
+  // Gets the line fit point from either our local cache, or from main memory if
+  // it is too far away.
+  __host__ __device__ LineFitPoint GetPoint(size_t global_index,
+                                            bool print = false) const {
+    if (global_index >= global_block_index_cache_end_ ||
+        global_index < global_block_index_cache_start_) {
+      if (print) {
+#ifdef DEBUG_BLOB_NUMBER
+        printf(
+            "Block %d Thread %d   Loading global %d, relative %d from global\n",
+            blockIdx.x, threadIdx.x, (int)global_index,
+            (int)(global_index - starting_offset_));
+#endif
+      }
+      return line_fit_points_device_[global_index];
+    } else {
+      if (print) {
+#ifdef DEBUG_BLOB_NUMBER
+        printf(
+            "Block %d Thread %d   Loading global %d, relative %d from cache\n",
+            blockIdx.x, threadIdx.x, (int)global_index,
+            (int)(global_index - starting_offset_));
+#endif
+      }
+      return storage_
+          ->tmp_storage[global_index - global_block_index_cache_start_];
+    }
+  }
+
+  // Returns the blob index for the provided buffer index.  This is relative to
+  // the start of the range we are responsible for.
+  __forceinline__ __host__ __device__ size_t
+  BlobIndex(size_t buffer_index) const {
+    return global_block_index_start_ + buffer_index - starting_offset_;
+  }
+
+  // Calculates the line fit error centered on the provided blob index in the
+  // current extent.
+  __host__ __device__ double CalculateError(ssize_t blob_index,
+                                            bool print = false) const {
+    // Index into the blob list for the current key.
+    const size_t i0 = (blob_index + 2 * count_ - ksz_) % count_;
+    const size_t i1 = (blob_index + count_ + ksz_) % count_;
+
+    const size_t global_i0 = i0 + starting_offset_;
+    const size_t global_i1 = i1 + starting_offset_;
+
+    int32_t Mx, My, W;
+    int64_t Mxx, Myy, Mxy;
+    int N;  // how many points are included in the set?
+
+    if (i0 < i1) {
+      N = i1 - i0 + 1;
+
+      LineFitPoint lf1 = GetPoint(global_i1);
+
+      Mx = lf1.Mx;
+      My = lf1.My;
+      Mxx = lf1.Mxx;
+      Mxy = lf1.Mxy;
+      Myy = lf1.Myy;
+      W = lf1.W;
+
+      if (i0 > 0) {
+        LineFitPoint lf0 = GetPoint(global_i0 - 1);
+
+        Mx -= lf0.Mx;
+        My -= lf0.My;
+        Mxx -= lf0.Mxx;
+        Mxy -= lf0.Mxy;
+        Myy -= lf0.Myy;
+        W -= lf0.W;
+      }
+    } else {
+      // i0 > i1, e.g. [15, 2]. Wrap around.
+      LineFitPoint lf0 = GetPoint(global_i0 - 1, print);
+      LineFitPoint lfsz = GetPoint(starting_offset_ + count_ - 1, print);
+
+      Mx = lfsz.Mx - lf0.Mx;
+      My = lfsz.My - lf0.My;
+      Mxx = lfsz.Mxx - lf0.Mxx;
+      Mxy = lfsz.Mxy - lf0.Mxy;
+      Myy = lfsz.Myy - lf0.Myy;
+      W = lfsz.W - lf0.W;
+
+      LineFitPoint lf1 = GetPoint(global_i1, print);
+
+      Mx += lf1.Mx;
+      My += lf1.My;
+      Mxx += lf1.Mxx;
+      Mxy += lf1.Mxy;
+      Myy += lf1.Myy;
+      W += lf1.W;
+
+      N = count_ - i0 + i1 + 1;
+    }
+
+    // And now fit it.
+    return FitLineError(N, Mx, My, Mxx, Myy, Mxy, W);
+  }
+
+  // Returns the starting global index of the region we are responsible for
+  // computing.
+  __host__ __device__ __forceinline__ uint32_t
+  global_block_index_start() const {
+    return global_block_index_start_;
+  }
+  // Returns the ending global index of the region we are responsible for
+  // computing.
+  __host__ __device__ __forceinline__ uint32_t global_block_index_end() const {
+    return global_block_index_end_;
+  }
+  // Returns the size of the region we are responsible for computing.
+  __host__ __device__ __forceinline__ uint32_t global_block_index_size() const {
+    return global_block_index_end_ - global_block_index_start_;
+  }
+
+  // Returns the starting index in global indices of the current blob ID.
+  __host__ __device__ __forceinline__ uint32_t
+  global_current_blob_starting_index() const {
+    return starting_offset_;
+  }
+
+  // Returns the number of points in the current blob ID.
+  __host__ __device__ __forceinline__ uint32_t
+  global_current_blob_count() const {
+    return count_;
+  }
+
+  // Returns the current blob ID.
+  __host__ __device__ __forceinline__ uint32_t blob_index() const {
+    return blob_index_;
+  }
+
+ private:
+  const LineFitPoint *line_fit_points_device_;
+  size_t points_;
+  const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device_;
+
+  TempStorage *storage_;
+
+  // Start and end location of the LineFitPoint cache in tmp_storage_.
+  const uint32_t global_block_index_cache_start_ =
+      blockIdx.x == 0
+          ? 0
+          : blockIdx.x * (kPointsPerBlock - 2 * kErrorsBuffer) - kErrorsBuffer;
+  const uint32_t global_block_index_cache_end_ = std::min<uint32_t>(
+      global_block_index_cache_start_ + kPointsPerBlock, points_);
+
+  const uint32_t global_block_index_start_ =
+      blockIdx.x * (kPointsPerBlock - 2 * kErrorsBuffer);
+  const uint32_t global_block_index_end_ = std::min<uint32_t>(
+      global_block_index_start_ + kPointsPerBlock - 2 * kErrorsBuffer, points_);
+
+  size_t blob_index_ = 0xffffffff;
+  size_t starting_offset_ = 0;
+  size_t count_ = 0;
+  size_t ksz_ = 0;
+};
+
+template <size_t kThreads>
+__global__ void DoFitLines(
+    const LineFitPoint *line_fit_points_device, size_t points,
+    const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+    double *errs_device, double *filtered_errs_device, Peak *peaks_device) {
+  __shared__ TempStorage storage;
+
+  ErrorCalculator<kThreads> calculator(line_fit_points_device, points,
+                                       selected_extents_device, &storage);
+
+  calculator.Load();
+
+  // TODO(austin): Whichever warp loads the first ksz blobs should load the
+  // prior ksz blobs.  Figure out if things are too slow first.  __syncwarp()
+  // and make the first warp do it?
+
+  __syncthreads();
+
+  // We need to compute a couple of errs_device past each end in shared memory.
+  // This needs to be (FilterCoefficients().size() - 1) + 2.  The first term is
+  // the number of extra errors we need for the filter.  The second is the extra
+  // number of errors needed to implement the peak finder (1 on each side).
+
+  // OK, the way this all works now, error calculation work [0, kErrorsBuffer)
+  // is done in the first kErrorsBuffer threads, and written to
+  // tmp_errs [0, kErrorsBuffer), even if this wraps back into the buffer.
+
+  // Now that everything is loaded, have each thread process its points by
+  // strides.
+
+  // TODO(austin): We need 4 reserved ranges.  Proposal is:
+  //   [0, kErrorsBuffer) -> beginning points of the extent of blob 0, ie
+  //     starting at starting_offset_ globally.
+  //
+  //   [kErrorsBuffer, 2 * kErrorsBuffer) -> 4 points immediately before the
+  //   starting point.
+  //
+  // And then symetrically off the back end too.  This will mean we process 16
+  // less points per thread.  Whatever.
+
+  for (size_t i = threadIdx.x; i < kPointsPerBlock; i += blockDim.x) {
+    // OK, now we need to do the actual line fit.
+    if (i < kErrorsBuffer) {
+      // Start by extending the errors before the blob in our buffer space.  All
+      // of this will be done in blob0's address space.
+      //
+      // In the first block, this will load the 8th element.  But, since blobs
+      // are 24 or longer, this will be the 0th blob.  For following blocks,
+      // this is actually the right index.
+      calculator.LoadExtents(0);
+
+      // First 4 threads do the first 4 elements in the first blob.
+      if (i < kBeforeBuffer) {
+        storage.tmp_errs_before[i] = calculator.CalculateError(i);
+      } else {
+        // Then, compute the 4 errors right before the buffer.
+        storage.tmp_errs[i - kBeforeBuffer] = calculator.CalculateError(
+            calculator.BlobIndex(i - kBeforeBuffer) - kBeforeBuffer);
+      }
+    } else if (i - kErrorsBuffer + calculator.global_block_index_start() <
+               calculator.global_block_index_end() + kBeforeBuffer) {
+      // Now we are solidly in the middle of the block.  Make sure anything
+      // computed after the end of the buffer uses the extents of the end.
+      calculator.LoadExtents(std::min<uint32_t>(
+          calculator.global_block_index_size() - 1, i - kErrorsBuffer));
+      size_t target_index = calculator.BlobIndex(i - kErrorsBuffer);
+
+#ifdef DEBUG_BLOB_NUMBER
+      const bool print =
+          (calculator.blob_index() == DEBUG_BLOB_NUMBER &&
+           ((484 - 4 <= target_index && target_index <= 484 + 4)));
+#endif
+
+      storage.tmp_errs[i - kBeforeBuffer] =
+          calculator.CalculateError(target_index);
+#ifdef DEBUG_BLOB_NUMBER
+      if (print) {
+        printf("Block %d Thread %d (idx %d)   Err: %f\n", blockIdx.x,
+               threadIdx.x, (int)target_index,
+               calculator.CalculateError(target_index, true));
+      }
+#endif
+    } else {
+      // Past the end of the normal calcs, do the end buffering.
+      if (i < kPointsPerBlock - kBeforeBuffer) {
+        continue;
+      }
+
+      // We are just supposed to continue with the extents of the last index
+      // and keep going.
+      calculator.LoadExtents(calculator.global_block_index_size() - 1);
+
+      // Wrap before the beginning now of the last blob.
+      storage.tmp_errs_after[i - (kPointsPerBlock - kBeforeBuffer)] =
+          calculator.CalculateError(calculator.global_current_blob_count() -
+                                    kBeforeBuffer +
+                                    (i - (kPointsPerBlock - kBeforeBuffer)));
+    }
+  }
+
+  __syncthreads();
+
+#ifdef DEBUG_BLOB_NUMBER
+  /*
+  if (threadIdx.x == 0) {
+    for (int i = (int)calculator.global_block_index_cache_start_;
+         i < (int)calculator.global_block_index_cache_end_; ++ i) {
+      auto x = calculator.GetPoint(i);
+      if (x.blob_index == DEBUG_BLOB_NUMBER) {
+        printf("Block %d Thread %d   Loading global %d, relative %d, Mx: %f\n",
+               blockIdx.x, threadIdx.x, i,
+               (int)(i - calculator.selected_extents_device_[x.blob_index]
+                             .value.starting_offset),
+               x.Mx / 2.0);
+      }
+    }
+  }
+
+  __syncthreads();
+  */
+#endif
+
+  // We now have all the errors loaded! Box filter them.
+  for (size_t i = threadIdx.x; i < kPointsPerBlock; i += blockDim.x) {
+    // The peak finder needs 1 more filtered error in each direction.
+    ssize_t target_index;
+    double *destination;
+    if (i < kErrorsBuffer) {
+      if (i < kErrorsBuffer - 2) {
+        continue;
+      } else {
+        calculator.LoadExtents(0);
+        if (i < kErrorsBuffer - 1) {
+          // Target index is now 0.
+          target_index = 0;
+          destination = &storage.tmp_filtered_err_before;
+        } else {
+          target_index = calculator.BlobIndex(0) - 1;
+          destination = &storage.tmp_filtered_errs[i + 1 - kErrorsBuffer];
+        }
+      }
+    } else if (i - kErrorsBuffer + calculator.global_block_index_start() >=
+               calculator.global_block_index_end()) {
+      if (i - kErrorsBuffer + calculator.global_block_index_start() >=
+          calculator.global_block_index_end() + 2) {
+        // Past 1 past the end for the peak finder.
+        break;
+      }
+      calculator.LoadExtents(calculator.global_block_index_end() -
+                             calculator.global_block_index_start() - 1);
+
+      if (i - kErrorsBuffer + calculator.global_block_index_start() >=
+          calculator.global_block_index_end() + 1) {
+        destination = &storage.tmp_filtered_err_after;
+        target_index = calculator.global_current_blob_count() - 1;
+      } else {
+        destination = &storage.tmp_filtered_errs[i + 1 - kErrorsBuffer];
+        target_index = calculator.BlobIndex(i - kErrorsBuffer);
+      }
+    } else {
+      calculator.LoadExtents(i - kErrorsBuffer);
+      target_index = calculator.BlobIndex(i - kErrorsBuffer);
+      destination = &storage.tmp_filtered_errs[i + 1 - kErrorsBuffer];
+    }
+
+    double accumulated_error = 0.0;
+
+#ifdef DEBUG_BLOB_NUMBER
+    const bool print = (calculator.blob_index() == DEBUG_BLOB_NUMBER &&
+                        ((484 - 4 <= target_index && target_index <= 484 + 4)));
+#endif
+
+#pragma unroll
+    for (size_t j = 0; j < FilterCoefficients().size(); ++j) {
+      const double e = calculator.LoadError(target_index + j -
+                                            FilterCoefficients().size() / 2);
+      accumulated_error += e * FilterCoefficients()[j];
+#ifdef DEBUG_BLOB_NUMBER
+      if (print) {
+        printf("Block %d Thread %d (idx %d)    + %f * %f (%f) -> %f\n",
+               blockIdx.x, threadIdx.x, (int)target_index, e,
+               FilterCoefficients()[j], e * FilterCoefficients()[j],
+               accumulated_error);
+      }
+#endif
+    }
+    *destination = accumulated_error;
+
+#ifdef DEBUG_BLOB_NUMBER
+    if (print) {
+      printf(
+          "Block %d Thread %d (idx %d)  Blob %d of size %d, index %d "
+          "filtered_error = "
+          "%f, error = %f\n",
+          blockIdx.x, threadIdx.x, (int)target_index,
+          (int)calculator.blob_index(),
+          (int)calculator.global_current_blob_count(), (int)target_index,
+          accumulated_error, calculator.LoadError(target_index));
+    }
+#endif
+  }
+
+  __syncthreads();
+  // OK, errors are now saved to shared memory.  Compute peaks and push out to
+  // disk.
+
+  // TODO(austin): Don't save out the unfiltered errors if we aren't
+  // testing/debugging...
+  for (size_t i = threadIdx.x; i < kPointsPerBlock; i += blockDim.x) {
+    if (i < kErrorsBuffer) {
+      continue;
+    }
+
+    if (i - kErrorsBuffer + calculator.global_block_index_start() >=
+        calculator.global_block_index_end()) {
+      break;
+    }
+
+    errs_device[i - kErrorsBuffer + calculator.global_block_index_start()] =
+        storage.tmp_errs[i - kBeforeBuffer];
+  }
+
+  for (size_t i = threadIdx.x; i < kPointsPerBlock; i += blockDim.x) {
+    if (i < kErrorsBuffer) {
+      continue;
+    }
+
+    if (i - kErrorsBuffer + calculator.global_block_index_start() >=
+        calculator.global_block_index_end()) {
+      break;
+    }
+
+    calculator.LoadExtents(i - kErrorsBuffer);
+
+    const double before_error =
+        calculator.LoadFilteredError(i - kErrorsBuffer - 1);
+    const double my_error = calculator.LoadFilteredError(i - kErrorsBuffer);
+    const double after_error =
+        calculator.LoadFilteredError(i - kErrorsBuffer + 1);
+
+    // This gets us ready to sort.
+    filtered_errs_device[i - kErrorsBuffer +
+                         calculator.global_block_index_start()] = my_error;
+
+    const bool is_peak = my_error > before_error && my_error > after_error;
+
+    Peak peak;
+    peak.error = -my_error;
+    peak.blob_index = is_peak ? calculator.blob_index() : Peak::kNoPeak();
+    peak.filtered_point_index =
+        i - kErrorsBuffer + calculator.global_block_index_start();
+
+    peaks_device[peak.filtered_point_index] = peak;
+  }
+}
+
+void FitLines(
+    const LineFitPoint *line_fit_points_device, size_t points,
+    const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+    size_t num_extents, double *errs_device, double *filtered_errs_device,
+    Peak *peaks_device, CudaStream *stream) {
+  constexpr size_t kThreads = 128;
+  const size_t kBlocks = (points + kPointsPerBlock - 2 * kErrorsBuffer - 1) /
+                         (kPointsPerBlock - 2 * kErrorsBuffer);
+
+  VLOG(1) << "Spawning with " << kThreads << " threads, and " << kBlocks
+          << " blocks for " << num_extents << " blob_ids and " << points
+          << " points";
+  DoFitLines<kThreads><<<kBlocks, kThreads, 0, stream->get()>>>(
+      line_fit_points_device, points, selected_extents_device, errs_device,
+      filtered_errs_device, peaks_device);
+}
+
+namespace {
+
+// Returns n choose k as a constexpr.
+constexpr uint Binomial(uint n, uint k) {
+  if (n < k) {
+    return 0;
+  }
+
+  uint m = std::min(k, n - k);
+  uint result = 1;
+
+  for (uint i = 0; i < m; ++i) {
+    result *= (n - i);
+    result /= (i + 1);
+  }
+
+  return result;
+}
+
+constexpr int kNMaxima = 10;
+
+// Returns the sum of binomials for CumulativeSumsM0.
+template <size_t nmaxima, size_t N>
+constexpr uint SumBinomial(uint i, uint start = 0) {
+  size_t result = 0;
+  for (uint m0 = start; m0 <= i; ++m0) {
+    result += Binomial(nmaxima - m0 - 1, N);
+  }
+  return result;
+}
+
+// Returns the cumulative number of indices consumed by incrementing each m0
+// index.
+constexpr std::array<uint, 7> CumulativeSumsM0() {
+  constexpr size_t nmaxima = kNMaxima;
+  return {
+      SumBinomial<nmaxima, 3>(0, 0), SumBinomial<nmaxima, 3>(1, 0),
+      SumBinomial<nmaxima, 3>(2, 0), SumBinomial<nmaxima, 3>(3, 0),
+      SumBinomial<nmaxima, 3>(4, 0), SumBinomial<nmaxima, 3>(5, 0),
+      SumBinomial<nmaxima, 3>(6, 0),
+  };
+}
+
+// Returns the number of indices consumed by each m1 index.  [0] is how many
+// you'd consume if m0 and m1 were both as small as they could be.  To handle m0
+// > 0, increment your index.
+constexpr std::array<uint, 7> BinomialM1() {
+  constexpr size_t nmaxima = kNMaxima;
+  return {
+      Binomial(nmaxima - 2, 2), Binomial(nmaxima - 3, 2),
+      Binomial(nmaxima - 4, 2), Binomial(nmaxima - 5, 2),
+      Binomial(nmaxima - 6, 2), Binomial(nmaxima - 7, 2),
+      Binomial(nmaxima - 8, 2),
+  };
+}
+
+// Returns the m0 for the provided global index.
+__host__ __device__ const cuda::std::pair<uint, uint> FindM0(uint i) {
+  uint m0 = 0;
+  uint last = 0;
+  while (true) {
+    uint next = CumulativeSumsM0()[m0];
+    if (i < next) {
+      return cuda::std::make_pair(m0, i - last);
+    }
+    last = next;
+    ++m0;
+  }
+}
+
+// Returns the m1 for the provided m0 and global index remainder.
+__host__ __device__ const cuda::std::pair<uint, uint> FindM1(uint m0, uint i) {
+  uint m1 = m0;
+  while (true) {
+    uint next = BinomialM1()[m1];
+    if (i < next || m1 == kNMaxima - 4) {
+      return cuda::std::make_pair(m1, i);
+    }
+    i -= next;
+    ++m1;
+  }
+}
+
+// Returns the m2 for the provided m1 and global index remainder.
+__host__ __device__ const cuda::std::pair<uint, uint> FindM2(uint m1, uint i) {
+  uint m2 = m1;
+  while (true) {
+    uint next = kNMaxima - m2 - 1 - 1;
+    if (i < next || m2 == kNMaxima - 3) {
+      return cuda::std::make_pair(m2, i);
+    }
+    i -= next;
+    ++m2;
+  }
+}
+
+}  // namespace
+
+__host__ __device__ std::tuple<uint, uint, uint, uint> Unrank(uint i) {
+  uint m0 = 0;
+  uint m1;
+  uint m2;
+  uint m3;
+
+  auto result0 = FindM0(i);
+  m0 = result0.first;
+  i = result0.second;
+
+  auto result1 = FindM1(m0, result0.second);
+  m1 = result1.first + 1;
+
+  i = result1.second;
+  auto result2 = FindM2(m1, result1.second);
+  m2 = result2.first + 1;
+
+  m3 = m2 + result2.second + 1;
+  return std::make_tuple(m0, m1, m2, m3);
+}
+
+// TODO(austin): Convert this into a constant lookup.
+__device__ __host__ cuda::std::pair<uint, uint> GetM0M1(uint tid) {
+  constexpr int nmaxima = kNMaxima;
+  uint count = 0;
+  for (uint m0 = 0; m0 < nmaxima - 3; m0++) {
+    for (uint m1 = m0 + 1; m1 < nmaxima - 2; m1++) {
+      if (count == tid) {
+        return cuda::std::make_pair(m0, m1);
+      }
+      ++count;
+    }
+  }
+  return cuda::std::make_pair(0, 0);
+}
+
+__device__ __forceinline__ LineFitMoments
+ReadMoments(const LineFitPoint *line_fit_points_device, size_t blob_point_count,
+            size_t index0, size_t index1) {
+  LineFitMoments result;
+
+  if (index0 < index1) {
+    result.N = index1 - index0 + 1;
+
+    LineFitPoint lf1 = line_fit_points_device[index1];
+
+    result.Mx = lf1.Mx;
+    result.My = lf1.My;
+    result.Mxx = lf1.Mxx;
+    result.Mxy = lf1.Mxy;
+    result.Myy = lf1.Myy;
+    result.W = lf1.W;
+
+    if (index0 > 0) {
+      LineFitPoint lf0 = line_fit_points_device[index0 - 1];
+
+      result.Mx -= lf0.Mx;
+      result.My -= lf0.My;
+      result.Mxx -= lf0.Mxx;
+      result.Mxy -= lf0.Mxy;
+      result.Myy -= lf0.Myy;
+      result.W -= lf0.W;
+    }
+  } else {
+    // index0 > index1, e.g. [15, 2]. Wrap around.
+    LineFitPoint lf0 = line_fit_points_device[index0 - 1];
+    LineFitPoint lfsz = line_fit_points_device[blob_point_count - 1];
+
+    result.Mx = lfsz.Mx - lf0.Mx;
+    result.My = lfsz.My - lf0.My;
+    result.Mxx = lfsz.Mxx - lf0.Mxx;
+    result.Mxy = lfsz.Mxy - lf0.Mxy;
+    result.Myy = lfsz.Myy - lf0.Myy;
+    result.W = lfsz.W - lf0.W;
+
+    LineFitPoint lf1 = line_fit_points_device[index1];
+
+    result.Mx += lf1.Mx;
+    result.My += lf1.My;
+    result.Mxx += lf1.Mxx;
+    result.Mxy += lf1.Mxy;
+    result.Myy += lf1.Myy;
+    result.W += lf1.W;
+
+    result.N = blob_point_count - index0 + index1 + 1;
+  }
+  return result;
+}
+
+__device__ void FitLine(LineFitMoments moments, double *lineparam01,
+                        double *lineparam23, double *err, double *mse,
+                        bool print = false) {
+  if (print) {
+    printf(
+        "Block %d Thread %d   Mx: %.4f, My: %.4f, Mxx: %.4f, Mxy: %.4f, Myy: "
+        "%.4f, W: %d, N: %d\n",
+        blockIdx.x, threadIdx.x, moments.Mx / 2.0, moments.My / 2.0,
+        moments.Mxx / 4.0, moments.Mxy / 4.0, moments.Myy / 4.0, moments.W,
+        moments.N);
+  }
+  int64_t Cxx = moments.Mxx * moments.W - static_cast<int64_t>(moments.Mx) *
+                                              static_cast<int64_t>(moments.Mx);
+  int64_t Cxy = moments.Mxy * moments.W - static_cast<int64_t>(moments.Mx) *
+                                              static_cast<int64_t>(moments.My);
+  int64_t Cyy = moments.Myy * moments.W - static_cast<int64_t>(moments.My) *
+                                              static_cast<int64_t>(moments.My);
+
+  // Pose it as an eigenvalue problem.
+  //
+  // TODO(austin): Are floats good enough?  Hard to tell what the "right answer"
+  // actually is...
+  const float hypot_cached = std::hypotf((Cxx - Cyy), 2 * Cxy);
+  const float eight_w_squared = static_cast<float>(
+      static_cast<int64_t>(moments.W) * static_cast<int64_t>(moments.W) * 8.0);
+  const float eig_small = (Cxx + Cyy - hypot_cached) / eight_w_squared;
+  if (print) {
+    printf("Block %d Thread %d   eig_small: (%ld + %ld - %f) / %f -> %f\n",
+           blockIdx.x, threadIdx.x, Cxx, Cyy, hypot_cached, eight_w_squared,
+           eig_small);
+  }
+
+  if (lineparam01) {
+    lineparam01[0] =
+        static_cast<float>(moments.Mx) / static_cast<float>(moments.W * 2);
+    lineparam01[1] =
+        static_cast<float>(moments.My) / static_cast<float>(moments.W * 2);
+  }
+  if (lineparam23) {
+    // These don't match the originals at all, but the math should come out
+    // right.  n{xy}{12} end up being multiplied by 8 W^2, and we compare the
+    // square but use hypot on nx, ny directly.  (and let the W^2 term come out
+    // as common to both the hypot and nxy terms.)
+    const float nx1 = static_cast<float>(Cxx - Cyy) - hypot_cached;
+    const float ny1 = 2 * Cxy;
+    const float M1 = nx1 * nx1 + ny1 * ny1;
+    const float nx2 = 2 * Cxy;
+    const float ny2 = static_cast<float>(Cyy - Cxx) - hypot_cached;
+    const float M2 = nx2 * nx2 + ny2 * ny2;
+
+    float nx, ny;
+    if (M1 > M2) {
+      nx = nx1;
+      ny = ny1;
+    } else {
+      nx = nx2;
+      ny = ny2;
+    }
+
+    float length = std::hypotf(nx, ny);
+    lineparam23[0] = nx / length;
+    lineparam23[1] = ny / length;
+  }
+
+  // sum of squared errors
+  *err = moments.N * eig_small;
+
+  if (print) {
+    printf("Block %d Thread %d   err: %f\n", blockIdx.x, threadIdx.x,
+           moments.N * eig_small);
+  }
+
+  // mean squared error
+  *mse = eig_small;
+}
+
+__device__ __forceinline__ void FitLine(
+    const LineFitPoint *line_fit_points_device, size_t blob_point_count,
+    size_t index0, size_t index1, double *lineparam01, double *lineparam23,
+    double *err, double *mse, bool print = false) {
+  LineFitMoments moments =
+      ReadMoments(line_fit_points_device, blob_point_count, index0, index1);
+  FitLine(moments, lineparam01, lineparam23, err, mse, print);
+}
+
+struct QuadFitStorage {
+  double errorm0m1[kNMaxima - 3][kNMaxima - 3];
+  double lineparams23m0m1[kNMaxima - 3][kNMaxima - 3][2];
+  uint16_t point_indices[16];
+};
+
+class QuadFitCalculator {
+ public:
+  __host__ __device__ QuadFitCalculator(
+      const Peak *peaks_device, const PeakExtents *peak_extents,
+      const LineFitPoint *line_fit_points_device,
+      const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+      float max_line_fit_mse, double max_dot, QuadFitStorage *storage)
+      : peaks_device_(peaks_device),
+        extents_(peak_extents[blockIdx.x]),
+        selected_extent_(selected_extents_device[extents_.blob_index].value),
+        line_fit_points_device_(line_fit_points_device +
+                                selected_extent_.starting_offset),
+        max_line_fit_mse_(max_line_fit_mse),
+        max_dot_(max_dot),
+        storage_(storage) {}
+
+  __host__ __device__ int RawIndexFromMaxima(int m) const {
+    return peaks_device_[m + extents_.starting_offset].filtered_point_index -
+           selected_extent_.starting_offset;
+  }
+
+  __host__ __device__ uint32_t IndexFromMaxima(int m) const {
+    return storage_->point_indices[m];
+  }
+
+  __device__ __forceinline__ uint32_t sz() const {
+    return selected_extent_.count;
+  }
+
+  __device__ __forceinline__ void FitLine(size_t index0, size_t index1,
+                                          double *lineparam01,
+                                          double *lineparam23, double *err,
+                                          double *mse,
+                                          bool print = false) const {
+    if (print) {
+      printf("Block %d Thread %d   Fitting line %d, %d\n", blockIdx.x,
+             threadIdx.x, (int)index0, (int)index1);
+    }
+    ::frc971::apriltag::FitLine(line_fit_points_device_, selected_extent_.count,
+                                index0, index1, lineparam01, lineparam23, err,
+                                mse, print);
+  }
+
+  __device__ __forceinline__ LineFitMoments ReadMoments(size_t index0,
+                                                        size_t index1) {
+    return ::frc971::apriltag::ReadMoments(
+        line_fit_points_device_, selected_extent_.count, index0, index1);
+  }
+
+  __host__ __device__ void ComputeM0M1Fit() {
+    if (extents_.count < 4) {
+      return;
+    }
+
+    cuda::std::pair<uint, uint> m0m1 = GetM0M1(threadIdx.x);
+    const uint m0 = m0m1.first;
+    const uint m1 = m0m1.second;
+    if (m0 >= m1) {
+      return;
+    }
+
+    // First 35 threads compute the first 35 m0, m1 pairs.
+    // That lets us cache them in shared memory.  Then, all the threads
+    // calculate the full depth of the tree.
+    //
+    // If we don't have the full kNMaxima maxima, skip any indices which are
+    // past the end.
+    if (m1 < kNMaxima && m1 < extents_.count) {
+      const uint i0 = IndexFromMaxima(m0);
+      const uint i1 = IndexFromMaxima(m1);
+
+      double err01;
+      double mse01;
+
+      FitLine(i0, i1, nullptr, storage_->lineparams23m0m1[m0][m1 - 1], &err01,
+              &mse01);
+
+      if (mse01 > max_line_fit_mse_) {
+        err01 = std::numeric_limits<double>::max();
+      }
+
+      storage_->errorm0m1[m0][m1 - 1] = err01;
+    } else {
+      storage_->errorm0m1[m0][m1 - 1] = std::numeric_limits<double>::max();
+    }
+  }
+
+  __host__ __device__ int blob_index() const { return extents_.blob_index; }
+
+  __host__ __device__ double FitLines(uint m0, uint m1, uint m2,
+                                      uint m3) const {
+    const bool print =
+#ifdef DEBUG_BLOB_NUMBER
+        (blob_index() == DEBUG_BLOB_NUMBER &&
+         (  //(m0 == 5 && m1 == 6 && m2 == 7 && m3 == 8) ||
+             (m0 == 0 && m1 == 4 && m2 == 8 && m3 == 9)));
+#else
+        false;
+#endif
+    double err = std::numeric_limits<double>::max();
+
+    if (extents_.count < 4) {
+      return err;
+    }
+
+    if (m3 >= kNMaxima || m3 >= extents_.count) {
+      return err;
+    }
+
+    const double errm0m1 = storage_->errorm0m1[m0][m1 - 1];
+    if (errm0m1 == std::numeric_limits<double>::max()) {
+      return err;
+    }
+
+    const double *paramsm0m123 = storage_->lineparams23m0m1[m0][m1 - 1];
+
+    double errm1m2;
+    double msem1m2;
+    double paramsm1m223[2];
+
+    const int i1 = IndexFromMaxima(m1);
+    const int i2 = IndexFromMaxima(m2);
+    FitLine(i1, i2, nullptr, paramsm1m223, &errm1m2, &msem1m2, print);
+    if (msem1m2 > max_line_fit_mse_) {
+      return std::numeric_limits<double>::max();
+    }
+
+    // return m0 + m1 * 10 + m2 * 100 + m3 * 1000 + errm0m1 + errm1m2;
+
+    const double dot =
+        paramsm0m123[0] * paramsm1m223[0] + paramsm0m123[1] * paramsm1m223[1];
+    if (fabs(dot) > max_dot_) {
+      return std::numeric_limits<double>::max();
+    }
+
+    const int i3 = IndexFromMaxima(m3);
+
+    double errm2m3;
+    double msem2m3;
+
+    FitLine(i2, i3, nullptr, nullptr, &errm2m3, &msem2m3, print);
+    if (msem2m3 > max_line_fit_mse_) {
+      return std::numeric_limits<double>::max();
+    }
+
+    const int i0 = IndexFromMaxima(m0);
+    double errm3m0;
+    double msem3m0;
+    FitLine(i3, i0, nullptr, nullptr, &errm3m0, &msem3m0, print);
+    if (msem3m0 > max_line_fit_mse_) {
+      return std::numeric_limits<double>::max();
+    }
+
+    if (print) {
+      printf(
+          "Block %d Thread %d  %d %d %d %d: errm0m1 %f errm1m2 %f errm2m3 %f "
+          "errm3m0 %f\n",
+          blockIdx.x, threadIdx.x, i0, i1, i2, i3, errm0m1, errm1m2, errm2m3,
+          errm3m0);
+    }
+
+    return errm0m1 + errm1m2 + errm2m3 + errm3m0;
+  }
+
+  __host__ __device__ size_t peaks_count() const { return extents_.count; }
+
+ private:
+  const Peak *peaks_device_;
+  const PeakExtents extents_;
+  const MinMaxExtents selected_extent_;
+
+  const LineFitPoint *line_fit_points_device_;
+  const float max_line_fit_mse_;
+  const double max_dot_;
+  QuadFitStorage *storage_;
+};
+
+struct QuadError {
+  uint8_t m0;
+  uint8_t m1;
+  uint8_t m2;
+  uint8_t m3;
+  double error;
+};
+
+struct MinQuadError {
+  __host__ __device__ QuadError operator()(const QuadError &a,
+                                           const QuadError &b) {
+    if (a.error <= b.error) {
+      return a;
+    } else {
+      return b;
+    }
+  }
+};
+
+struct CustomLess {
+  __device__ bool operator()(const uint16_t lhs, const uint16_t rhs) {
+    return lhs < rhs;
+  }
+};
+
+__global__ void DoFitQuads(
+    const Peak *peaks_device, const PeakExtents *peak_extents,
+    const LineFitPoint *line_fit_points_device,
+    const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+    float max_line_fit_mse, double cos_critical_rad,
+    FitQuad *fit_quads_device) {
+  __shared__ QuadFitStorage storage;
+  // Specialize BlockReduce for a 1D block of 128 threads of type int
+  typedef cub::BlockReduce<QuadError, MaxRankedIndex()> BlockReduce;
+  // Allocate shared memory for BlockReduce
+  __shared__ typename BlockReduce::TempStorage temp_storage_reduce;
+
+  QuadFitCalculator calculator(peaks_device, peak_extents,
+                               line_fit_points_device, selected_extents_device,
+                               max_line_fit_mse, cos_critical_rad, &storage);
+
+  // Step 1, unsort the maxima back by point index.
+  constexpr size_t kItemsPerThread = 1;
+  constexpr size_t kItems = 16;
+  using WarpMergeSortT = cub::WarpMergeSort<uint16_t, kItemsPerThread, kItems>;
+
+  if (threadIdx.x < kItems) {
+    __shared__ typename WarpMergeSortT::TempStorage temp_storage_merge;
+    uint16_t index[1];
+    if (threadIdx.x >= calculator.peaks_count() || threadIdx.x >= kNMaxima) {
+      index[0] = std::numeric_limits<uint16_t>::max();
+    } else {
+      index[0] = calculator.RawIndexFromMaxima(threadIdx.x);
+    }
+    WarpMergeSortT(temp_storage_merge).Sort(index, CustomLess());
+    storage.point_indices[threadIdx.x] = index[0];
+  }
+
+  __syncthreads();
+
+#ifdef DEBUG_BLOB_NUMBER
+  if (threadIdx.x == 0) {
+    if (calculator.blob_index() == DEBUG_BLOB_NUMBER) {
+      for (size_t i = 0; i < std::min(calculator.peaks_count(), kItems); ++i) {
+        printf("  %d: %d, now %d\n", (int)i, calculator.RawIndexFromMaxima(i),
+               storage.point_indices[i]);
+      }
+    }
+  }
+#endif
+
+  // TODO(austin): Where do we handle blobs with less than 4 extents?
+  calculator.ComputeM0M1Fit();
+
+  __syncthreads();
+
+  const std::tuple<uint, uint, uint, uint> ms = Unrank(threadIdx.x);
+  QuadError quad_error;
+  quad_error.m0 = std::get<0>(ms);
+  quad_error.m1 = std::get<1>(ms);
+  quad_error.m2 = std::get<2>(ms);
+  quad_error.m3 = std::get<3>(ms);
+  quad_error.error = calculator.FitLines(quad_error.m0, quad_error.m1,
+                                         quad_error.m2, quad_error.m3);
+
+#ifdef DEBUG_BLOB_NUMBER
+  if (calculator.blob_index() == DEBUG_BLOB_NUMBER) {
+    if (quad_error.error < 0.0) {
+      printf("blob index %d maxima: %d %d %d %d, error negative of: %f\n",
+             (int)calculator.blob_index(), (int)quad_error.m0,
+             (int)quad_error.m1, (int)quad_error.m2, (int)quad_error.m3,
+             quad_error.error);
+    }
+  }
+#endif
+
+  // Each thread obtains an input item
+  // Compute the block-wide max for thread0
+  QuadError min_error =
+      BlockReduce(temp_storage_reduce).Reduce(quad_error, MinQuadError());
+
+  if (threadIdx.x == 0) {
+    const bool valid = min_error.error < max_line_fit_mse * calculator.sz();
+    fit_quads_device[blockIdx.x].valid = valid;
+    fit_quads_device[blockIdx.x].blob_index = calculator.blob_index();
+    uint32_t i0 = calculator.IndexFromMaxima(min_error.m0);
+    uint32_t i1 = calculator.IndexFromMaxima(min_error.m1);
+    uint32_t i2 = calculator.IndexFromMaxima(min_error.m2);
+    uint32_t i3 = calculator.IndexFromMaxima(min_error.m3);
+#ifdef DEBUG_BLOB_NUMBER
+    if (calculator.blob_index() == DEBUG_BLOB_NUMBER) {
+      printf("blob index %d maxima: %d\n", (int)calculator.blob_index(),
+             (int)calculator.peaks_count());
+      for (size_t i = 0; i < std::min(kItems, calculator.peaks_count()); ++i) {
+        printf("  %d: %d\n", (int)i, calculator.IndexFromMaxima(i));
+      }
+      printf("Best fit is %d(%d) %d(%d) %d(%d) %d(%d) -> %f, valid? %d\n", i0,
+             (int)min_error.m0, i1, (int)min_error.m1, i2, (int)min_error.m2,
+             i3, (int)min_error.m3, min_error.error, valid);
+    }
+#endif
+    fit_quads_device[blockIdx.x].indices[0] = i0;
+    fit_quads_device[blockIdx.x].indices[1] = i1;
+    fit_quads_device[blockIdx.x].indices[2] = i2;
+    fit_quads_device[blockIdx.x].indices[3] = i3;
+    fit_quads_device[blockIdx.x].moments[0] = calculator.ReadMoments(i0, i1);
+    fit_quads_device[blockIdx.x].moments[1] = calculator.ReadMoments(i1, i2);
+    fit_quads_device[blockIdx.x].moments[2] = calculator.ReadMoments(i2, i3);
+    fit_quads_device[blockIdx.x].moments[3] = calculator.ReadMoments(i3, i0);
+  }
+}
+
+void FitQuads(
+    const Peak *peaks_device, size_t /*peaks*/, const PeakExtents *peak_extents,
+    size_t num_extents, const LineFitPoint *line_fit_points_device, int nmaxima,
+    const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+    float max_line_fit_mse, double cos_critical_rad, FitQuad *fit_quads_device,
+    CudaStream *stream) {
+  constexpr size_t kThreads = MaxRankedIndex();
+  const size_t kBlocks = num_extents;
+  VLOG(1) << "Spawning with " << kThreads << " threads, and " << kBlocks
+          << " blocks for " << num_extents << " blob_ids";
+  CHECK_EQ(nmaxima, kNMaxima)
+      << ": Kernel is compiled and optimized for a fixed nmaxima, please "
+         "recompile if you want to change it.";
+  DoFitQuads<<<kBlocks, kThreads, 0, stream->get()>>>(
+      peaks_device, peak_extents, line_fit_points_device,
+      selected_extents_device, max_line_fit_mse, cos_critical_rad,
+      fit_quads_device);
+}
+
+std::ostream &operator<<(std::ostream &os,
+                         const frc971::apriltag::LineFitMoments &moments) {
+  os << "{Mx:" << std::setprecision(20) << moments.Mx / 2.
+     << ", My:" << std::setprecision(20) << moments.My / 2.
+     << ", Mxx:" << std::setprecision(20) << moments.Mxx / 4.
+     << ", Mxy:" << std::setprecision(20) << moments.Mxy / 4.
+     << ", Myy:" << std::setprecision(20) << moments.Myy / 4.
+     << ", W:" << std::setprecision(20) << moments.W << ", N:" << moments.N
+     << "}";
+  return os;
+}
+
+}  // namespace apriltag
+}  // namespace frc971
diff --git a/frc971/orin/line_fit_filter.h b/frc971/orin/line_fit_filter.h
new file mode 100644
index 0000000..0c37f0b
--- /dev/null
+++ b/frc971/orin/line_fit_filter.h
@@ -0,0 +1,166 @@
+#ifndef FRC971_ORIN_LINE_FIT_FILTER_H_
+#define FRC971_ORIN_LINE_FIT_FILTER_H_
+
+#include <cub/iterator/transform_input_iterator.cuh>
+#include <cuda/std/tuple>
+
+#include "cuda_runtime.h"
+#include "device_launch_parameters.h"
+#include "frc971/orin/cuda.h"
+
+namespace frc971 {
+namespace apriltag {
+
+// Class to hold the extents of a blob of points.
+struct MinMaxExtents {
+  // Min and max coordinates (in non-decimated coordinates)
+  uint16_t min_x;
+  uint16_t min_y;
+  uint16_t max_x;
+  uint16_t max_y;
+
+  // The starting offset of this blob of points in the vector holding all the
+  // points.
+  uint32_t starting_offset;
+
+  // The number of points in the blob.
+  uint32_t count;
+
+  // The dot product is:
+  //  dot = sum( (px - cx) * gx) + (py - cy) * gy )
+  //
+  // We can split this up into:
+  //  dot = sum(px * gx + py * gy) - cx * sum(gx) - cy * sum(gy)
+  //
+  // Which can be calculated without knowing the center.
+  //
+  // Since p and g are all integers, we can sum them into integers too so we
+  // don't lose precision.
+  int32_t gx_sum;
+  int32_t gy_sum;
+
+  int64_t pxgx_plus_pygy_sum;
+
+  // Center location of the blob using the aprilrobotics algorithm.
+  __host__ __device__ double cx() const {
+    return (min_x + max_x) * 0.5f + 0.05118;
+  }
+  __host__ __device__ double cy() const {
+    return (min_y + max_y) * 0.5f + -0.028581;
+  }
+
+  __host__ __device__ float dot() const {
+    return static_cast<double>(pxgx_plus_pygy_sum * 2 -
+                               (min_x + max_x) * gx_sum -
+                               (min_y + max_y) * gy_sum) *
+               0.5 -
+           0.05118 * static_cast<double>(gx_sum) +
+           0.028581 * static_cast<double>(gy_sum);
+  }
+};
+
+__align__(16) struct LineFitPoint {
+  // TODO(austin): How much precision do we actually need?  The less, the
+  // faster...  The less memory too, the faster.
+  //
+  // Is it the double's or the memory which makes us slow?  Could probably bit
+  // pack ints in here...
+  int64_t Mxx;
+  int64_t Myy;
+  int64_t Mxy;
+  // TODO(austin): These both fit in 4 byte numbers :)
+  // There are at most 2 * (width + height) -> 13 bits points in a blob.
+  // The weight can be at most 8 bits (probably 9 because it has a sign).
+  // A point has 11 bits of position data in it.
+  //
+  // To do this with sub bit precision:
+  //   2 * (1088 + 1456) * 1456 * 255 -> 0x7098f600
+  //
+  // Which is < 31 bits, so we can hold a sign bit too!
+  int32_t Mx;
+  int32_t My;
+  int32_t W;
+  uint32_t blob_index;
+};
+
+struct LineFitMoments {
+  // See LineFitPoint for more info.
+  int32_t Mx;
+  int32_t My;
+  int32_t W;
+  int64_t Mxx;
+  int64_t Myy;
+  int64_t Mxy;
+  int N;  // how many points are included in the set?
+};
+
+std::ostream &operator<<(std::ostream &os,
+                         const frc971::apriltag::LineFitMoments &moments);
+
+struct Peak {
+  static constexpr uint16_t kNoPeak() { return 0xffff; }
+  float error;
+  // Point index.
+  uint32_t filtered_point_index;
+  // 0xffff if this isn't a peak, otherwise the blob index.
+  uint16_t blob_index;
+};
+
+struct PeakExtents {
+  uint16_t blob_index;
+  uint32_t starting_offset;
+  uint32_t count;
+};
+
+struct PeakDecomposer {
+  static constexpr size_t kBitsInKey = 16 + 32;
+  __host__ __device__ ::cuda::std::tuple<uint16_t &, float &> operator()(
+      Peak &key) const {
+    return {key.blob_index, key.error};
+  }
+};
+
+constexpr std::array<float, 7> FilterCoefficients() {
+  return std::array<float, 7>{
+      0.01110899634659290314, 0.13533528149127960205, 0.60653066635131835938,
+      1.00000000000000000000, 0.60653066635131835938, 0.13533528149127960205,
+      0.01110899634659290314,
+  };
+}
+
+struct FitQuad {
+  uint16_t blob_index;
+  bool valid;
+  uint16_t indices[4];
+  LineFitMoments moments[4];
+};
+
+__host__ __device__ void FitLine(LineFitMoments moments, double *lineparam01,
+                                 double *lineparam23, double *err, double *mse);
+
+void FitLines(
+    const LineFitPoint *line_fit_points_device, size_t points,
+    const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+    size_t num_extents, double *errs_device, double *filtered_errs_device,
+    Peak *peaks_device, CudaStream *stream);
+
+void FitQuads(
+    const Peak *peaks_device, size_t peaks, const PeakExtents *peak_extents,
+    size_t num_extents, const LineFitPoint *line_fit_points_device, int nmaxima,
+    const cub::KeyValuePair<long, MinMaxExtents> *selected_extents_device,
+    float max_line_fit_mse, double cos_critical_rad, FitQuad *fit_quads_device,
+    CudaStream *stream);
+
+// Returns the m0, m1, m2, m3 indices for the provided index.  The index is the
+// inner loop number when you process the 4 for loops in order and count.  See
+// FilterTest.Unrank for an example.
+//
+// This lets us distribute work amoung the cuda threads and get back the index.
+__host__ __device__ std::tuple<uint, uint, uint, uint> Unrank(uint i);
+// The max number of work elements for a max maxes of 10.
+constexpr size_t MaxRankedIndex() { return 210; }
+
+}  // namespace apriltag
+}  // namespace frc971
+
+#endif  // FRC971_ORIN_LINE_FIT_FILTER_H_
diff --git a/third_party/apriltag/apriltag.c b/third_party/apriltag/apriltag.c
index 3086228..a2170fd 100644
--- a/third_party/apriltag/apriltag.c
+++ b/third_party/apriltag/apriltag.c
@@ -888,6 +888,77 @@
     }
 }
 
+void quad_decode_index(apriltag_detector_t *td, struct quad *quad_original, image_u8_t *im, image_u8_t *im_samples, zarray_t *detections) {
+    // make sure the homographies are computed...
+    if (quad_update_homographies(quad_original) != 0)
+        return;
+
+    for (int famidx = 0; famidx < zarray_size(td->tag_families); famidx++) {
+        apriltag_family_t *family;
+        zarray_get(td->tag_families, famidx, &family);
+
+        if (family->reversed_border != quad_original->reversed_border) {
+            continue;
+        }
+
+        // since the geometry of tag families can vary, start any
+        // optimization process over with the original quad.
+        struct quad *quad = quad_copy(quad_original);
+
+        struct quick_decode_entry entry;
+
+        float decision_margin = quad_decode(td, family, im, quad, &entry, im_samples);
+
+        if (decision_margin >= 0 && entry.hamming < 255) {
+            apriltag_detection_t *det = calloc(1, sizeof(apriltag_detection_t));
+
+            det->family = family;
+            det->id = entry.id;
+            det->hamming = entry.hamming;
+            det->decision_margin = decision_margin;
+
+            double theta = entry.rotation * M_PI / 2.0;
+            double c = cos(theta), s = sin(theta);
+
+            // Fix the rotation of our homography to properly orient the tag
+            matd_t *R = matd_create(3,3);
+            MATD_EL(R, 0, 0) = c;
+            MATD_EL(R, 0, 1) = -s;
+            MATD_EL(R, 1, 0) = s;
+            MATD_EL(R, 1, 1) = c;
+            MATD_EL(R, 2, 2) = 1;
+
+            det->H = matd_op("M*M", quad->H, R);
+
+            matd_destroy(R);
+
+            homography_project(det->H, 0, 0, &det->c[0], &det->c[1]);
+
+            // [-1, -1], [1, -1], [1, 1], [-1, 1], Desired points
+            // [-1, 1], [1, 1], [1, -1], [-1, -1], FLIP Y
+            // adjust the points in det->p so that they correspond to
+            // counter-clockwise around the quad, starting at -1,-1.
+            for (int i = 0; i < 4; i++) {
+                int tcx = (i == 1 || i == 2) ? 1 : -1;
+                int tcy = (i < 2) ? 1 : -1;
+
+                double p[2];
+
+                homography_project(det->H, tcx, tcy, &p[0], &p[1]);
+
+                det->p[i][0] = p[0];
+                det->p[i][1] = p[1];
+            }
+
+            pthread_mutex_lock(&td->mutex);
+            zarray_add(detections, &det);
+            pthread_mutex_unlock(&td->mutex);
+        }
+
+        quad_destroy(quad);
+    }
+}
+
 static void quad_decode_task(void *_u)
 {
     struct quad_decode_task *task = (struct quad_decode_task*) _u;
@@ -905,74 +976,7 @@
             refine_edges(td, im, quad_original);
         }
 
-        // make sure the homographies are computed...
-        if (quad_update_homographies(quad_original) != 0)
-            continue;
-
-        for (int famidx = 0; famidx < zarray_size(td->tag_families); famidx++) {
-            apriltag_family_t *family;
-            zarray_get(td->tag_families, famidx, &family);
-
-            if (family->reversed_border != quad_original->reversed_border) {
-                continue;
-            }
-
-            // since the geometry of tag families can vary, start any
-            // optimization process over with the original quad.
-            struct quad *quad = quad_copy(quad_original);
-
-            struct quick_decode_entry entry;
-
-            float decision_margin = quad_decode(td, family, im, quad, &entry, task->im_samples);
-
-            if (decision_margin >= 0 && entry.hamming < 255) {
-                apriltag_detection_t *det = calloc(1, sizeof(apriltag_detection_t));
-
-                det->family = family;
-                det->id = entry.id;
-                det->hamming = entry.hamming;
-                det->decision_margin = decision_margin;
-
-                double theta = entry.rotation * M_PI / 2.0;
-                double c = cos(theta), s = sin(theta);
-
-                // Fix the rotation of our homography to properly orient the tag
-                matd_t *R = matd_create(3,3);
-                MATD_EL(R, 0, 0) = c;
-                MATD_EL(R, 0, 1) = -s;
-                MATD_EL(R, 1, 0) = s;
-                MATD_EL(R, 1, 1) = c;
-                MATD_EL(R, 2, 2) = 1;
-
-                det->H = matd_op("M*M", quad->H, R);
-
-                matd_destroy(R);
-
-                homography_project(det->H, 0, 0, &det->c[0], &det->c[1]);
-
-                // [-1, -1], [1, -1], [1, 1], [-1, 1], Desired points
-                // [-1, 1], [1, 1], [1, -1], [-1, -1], FLIP Y
-                // adjust the points in det->p so that they correspond to
-                // counter-clockwise around the quad, starting at -1,-1.
-                for (int i = 0; i < 4; i++) {
-                    int tcx = (i == 1 || i == 2) ? 1 : -1;
-                    int tcy = (i < 2) ? 1 : -1;
-
-                    double p[2];
-
-                    homography_project(det->H, tcx, tcy, &p[0], &p[1]);
-
-                    det->p[i][0] = p[0];
-                    det->p[i][1] = p[1];
-                }
-
-                pthread_mutex_lock(&td->mutex);
-                zarray_add(task->detections, &det);
-                pthread_mutex_unlock(&td->mutex);
-            }
-
-            quad_destroy(quad);
-        }
+        quad_decode_index(td, quad_original, im, task->im_samples, task->detections);
     }
 }
 
@@ -999,6 +1003,69 @@
     return 0;
 }
 
+void reconcile_detections(zarray_t *detections, zarray_t *poly0, zarray_t *poly1)
+{
+    for (int i0 = 0; i0 < zarray_size(detections); i0++) {
+
+        apriltag_detection_t *det0;
+        zarray_get(detections, i0, &det0);
+
+        for (int k = 0; k < 4; k++)
+            zarray_set(poly0, k, det0->p[k], NULL);
+
+        for (int i1 = i0+1; i1 < zarray_size(detections); i1++) {
+
+            apriltag_detection_t *det1;
+            zarray_get(detections, i1, &det1);
+
+            if (det0->id != det1->id || det0->family != det1->family)
+                continue;
+
+            for (int k = 0; k < 4; k++)
+                zarray_set(poly1, k, det1->p[k], NULL);
+
+            if (g2d_polygon_overlaps_polygon(poly0, poly1)) {
+                // the tags overlap. Delete one, keep the other.
+
+                int pref = 0; // 0 means undecided which one we'll keep.
+                pref = prefer_smaller(pref, det0->hamming, det1->hamming);     // want small hamming
+                pref = prefer_smaller(pref, -det0->decision_margin, -det1->decision_margin);      // want bigger margins
+
+                // if we STILL don't prefer one detection over the other, then pick
+                // any deterministic criterion.
+                for (int i = 0; i < 4; i++) {
+                    pref = prefer_smaller(pref, det0->p[i][0], det1->p[i][0]);
+                    pref = prefer_smaller(pref, det0->p[i][1], det1->p[i][1]);
+                }
+
+                if (pref == 0) {
+                    // at this point, we should only be undecided if the tag detections
+                    // are *exactly* the same. How would that happen?
+                    debug_print("uh oh, no preference for overlappingdetection\n");
+                }
+
+                if (pref < 0) {
+                    // keep det0, destroy det1
+                    apriltag_detection_destroy(det1);
+                    zarray_remove_index(detections, i1, 1);
+                    i1--; // retry the same index
+                    goto retry1;
+                } else {
+                    // keep det1, destroy det0
+                    apriltag_detection_destroy(det0);
+                    zarray_remove_index(detections, i0, 1);
+                    i0--; // retry the same index.
+                    goto retry0;
+                }
+            }
+
+          retry1: ;
+        }
+
+      retry0: ;
+    }
+}
+
 zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
 {
     if (zarray_size(td->tag_families) == 0) {
@@ -1201,65 +1268,7 @@
         zarray_t *poly0 = g2d_polygon_create_zeros(4);
         zarray_t *poly1 = g2d_polygon_create_zeros(4);
 
-        for (int i0 = 0; i0 < zarray_size(detections); i0++) {
-
-            apriltag_detection_t *det0;
-            zarray_get(detections, i0, &det0);
-
-            for (int k = 0; k < 4; k++)
-                zarray_set(poly0, k, det0->p[k], NULL);
-
-            for (int i1 = i0+1; i1 < zarray_size(detections); i1++) {
-
-                apriltag_detection_t *det1;
-                zarray_get(detections, i1, &det1);
-
-                if (det0->id != det1->id || det0->family != det1->family)
-                    continue;
-
-                for (int k = 0; k < 4; k++)
-                    zarray_set(poly1, k, det1->p[k], NULL);
-
-                if (g2d_polygon_overlaps_polygon(poly0, poly1)) {
-                    // the tags overlap. Delete one, keep the other.
-
-                    int pref = 0; // 0 means undecided which one we'll keep.
-                    pref = prefer_smaller(pref, det0->hamming, det1->hamming);     // want small hamming
-                    pref = prefer_smaller(pref, -det0->decision_margin, -det1->decision_margin);      // want bigger margins
-
-                    // if we STILL don't prefer one detection over the other, then pick
-                    // any deterministic criterion.
-                    for (int i = 0; i < 4; i++) {
-                        pref = prefer_smaller(pref, det0->p[i][0], det1->p[i][0]);
-                        pref = prefer_smaller(pref, det0->p[i][1], det1->p[i][1]);
-                    }
-
-                    if (pref == 0) {
-                        // at this point, we should only be undecided if the tag detections
-                        // are *exactly* the same. How would that happen?
-                        debug_print("uh oh, no preference for overlappingdetection\n");
-                    }
-
-                    if (pref < 0) {
-                        // keep det0, destroy det1
-                        apriltag_detection_destroy(det1);
-                        zarray_remove_index(detections, i1, 1);
-                        i1--; // retry the same index
-                        goto retry1;
-                    } else {
-                        // keep det1, destroy det0
-                        apriltag_detection_destroy(det0);
-                        zarray_remove_index(detections, i0, 1);
-                        i0--; // retry the same index.
-                        goto retry0;
-                    }
-                }
-
-              retry1: ;
-            }
-
-          retry0: ;
-        }
+        reconcile_detections(detections, poly0, poly1);
 
         zarray_destroy(poly0);
         zarray_destroy(poly1);
diff --git a/third_party/apriltag/apriltag_quad_thresh.c b/third_party/apriltag/apriltag_quad_thresh.c
index 1ce73ad..5811770 100644
--- a/third_party/apriltag/apriltag_quad_thresh.c
+++ b/third_party/apriltag/apriltag_quad_thresh.c
@@ -262,7 +262,15 @@
         *mse = eig_small;
 }
 
-float pt_compare_angle(struct pt *a, struct pt *b) {
+float pt_compare_angle(float cx, float cy, struct pt *a, struct pt *b) {
+    if (a->slope == b->slope) {
+      float dxa = a->x - cx;
+      float dya = a->y - cy;
+      float dxb = b->x - cx;
+      float dyb = b->y - cy;
+      return dya * dxb - dxa * dyb;
+    }
+
     return a->slope - b->slope;
 }
 
@@ -354,7 +362,7 @@
             for (int i = 0; i < fsz; i++) {
                 acc += errs[(iy + i - fsz / 2 + sz) % sz] * f[i];
             }
-            y[iy] = acc;
+            y[iy] = max(0.0, acc);
         }
 
         memcpy(errs, y, sizeof(double)*sz);
@@ -367,7 +375,7 @@
     int nmaxima = 0;
 
     for (int i = 0; i < sz; i++) {
-        if (errs[i] > errs[(i+1)%sz] && errs[i] > errs[(i+sz-1)%sz]) {
+        if (errs[i] > errs[(i+1)%sz] && errs[i] > errs[(i+sz-1)%sz] && errs[i] > 1e-5) {
             maxima[nmaxima] = i;
             maxima_errs[nmaxima] = errs[i];
             nmaxima++;
@@ -409,7 +417,7 @@
 
     double err01, err12, err23, err30;
     double mse01, mse12, mse23, mse30;
-    double params01[4], params12[4], params23[4], params30[4];
+    double params01[4], params12[4];
 
     // disallow quads where the angle is less than a critical value.
     double max_dot = td->qtp.cos_critical_rad; //25*M_PI/180);
@@ -439,11 +447,11 @@
                 for (int m3 = m2+1; m3 < nmaxima; m3++) {
                     int i3 = maxima[m3];
 
-                    fit_line(lfps, sz, i2, i3, params23, &err23, &mse23);
+                    fit_line(lfps, sz, i2, i3, NULL, &err23, &mse23);
                     if (mse23 > td->qtp.max_line_fit_mse)
                         continue;
 
-                    fit_line(lfps, sz, i3, i0, params30, &err30, &mse30);
+                    fit_line(lfps, sz, i3, i0, NULL, &err30, &mse30);
                     if (mse30 > td->qtp.max_line_fit_mse)
                         continue;
 
@@ -605,7 +613,7 @@
             double x = p->x * .5 + delta;
             double y = p->y * .5 + delta;
             int ix = x, iy = y;
-            double W = 1;
+            int W = 1;
 
             if (ix > 0 && ix+1 < im->width && iy > 0 && iy+1 < im->height) {
                 int grad_x = im->buf[iy * im->stride + ix + 1] -
@@ -615,7 +623,7 @@
                     im->buf[(iy-1) * im->stride + ix];
 
                 // XXX Tunable. How to shape the gradient magnitude?
-                W = sqrt(grad_x*grad_x + grad_y*grad_y) + 1;
+                W = hypotf(grad_x, grad_y) + 1;
             }
 
             double fx = x, fy = y;
@@ -630,10 +638,10 @@
     return lfps;
 }
 
-static inline void ptsort(struct pt *pts, int sz)
+static inline void ptsort(float cx, float cy, struct pt *pts, int sz)
 {
 #define MAYBE_SWAP(arr,apos,bpos)                                   \
-    if (pt_compare_angle(&(arr[apos]), &(arr[bpos])) > 0) {                        \
+    if (pt_compare_angle(cx, cy, &(arr[apos]), &(arr[bpos])) > 0) {                        \
         tmp = arr[apos]; arr[apos] = arr[bpos]; arr[bpos] = tmp;    \
     };
 
@@ -695,11 +703,11 @@
     struct pt *as = &tmp[0];
     struct pt *bs = &tmp[asz];
 
-    ptsort(as, asz);
-    ptsort(bs, bsz);
+    ptsort(cx, cy, as, asz);
+    ptsort(cx, cy, bs, bsz);
 
     #define MERGE(apos,bpos)                        \
-    if (pt_compare_angle(&(as[apos]), &(bs[bpos])) < 0)        \
+    if (pt_compare_angle(cx, cy, &(as[apos]), &(bs[bpos])) < 0)        \
         pts[outpos++] = as[apos++];             \
     else                                        \
         pts[outpos++] = bs[bpos++];
@@ -820,38 +828,7 @@
     // we now sort the points according to theta. This is a prepatory
     // step for segmenting them into four lines.
     if (1) {
-        ptsort((struct pt*) cluster->data, zarray_size(cluster));
-
-        // remove duplicate points. (A byproduct of our segmentation system.)
-        if (1) {
-            int outpos = 1;
-
-            struct pt *last;
-            zarray_get_volatile(cluster, 0, &last);
-
-            for (int i = 1; i < sz; i++) {
-
-                struct pt *p;
-                zarray_get_volatile(cluster, i, &p);
-
-                if (p->x != last->x || p->y != last->y) {
-
-                    if (i != outpos)  {
-                        struct pt *out;
-                        zarray_get_volatile(cluster, outpos, &out);
-                        memcpy(out, p, sizeof(struct pt));
-                    }
-
-                    outpos++;
-                }
-
-                last = p;
-            }
-
-            cluster->size = outpos;
-            sz = outpos;
-        }
-
+        ptsort(cx, cy, (struct pt*) cluster->data, zarray_size(cluster));
     }
 
     if (sz < 24)
@@ -877,9 +854,10 @@
         int i1 = indices[(i+1)&3];
 
         double err;
-        fit_line(lfps, sz, i0, i1, lines[i], NULL, &err);
+        double mse;
+        fit_line(lfps, sz, i0, i1, lines[i], &err, &mse);
 
-        if (err > td->qtp.max_line_fit_mse) {
+        if (mse > td->qtp.max_line_fit_mse) {
             res = 0;
             goto finish;
         }
@@ -1083,7 +1061,7 @@
         // fit quads to.) A typical point along an edge is added three
         // times (because it has 3 neighbors). The maximum perimeter
         // is 2w+2h.
-        if (zarray_size(cluster) > 3*(2*w+2*h)) {
+        if (zarray_size(cluster) > 2*(2*w+2*h)) {
             continue;
         }
 
@@ -1498,15 +1476,20 @@
     mem_pools[mem_pool_idx] = calloc(mem_chunk_size, sizeof(struct uint64_zarray_entry));
 
     for (int y = y0; y < y1; y++) {
+        bool connected_last = false;
         for (int x = 1; x < w-1; x++) {
+            bool connected = false;
 
             uint8_t v0 = threshim->buf[y*ts + x];
-            if (v0 == 127)
+            if (v0 == 127) {
+                connected_last = false;
                 continue;
+            }
 
             // XXX don't query this until we know we need it?
             uint64_t rep0 = unionfind_get_representative(uf, y*w + x);
             if (unionfind_get_set_size(uf, rep0) < 25) {
+                connected_last = false;
                 continue;
             }
 
@@ -1570,9 +1553,8 @@
                        .y = 2 * y + dy,                                        \
                        .gx = dx * ((int)v1 - v0),                              \
                        .gy = dy * ((int)v1 - v0)};                             \
-        /*fprintf(stderr, "Adding point %d+%d(%llx) -> (%d, %d)\n",              \
-                min(rep0, rep1), max(rep0, rep1), entry->id, p.x, p.y);       */ \
         zarray_add(entry->cluster, &p);                                        \
+        connected = true;                                                      \
       }                                                                        \
     }                                                                          \
   }
@@ -1582,8 +1564,12 @@
             DO_CONN(0, 1);
 
             // do 8 connectivity
+            if (!connected_last) {
             DO_CONN(-1, 1);
+            }
+            connected = false;
             DO_CONN(1, 1);
+            connected_last = connected;
         }
     }
 #undef DO_CONN