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/frc971/vision/geometry_test.cc b/frc971/vision/geometry_test.cc
new file mode 100644
index 0000000..19e54c4
--- /dev/null
+++ b/frc971/vision/geometry_test.cc
@@ -0,0 +1,111 @@
+#include "frc971/vision/geometry.h"
+
+#include <cmath>
+
+#include "aos/util/math.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace frc971::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 frc971::vision::testing