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
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index e694b5e..b943db0 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -21,13 +21,12 @@
: calibration_data_(event_loop),
image_size_(0, 0),
ftrace_(),
- image_callback_(
- event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- },
- chrono::milliseconds(5)),
+ image_callback_(event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
image_annotations_sender_(
@@ -160,6 +159,17 @@
return distortion_factor;
}
+std::vector<cv::Point2f> AprilRoboticsDetector::MakeCornerVector(
+ const apriltag_detection_t *det) {
+ std::vector<cv::Point2f> corner_points;
+ corner_points.emplace_back(det->p[0][0], det->p[0][1]);
+ corner_points.emplace_back(det->p[1][0], det->p[1][1]);
+ corner_points.emplace_back(det->p[2][0], det->p[2][1]);
+ corner_points.emplace_back(det->p[3][0], det->p[3][1]);
+
+ return corner_points;
+}
+
std::vector<AprilRoboticsDetector::Detection> AprilRoboticsDetector::DetectTags(
cv::Mat image, aos::monotonic_clock::time_point eof) {
const aos::monotonic_clock::time_point start_time =
@@ -180,21 +190,25 @@
std::vector<Detection> results;
- std::vector<std::vector<cv::Point2f>> orig_corners_vector;
- std::vector<std::vector<cv::Point2f>> corners_vector;
-
auto builder = image_annotations_sender_.MakeBuilder();
+ std::vector<flatbuffers::Offset<foxglove::PointsAnnotation>> foxglove_corners;
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
if (det->decision_margin > FLAGS_min_decision_margin) {
+ // TODO<jim>: Should we check for top/bottom of image?
if (det->p[0][0] < min_x || det->p[0][0] > max_x ||
det->p[1][0] < min_x || det->p[1][0] > max_x ||
det->p[2][0] < min_x || det->p[2][0] > max_x ||
det->p[3][0] < min_x || det->p[3][0] > max_x) {
VLOG(1) << "Rejecting detection because corner is outside pixel border";
+ // Send rejected corner points in red
+ std::vector<cv::Point2f> rejected_corner_points = MakeCornerVector(det);
+ foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
+ builder.fbb(), eof, rejected_corner_points,
+ std::vector<double>{1.0, 0.0, 0.0, 0.5}));
continue;
}
VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
@@ -211,14 +225,11 @@
info.cx = intrinsics_.at<double>(0, 2);
info.cy = intrinsics_.at<double>(1, 2);
- // Store out the original, pre-undistortion corner points for sending
- std::vector<cv::Point2f> orig_corner_points;
- orig_corner_points.emplace_back(det->p[0][0], det->p[0][1]);
- orig_corner_points.emplace_back(det->p[1][0], det->p[1][1]);
- orig_corner_points.emplace_back(det->p[2][0], det->p[2][1]);
- orig_corner_points.emplace_back(det->p[3][0], det->p[3][1]);
-
- orig_corners_vector.emplace_back(orig_corner_points);
+ // Send original corner points in green
+ std::vector<cv::Point2f> orig_corner_points = MakeCornerVector(det);
+ foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
+ builder.fbb(), eof, orig_corner_points,
+ std::vector<double>{0.0, 1.0, 0.0, 0.5}));
UndistortDetection(det);
@@ -237,13 +248,11 @@
<< " seconds for pose estimation";
VLOG(1) << "Pose err: " << pose_error;
- std::vector<cv::Point2f> corner_points;
- corner_points.emplace_back(det->p[0][0], det->p[0][1]);
- corner_points.emplace_back(det->p[1][0], det->p[1][1]);
- corner_points.emplace_back(det->p[2][0], det->p[2][1]);
- corner_points.emplace_back(det->p[3][0], det->p[3][1]);
-
- corners_vector.emplace_back(corner_points);
+ // Send undistorted corner points in pink
+ std::vector<cv::Point2f> corner_points = MakeCornerVector(det);
+ foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
+ builder.fbb(), eof, corner_points,
+ std::vector<double>{1.0, 0.75, 0.8, 1.0}));
double distortion_factor =
ComputeDistortionFactor(orig_corner_points, corner_points);
@@ -255,9 +264,10 @@
}
}
- const auto annotations_offset = frc971::vision::BuildAnnotations(
- eof, orig_corners_vector, 5.0, builder.fbb());
- builder.CheckOk(builder.Send(annotations_offset));
+ foxglove::ImageAnnotations::Builder annotation_builder(*builder.fbb());
+ const auto corners_offset = builder.fbb()->CreateVector(foxglove_corners);
+ annotation_builder.add_points(corners_offset);
+ builder.CheckOk(builder.Send(annotation_builder.Finish()));
apriltag_detections_destroy(detections);
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index fd371c7..bf9265b 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -40,6 +40,9 @@
// Undistorts the april tag corners using the camera calibration
void UndistortDetection(apriltag_detection_t *det) const;
+ // Helper function to store detection points in vector of Point2f's
+ std::vector<cv::Point2f> MakeCornerVector(const apriltag_detection_t *det);
+
std::vector<Detection> DetectTags(cv::Mat image,
aos::monotonic_clock::time_point eof);