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/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 4b59406..132bb4b 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -89,7 +89,8 @@
std::vector<std::vector<cv::Point2f>> charuco_corners) {
auto builder = annotations_sender_.MakeBuilder();
builder.CheckOk(builder.Send(
- BuildAnnotations(eof, charuco_corners, 2.0, builder.fbb())));
+ BuildAnnotations(builder.fbb(), eof, charuco_corners,
+ std::vector<double>{0.0, 1.0, 0.0, 1.0}, 2.0)));
}
private:
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 2f9e81a..ac708f6 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -456,35 +456,15 @@
}
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, const double thickness,
+ const foxglove::PointsAnnotationType line_type) {
std::vector<flatbuffers::Offset<foxglove::PointsAnnotation>> rectangles;
- const struct timespec now_t = aos::time::to_timespec(monotonic_now);
- foxglove::Time time{static_cast<uint32_t>(now_t.tv_sec),
- static_cast<uint32_t>(now_t.tv_nsec)};
- // Draw the points in pink
- const flatbuffers::Offset<foxglove::Color> color_offset =
- foxglove::CreateColor(*fbb, 1.0, 0.75, 0.8, 1.0);
for (const std::vector<cv::Point2f> &rectangle : corners) {
- std::vector<flatbuffers::Offset<foxglove::Point2>> points_offsets;
- for (const cv::Point2f &point : rectangle) {
- points_offsets.push_back(foxglove::CreatePoint2(*fbb, point.x, point.y));
- }
- const flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<foxglove::Point2>>>
- points_offset = fbb->CreateVector(points_offsets);
- std::vector<flatbuffers::Offset<foxglove::Color>> color_offsets(
- points_offsets.size(), color_offset);
- auto colors_offset = fbb->CreateVector(color_offsets);
- foxglove::PointsAnnotation::Builder points_builder(*fbb);
- points_builder.add_timestamp(&time);
- points_builder.add_type(foxglove::PointsAnnotationType::POINTS);
- points_builder.add_points(points_offset);
- points_builder.add_outline_color(color_offset);
- points_builder.add_outline_colors(colors_offset);
- points_builder.add_thickness(thickness);
- rectangles.push_back(points_builder.Finish());
+ rectangles.push_back(BuildPointsAnnotation(
+ fbb, monotonic_now, rectangle, rgba_color, thickness, line_type));
}
const auto rectangles_offset = fbb->CreateVector(rectangles);
@@ -493,6 +473,39 @@
return annotation_builder.Finish();
}
+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, const double thickness,
+ const foxglove::PointsAnnotationType line_type) {
+ const struct timespec now_t = aos::time::to_timespec(monotonic_now);
+ foxglove::Time time{static_cast<uint32_t>(now_t.tv_sec),
+ static_cast<uint32_t>(now_t.tv_nsec)};
+ const flatbuffers::Offset<foxglove::Color> color_offset =
+ foxglove::CreateColor(*fbb, rgba_color[0], rgba_color[1], rgba_color[2],
+ rgba_color[3]);
+ std::vector<flatbuffers::Offset<foxglove::Point2>> points_offsets;
+ for (const cv::Point2f &point : corners) {
+ points_offsets.push_back(foxglove::CreatePoint2(*fbb, point.x, point.y));
+ }
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<foxglove::Point2>>>
+ points_offset = fbb->CreateVector(points_offsets);
+ std::vector<flatbuffers::Offset<foxglove::Color>> color_offsets(
+ points_offsets.size(), color_offset);
+ auto colors_offset = fbb->CreateVector(color_offsets);
+ foxglove::PointsAnnotation::Builder points_builder(*fbb);
+ points_builder.add_timestamp(&time);
+ points_builder.add_type(line_type);
+ points_builder.add_points(points_offset);
+ points_builder.add_outline_color(color_offset);
+ points_builder.add_outline_colors(colors_offset);
+ points_builder.add_thickness(thickness);
+
+ return points_builder.Finish();
+}
+
TargetType TargetTypeFromString(std::string_view str) {
if (str == "aruco") {
return TargetType::kAruco;
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