Move geometry lib to frc971/vision
Going to use for target mapping. Previously used this for 2022 vision.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Ifd9d13d3f9b0ea1aac42fb6c96ef2a5062b9f637
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 6325234..233465e 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -121,7 +121,6 @@
":blob_detector_lib",
":calibration_data",
":calibration_fbs",
- ":geometry_lib",
":target_estimate_fbs",
":target_estimator_lib",
"//aos:flatbuffer_merge",
@@ -217,31 +216,6 @@
)
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",
@@ -252,9 +226,9 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2022:__subpackages__"],
deps = [
- ":geometry_lib",
"//aos/network:team_number",
"//aos/time",
+ "//frc971/vision:geometry_lib",
"//third_party:opencv",
],
)
@@ -276,6 +250,7 @@
"//aos/logging",
"//aos/time",
"//frc971/control_loops:quaternion_utils",
+ "//frc971/vision:geometry_lib",
"//third_party:opencv",
"//y2022:constants",
"@com_google_absl//absl/strings:str_format",
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index 2f90513..3893ed6 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -6,10 +6,10 @@
#include "aos/network/team_number.h"
#include "aos/time/time.h"
+#include "frc971/vision/geometry.h"
#include "opencv2/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
-#include "y2022/vision/geometry.h"
DEFINE_bool(
use_outdoors, true,
@@ -140,7 +140,7 @@
constexpr double kMaxBlobAngle = M_PI - kMinBlobAngle;
std::vector<std::vector<cv::Point>> blob_circle;
std::vector<BlobStats> blob_circle_stats;
- Circle circle;
+ frc971::vision::Circle circle;
// If we see more than this number of blobs after filtering based on
// color/size, the circle fit may detect noise so just return no blobs.
@@ -165,9 +165,10 @@
filtered_blobs[j], filtered_blobs[k], filtered_blobs[l]};
std::vector<BlobStats> current_stats{filtered_stats[j], filtered_stats[k],
filtered_stats[l]};
- const std::optional<Circle> current_circle =
- Circle::Fit({current_stats[0].centroid, current_stats[1].centroid,
- current_stats[2].centroid});
+ const std::optional<frc971::vision::Circle> current_circle =
+ frc971::vision::Circle::Fit({current_stats[0].centroid,
+ current_stats[1].centroid,
+ current_stats[2].centroid});
// Make sure that a circle could be created from the points
if (!current_circle) {
diff --git a/y2022/vision/geometry.h b/y2022/vision/geometry.h
deleted file mode 100644
index c27eb4d..0000000
--- a/y2022/vision/geometry.h
+++ /dev/null
@@ -1,136 +0,0 @@
-#ifndef Y2022_VISION_GEOMETRY_H_
-#define Y2022_VISION_GEOMETRY_H_
-
-#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);
- }
-
- // Expects all angles to be from 0 to 2pi
- // TODO(milind): handle wrapping
- static inline bool AngleInRange(double theta, double theta_min,
- double theta_max) {
- return (
- (theta >= theta_min && theta <= theta_max) ||
- (theta_min > theta_max && (theta >= theta_min || theta <= theta_max)));
- }
-
- inline bool InAngleRange(cv::Point2d p, double theta_min,
- double theta_max) const {
- return AngleInRange(AngleOf(p), theta_min, theta_max);
- }
-
- 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
-
-#endif // Y2022_VISION_GEOMETRY_H_
diff --git a/y2022/vision/geometry_test.cc b/y2022/vision/geometry_test.cc
deleted file mode 100644
index 0a5a759..0000000
--- a/y2022/vision/geometry_test.cc
+++ /dev/null
@@ -1,111 +0,0 @@
-#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.0 * M_PI) - 0.1, 0.1));
- 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());
- }
- // Test if angles are in ranges
- {
- EXPECT_TRUE(Circle::AngleInRange(0.5, 0.4, 0.6));
- EXPECT_TRUE(Circle::AngleInRange(0, (2.0 * M_PI) - 0.2, 0.2));
- EXPECT_FALSE(
- Circle::AngleInRange(0, (2.0 * M_PI) - 0.2, (2.0 * M_PI) - 0.1));
- EXPECT_TRUE(Circle::AngleInRange(0.5, (2.0 * M_PI) - 0.1, 0.6));
- }
-}
-
-} // namespace y2022::vision::testing
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 1447d81..9bbaf75 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -4,7 +4,7 @@
#include "aos/time/time.h"
#include "ceres/ceres.h"
#include "frc971/control_loops/quaternion_utils.h"
-#include "geometry.h"
+#include "frc971/vision/geometry.h"
#include "opencv2/core/core.hpp"
#include "opencv2/core/eigen.hpp"
#include "opencv2/features2d.hpp"
@@ -156,9 +156,9 @@
CHECK_GE(blob_stats_.size(), 3) << "Expected at least 3 blobs";
- const auto circle =
- Circle::Fit({blob_stats_[0].centroid, blob_stats_[1].centroid,
- blob_stats_[2].centroid});
+ const auto circle = frc971::vision::Circle::Fit({blob_stats_[0].centroid,
+ blob_stats_[1].centroid,
+ blob_stats_[2].centroid});
CHECK(circle.has_value());
max_blob_area_ = 0.0;