Refactored sending ImageAnnotations to allow corner-by-corner build

Gives us ability to send different colors, line types, thicknesses for each corner set

Made a few things const that probably should have been

Change-Id: Idb50c1c80f4daf37df2a48de225f306c9477d4f8
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index f1846b5..2c35a0b 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -174,9 +174,25 @@
 // Puts the provided charuco corners into a foxglove ImageAnnotation type for
 // visualization purposes.
 flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
+    flatbuffers::FlatBufferBuilder *fbb,
     const aos::monotonic_clock::time_point monotonic_now,
-    const std::vector<std::vector<cv::Point2f>> &corners, double thickness,
-    flatbuffers::FlatBufferBuilder *fbb);
+    const std::vector<std::vector<cv::Point2f>> &corners,
+    const std::vector<double> rgba_color = std::vector<double>{0.0, 1.0, 0.0,
+                                                               1.0},
+    const double thickness = 5,
+    const foxglove::PointsAnnotationType line_type =
+        foxglove::PointsAnnotationType::POINTS);
+
+// Creates a PointsAnnotation to build up ImageAnnotations with different types
+flatbuffers::Offset<foxglove::PointsAnnotation> BuildPointsAnnotation(
+    flatbuffers::FlatBufferBuilder *fbb,
+    const aos::monotonic_clock::time_point monotonic_now,
+    const std::vector<cv::Point2f> &corners,
+    const std::vector<double> rgba_color = std::vector<double>{0.0, 1.0, 0.0,
+                                                               1.0},
+    const double thickness = 5,
+    const foxglove::PointsAnnotationType line_type =
+        foxglove::PointsAnnotationType::POINTS);
 
 }  // namespace vision
 }  // namespace frc971