diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 599b4ed..0b96539 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -97,6 +97,7 @@
         ":blob_detector_lib",
         ":calibration_data",
         ":calibration_fbs",
+        ":geometry_lib",
         ":target_estimate_fbs",
         ":target_estimator_lib",
         "//aos:flatbuffer_merge",
@@ -124,6 +125,31 @@
 )
 
 cc_library(
+    name = "geometry_lib",
+    hdrs = [
+        "geometry.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2022:__subpackages__"],
+    deps = [
+        "//aos/util:math",
+        "//third_party:opencv",
+        "@com_github_google_glog//:glog",
+    ],
+)
+
+cc_test(
+    name = "geometry_test",
+    srcs = [
+        "geometry_test.cc",
+    ],
+    deps = [
+        ":geometry_lib",
+        "//aos/testing:googletest",
+    ],
+)
+
+cc_library(
     name = "blob_detector_lib",
     srcs = [
         "blob_detector.cc",
@@ -134,6 +160,7 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2022:__subpackages__"],
     deps = [
+        ":geometry_lib",
         "//aos/network:team_number",
         "//aos/time",
         "//third_party:opencv",
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index 2c68a27..68832fd 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -8,6 +8,7 @@
 #include "aos/time/time.h"
 #include "opencv2/features2d.hpp"
 #include "opencv2/imgproc.hpp"
+#include "y2022/vision/geometry.h"
 
 DEFINE_uint64(red_delta, 100,
               "Required difference between green pixels vs. red");
@@ -84,105 +85,6 @@
   return blob_stats;
 }
 
-namespace {
-
-// Linear equation in the form ax + by = c
-struct Line {
- public:
-  double a, b, c;
-
-  std::optional<cv::Point2d> Intersection(const Line &l) const {
-    // Use Cramer's rule to solve for the intersection
-    const double denominator = Determinant(a, b, l.a, l.b);
-    const double numerator_x = Determinant(c, b, l.c, l.b);
-    const double numerator_y = Determinant(a, c, l.a, l.c);
-
-    std::optional<cv::Point2d> intersection = std::nullopt;
-    // Return nullopt if the denominator is 0, meaning the same slopes
-    if (denominator != 0) {
-      intersection =
-          cv::Point2d(numerator_x / denominator, numerator_y / denominator);
-    }
-
-    return intersection;
-  }
-
- private:  // Determinant of [[a, b], [c, d]]
-  static double Determinant(double a, double b, double c, double d) {
-    return (a * d) - (b * c);
-  }
-};
-
-struct Circle {
- public:
-  cv::Point2d center;
-  double radius;
-
-  static std::optional<Circle> Fit(std::vector<cv::Point2d> centroids) {
-    CHECK_EQ(centroids.size(), 3ul);
-    // For the 3 points, we have 3 equations in the form
-    // (x - h)^2 + (y - k)^2 = r^2
-    // Manipulate them to solve for the center and radius
-    // (x1 - h)^2 + (y1 - k)^2 = r^2 ->
-    // x1^2 + h^2 - 2x1h + y1^2 + k^2 - 2y1k = r^2
-    // Also, (x2 - h)^2 + (y2 - k)^2 = r^2
-    // Subtracting these two, we get
-    // x1^2 - x2^2 - 2h(x1 - x2) + y1^2 - y2^2 - 2k(y1 - y2) = 0 ->
-    // h(x1 - x2) + k(y1 - y2) = (-x1^2 + x2^2 - y1^2 + y2^2) / -2
-    // Doing the same with equations 1 and 3, we get the second linear equation
-    // h(x1 - x3) + k(y1 - y3) = (-x1^2 + x3^2 - y1^2 + y3^2) / -2
-    // Now, we can solve for their intersection and find the center
-    const auto l =
-        Line{centroids[0].x - centroids[1].x, centroids[0].y - centroids[1].y,
-             (-std::pow(centroids[0].x, 2) + std::pow(centroids[1].x, 2) -
-              std::pow(centroids[0].y, 2) + std::pow(centroids[1].y, 2)) /
-                 -2.0};
-    const auto m =
-        Line{centroids[0].x - centroids[2].x, centroids[0].y - centroids[2].y,
-             (-std::pow(centroids[0].x, 2) + std::pow(centroids[2].x, 2) -
-              std::pow(centroids[0].y, 2) + std::pow(centroids[2].y, 2)) /
-                 -2.0};
-    const auto center = l.Intersection(m);
-
-    std::optional<Circle> circle = std::nullopt;
-    if (center) {
-      // Now find the radius
-      const double radius = cv::norm(centroids[0] - *center);
-      circle = Circle{*center, radius};
-    }
-    return circle;
-  }
-
-  double DistanceTo(cv::Point2d p) const {
-    const auto p_prime = TranslateToOrigin(p);
-    // Now, the distance is simply the difference between distance from the
-    // origin to p' and the radius.
-    return std::abs(cv::norm(p_prime) - radius);
-  }
-
-  double AngleOf(cv::Point2d p) const {
-    auto p_prime = TranslateToOrigin(p);
-    // Flip the y because y values go downwards.
-    p_prime.y *= -1;
-    return std::atan2(p_prime.y, p_prime.x);
-  }
-
-  // TODO(milind): handle wrapping around 2pi
-  bool InAngleRange(cv::Point2d p, double theta_min, double theta_max) const {
-    const double theta = AngleOf(p);
-    return (theta >= theta_min && theta <= theta_max);
-  }
-
- private:
-  // Translate the point on the circle
-  // as if the circle's center is the origin (0,0)
-  cv::Point2d TranslateToOrigin(cv::Point2d p) const {
-    return cv::Point2d(p.x - center.x, p.y - center.y);
-  }
-};
-
-}  // namespace
-
 void BlobDetector::FilterBlobs(BlobResult *blob_result) {
   std::vector<std::vector<cv::Point>> filtered_blobs;
   std::vector<BlobStats> filtered_stats;
diff --git a/y2022/vision/geometry.h b/y2022/vision/geometry.h
new file mode 100644
index 0000000..75a6496
--- /dev/null
+++ b/y2022/vision/geometry.h
@@ -0,0 +1,129 @@
+#include "aos/util/math.h"
+#include "glog/logging.h"
+#include "opencv2/core/types.hpp"
+
+namespace y2022::vision {
+
+// Linear equation in the form y = mx + b
+struct SlopeInterceptLine {
+  double m, b;
+
+  inline SlopeInterceptLine(cv::Point2d p, cv::Point2d q) {
+    if (p.x == q.x) {
+      CHECK_EQ(p.y, q.y) << "Can't fit line to infinite slope";
+
+      // If two identical points were passed in, give the slope 0,
+      // with it passing the point.
+      m = 0.0;
+    } else {
+      m = (p.y - q.y) / (p.x - q.x);
+    }
+    // y = mx + b -> b = y - mx
+    b = p.y - (m * p.x);
+  }
+
+  inline double operator()(double x) const { return (m * x) + b; }
+};
+
+// Linear equation in the form ax + by = c
+struct StdFormLine {
+ public:
+  double a, b, c;
+
+  inline std::optional<cv::Point2d> Intersection(const StdFormLine &l) const {
+    // Use Cramer's rule to solve for the intersection
+    const double denominator = Determinant(a, b, l.a, l.b);
+    const double numerator_x = Determinant(c, b, l.c, l.b);
+    const double numerator_y = Determinant(a, c, l.a, l.c);
+
+    std::optional<cv::Point2d> intersection = std::nullopt;
+    // Return nullopt if the denominator is 0, meaning the same slopes
+    if (denominator != 0) {
+      intersection =
+          cv::Point2d(numerator_x / denominator, numerator_y / denominator);
+    }
+
+    return intersection;
+  }
+
+ private:  // Determinant of [[a, b], [c, d]]
+  static inline double Determinant(double a, double b, double c, double d) {
+    return (a * d) - (b * c);
+  }
+};
+
+struct Circle {
+ public:
+  cv::Point2d center;
+  double radius;
+
+  static inline std::optional<Circle> Fit(std::vector<cv::Point2d> points) {
+    CHECK_EQ(points.size(), 3ul);
+    // For the 3 points, we have 3 equations in the form
+    // (x - h)^2 + (y - k)^2 = r^2
+    // Manipulate them to solve for the center and radius
+    // (x1 - h)^2 + (y1 - k)^2 = r^2 ->
+    // x1^2 + h^2 - 2x1h + y1^2 + k^2 - 2y1k = r^2
+    // Also, (x2 - h)^2 + (y2 - k)^2 = r^2
+    // Subtracting these two, we get
+    // x1^2 - x2^2 - 2h(x1 - x2) + y1^2 - y2^2 - 2k(y1 - y2) = 0 ->
+    // h(x1 - x2) + k(y1 - y2) = (-x1^2 + x2^2 - y1^2 + y2^2) / -2
+    // Doing the same with equations 1 and 3, we get the second linear equation
+    // h(x1 - x3) + k(y1 - y3) = (-x1^2 + x3^2 - y1^2 + y3^2) / -2
+    // Now, we can solve for their intersection and find the center
+    const auto l =
+        StdFormLine{points[0].x - points[1].x, points[0].y - points[1].y,
+                    (-std::pow(points[0].x, 2) + std::pow(points[1].x, 2) -
+                     std::pow(points[0].y, 2) + std::pow(points[1].y, 2)) /
+                        -2.0};
+    const auto m =
+        StdFormLine{points[0].x - points[2].x, points[0].y - points[2].y,
+                    (-std::pow(points[0].x, 2) + std::pow(points[2].x, 2) -
+                     std::pow(points[0].y, 2) + std::pow(points[2].y, 2)) /
+                        -2.0};
+    const auto center = l.Intersection(m);
+
+    std::optional<Circle> circle = std::nullopt;
+    if (center) {
+      // Now find the radius
+      const double radius = cv::norm(points[0] - *center);
+      circle = Circle{*center, radius};
+    }
+    return circle;
+  }
+
+  inline double DistanceTo(cv::Point2d p) const {
+    const auto p_prime = TranslateToOrigin(p);
+    // Now, the distance is simply the difference between distance from the
+    // origin to p' and the radius.
+    return std::abs(cv::norm(p_prime) - radius);
+  }
+
+  inline double AngleOf(cv::Point2d p) const {
+    auto p_prime = TranslateToOrigin(p);
+    // Flip the y because y values go downwards.
+    p_prime.y *= -1;
+    return std::atan2(p_prime.y, p_prime.x);
+  }
+
+  inline bool InAngleRange(cv::Point2d p, double theta_min,
+                           double theta_max) const {
+    const double theta = AngleOf(p);
+
+    // Handle the case if the bounds wrap around 2pi
+    const double max_diff = aos::math::NormalizeAngle(theta_max - theta);
+    const double min_diff = aos::math::NormalizeAngle(theta - theta_min);
+
+    return ((theta == theta_max) || (theta == theta_min) ||
+            (std::signbit(min_diff) == std::signbit(max_diff)));
+  }
+
+ private:
+  // Translate the point on the circle
+  // as if the circle's center is the origin (0,0)
+  inline cv::Point2d TranslateToOrigin(cv::Point2d p) const {
+    return cv::Point2d(p.x - center.x, p.y - center.y);
+  }
+};
+
+}  // namespace y2022::vision
diff --git a/y2022/vision/geometry_test.cc b/y2022/vision/geometry_test.cc
new file mode 100644
index 0000000..9d2159b
--- /dev/null
+++ b/y2022/vision/geometry_test.cc
@@ -0,0 +1,106 @@
+#include "y2022/vision/geometry.h"
+
+#include <cmath>
+
+#include "aos/util/math.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace y2022::vision::testing {
+
+TEST(GeometryTest, SlopeInterceptLine) {
+  // Test a normal line
+  {
+    SlopeInterceptLine l({2.0, 3.0}, {4.0, 2.0});
+    EXPECT_DOUBLE_EQ(l.m, -0.5);
+    EXPECT_DOUBLE_EQ(l.b, 4.0);
+    EXPECT_DOUBLE_EQ(l(5), 1.5);
+  }
+  // Test a horizontal line
+  {
+    SlopeInterceptLine l({2.0, 3.0}, {4.0, 3.0});
+    EXPECT_DOUBLE_EQ(l.m, 0.0);
+    EXPECT_DOUBLE_EQ(l.b, 3.0);
+    EXPECT_DOUBLE_EQ(l(1000.0), 3.0);
+  }
+  // Test duplicate points
+  {
+    SlopeInterceptLine l({2.0, 3.0}, {2.0, 3.0});
+    EXPECT_DOUBLE_EQ(l.m, 0.0);
+    EXPECT_DOUBLE_EQ(l.b, 3.0);
+    EXPECT_DOUBLE_EQ(l(1000.0), 3.0);
+  }
+  // Test infinite slope
+  {
+    EXPECT_DEATH(SlopeInterceptLine({2.0, 3.0}, {2.0, 5.0}),
+                 "(.*)infinite slope(.*)");
+  }
+}
+
+TEST(GeometryTest, StdFormLine) {
+  // Test the intersection of normal lines
+  {
+    StdFormLine l{3.0, 2.0, 2.3};
+    StdFormLine m{-2.0, 1.2, -0.3};
+    const cv::Point2d kIntersection = {42.0 / 95.0, 37.0 / 76.0};
+    EXPECT_EQ(*l.Intersection(m), kIntersection);
+    EXPECT_EQ(*m.Intersection(l), kIntersection);
+  }
+  // Test the intersection of parallel lines
+  {
+    StdFormLine l{-3.0, 2.0, -3.7};
+    StdFormLine m{-6.0, 4.0, 0.1};
+    EXPECT_EQ(l.Intersection(m), std::nullopt);
+    EXPECT_EQ(m.Intersection(l), std::nullopt);
+  }
+  // Test the intersection of duplicate lines
+  {
+    StdFormLine l{6.0, -8.0, 0.23};
+    StdFormLine m{6.0, -8.0, 0.23};
+    EXPECT_EQ(l.Intersection(m), std::nullopt);
+    EXPECT_EQ(m.Intersection(l), std::nullopt);
+  }
+}
+
+TEST(GeometryTest, Circle) {
+  // Test fitting a normal circle
+  {
+    auto c = Circle::Fit({{-6.0, 3.2}, {-3.0, 2.0}, {-9.3, 1.4}});
+    EXPECT_TRUE(c.has_value());
+    EXPECT_NEAR(c->center.x, -5.901, 1e-3);
+    EXPECT_NEAR(c->center.y, -0.905, 1e-3);
+    EXPECT_NEAR(c->radius, 4.106, 1e-3);
+
+    // Coordinate systems flipped because of image
+    const cv::Point2d kPoint = {c->center.x - c->radius * std::sqrt(3.0) / 2.0,
+                                c->center.y - c->radius / 2.0};
+    EXPECT_NEAR(c->AngleOf(kPoint), 5.0 * M_PI / 6.0, 1e-5);
+    EXPECT_TRUE(c->InAngleRange(kPoint, 4.0 * M_PI / 6.0, M_PI));
+    EXPECT_FALSE(c->InAngleRange(kPoint, 0, 2.0 * M_PI / 6.0));
+    EXPECT_EQ(c->DistanceTo(kPoint), 0.0);
+
+    const cv::Point2d kZeroPoint = {c->center.x + c->radius, c->center.y};
+    EXPECT_NEAR(c->AngleOf(kZeroPoint), 0.0, 1e-5);
+    EXPECT_TRUE(
+        c->InAngleRange(kZeroPoint, -2.1 * 2.0 * M_PI, -1.9 * 2.0 * M_PI));
+    EXPECT_TRUE(
+        c->InAngleRange(kZeroPoint, 1.9 * 2.0 * M_PI, 2.1 * 2.0 * M_PI));
+    EXPECT_EQ(c->DistanceTo(kZeroPoint), 0.0);
+
+    // Test the distance to another point
+    const cv::Point2d kDoubleDistPoint = {
+        c->center.x - (c->radius * 2.0) * std::sqrt(3.0) / 2.0,
+        c->center.y - (c->radius * 2.0) / 2.0};
+    EXPECT_DOUBLE_EQ(c->DistanceTo(kDoubleDistPoint), c->radius);
+
+    // Distance to center should be radius
+    EXPECT_DOUBLE_EQ(c->DistanceTo(c->center), c->radius);
+  }
+  // Test fitting an invalid circle (duplicate points)
+  {
+    auto c = Circle::Fit({{-6.0, 3.2}, {-3.0, 2.0}, {-6.0, 3.2}});
+    EXPECT_FALSE(c.has_value());
+  }
+}
+
+}  // namespace y2022::vision::testing