Split 2022 vision math code into library
This allows it to be used in target estimator and camera reader in a
addition to blob detector. Will be used for comparing size of projected
blobs to detected blobs, as well as for interpolating between localizer
outputs.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I80a726e93d638106431614459837e8671bcb7251
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