Send april corners for foxglove displaying
Deleted AprilDebug and switched to foxglove image annotations.
Also made the displayed points a more visible color (pink).
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ifb89784d00c80406237587acf17c40424632069b
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 93d68a5..4b59406 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -88,8 +88,8 @@
void HandleCharuco(const aos::monotonic_clock::time_point eof,
std::vector<std::vector<cv::Point2f>> charuco_corners) {
auto builder = annotations_sender_.MakeBuilder();
- builder.CheckOk(
- builder.Send(BuildAnnotations(eof, charuco_corners, builder.fbb())));
+ builder.CheckOk(builder.Send(
+ BuildAnnotations(eof, charuco_corners, 2.0, builder.fbb())));
}
private:
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index dbff4db..2f9e81a 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -457,14 +457,15 @@
flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
const aos::monotonic_clock::time_point monotonic_now,
- const std::vector<std::vector<cv::Point2f>> &corners,
+ const std::vector<std::vector<cv::Point2f>> &corners, double thickness,
flatbuffers::FlatBufferBuilder *fbb) {
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, 0.0, 1.0, 0.0, 1.0);
+ 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) {
@@ -482,7 +483,7 @@
points_builder.add_points(points_offset);
points_builder.add_outline_color(color_offset);
points_builder.add_outline_colors(colors_offset);
- points_builder.add_thickness(2.0);
+ points_builder.add_thickness(thickness);
rectangles.push_back(points_builder.Finish());
}
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 40b1601..f1846b5 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -175,7 +175,7 @@
// visualization purposes.
flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
const aos::monotonic_clock::time_point monotonic_now,
- const std::vector<std::vector<cv::Point2f>> &corners,
+ const std::vector<std::vector<cv::Point2f>> &corners, double thickness,
flatbuffers::FlatBufferBuilder *fbb);
} // namespace vision
diff --git a/y2023/BUILD b/y2023/BUILD
index 1c4ef71..3824ba1 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -110,7 +110,6 @@
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
- "//y2023/vision:april_debug_fbs",
"@com_github_foxglove_schemas//:schemas",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 419217f..a0b0ce5 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,5 +1,3 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-
cc_binary(
name = "camera_reader",
srcs = [
@@ -33,7 +31,6 @@
"//frc971/constants:constants_sender_lib",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2023/vision:april_debug_fbs",
"//y2023/vision:vision_util",
"@com_google_absl//absl/strings",
],
@@ -75,14 +72,6 @@
],
)
-flatbuffer_cc_library(
- name = "april_debug_fbs",
- srcs = ["april_debug.fbs"],
- gen_reflections = 1,
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
-
cc_library(
name = "aprilrobotics_lib",
srcs = [
@@ -92,7 +81,6 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
- ":april_debug_fbs",
":vision_util",
"//aos:init",
"//aos/events:shm_event_loop",
diff --git a/y2023/vision/april_debug.fbs b/y2023/vision/april_debug.fbs
deleted file mode 100644
index 4442448..0000000
--- a/y2023/vision/april_debug.fbs
+++ /dev/null
@@ -1,21 +0,0 @@
-namespace y2023.vision;
-
-// Stores image xy pixel coordinates of apriltag corners
-struct Point {
- x:double (id: 0);
- y:double (id: 1);
-}
-
-// Corner locations for each apriltag
-table AprilCorners {
- id:uint64 (id: 0);
- // Will always have 4 values, one for each corner
- points: [Point] (id: 1);
-}
-
-// List of positions of all apriltags detected in current frame
-table AprilDebug {
- corners: [AprilCorners] (id: 0);
-}
-
-root_type AprilDebug;
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 07eee64..b132f85 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -27,8 +27,8 @@
chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
- april_debug_sender_(
- event_loop->MakeSender<y2023::vision::AprilDebug>("/camera")) {
+ image_annotations_sender_(
+ event_loop->MakeSender<foxglove::ImageAnnotations>("/camera")) {
tag_family_ = tag16h5_create();
tag_detector_ = apriltag_detector_create();
@@ -80,7 +80,7 @@
void AprilRoboticsDetector::HandleImage(cv::Mat image_grayscale,
aos::monotonic_clock::time_point eof) {
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
- DetectTags(image_grayscale);
+ DetectTags(image_grayscale, eof);
auto builder = target_map_sender_.MakeBuilder();
std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
@@ -136,7 +136,8 @@
}
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
-AprilRoboticsDetector::DetectTags(cv::Mat image) {
+AprilRoboticsDetector::DetectTags(cv::Mat image,
+ aos::monotonic_clock::time_point eof) {
const aos::monotonic_clock::time_point start_time =
aos::monotonic_clock::now();
@@ -153,9 +154,9 @@
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
- std::vector<flatbuffers::Offset<AprilCorners>> corners_vector;
+ std::vector<std::vector<cv::Point2f>> corners_vector;
- auto builder = april_debug_sender_.MakeBuilder();
+ auto builder = image_annotations_sender_.MakeBuilder();
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
@@ -195,33 +196,19 @@
.count()
<< " seconds for pose estimation";
- std::vector<Point> corner_points;
-
+ 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]);
- auto corner_points_fbs =
- builder.fbb()->CreateVectorOfStructs(corner_points);
-
- AprilCorners::Builder april_corners_builder =
- builder.MakeBuilder<AprilCorners>();
-
- april_corners_builder.add_id(det->id);
- april_corners_builder.add_points(corner_points_fbs);
-
- corners_vector.emplace_back(april_corners_builder.Finish());
+ corners_vector.emplace_back(corner_points);
}
}
- auto corners_vector_fbs = builder.fbb()->CreateVector(corners_vector);
-
- AprilDebug::Builder april_debug_builder = builder.MakeBuilder<AprilDebug>();
-
- april_debug_builder.add_corners(corners_vector_fbs);
-
- builder.CheckOk(builder.Send(april_debug_builder.Finish()));
+ const auto annotations_offset =
+ frc971::vision::BuildAnnotations(eof, corners_vector, 5.0, builder.fbb());
+ builder.CheckOk(builder.Send(annotations_offset));
apriltag_detections_destroy(detections);
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 66b0aac..bbc1661 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -17,7 +17,6 @@
#include "third_party/apriltag/apriltag_pose.h"
#include "third_party/apriltag/tag16h5.h"
#include "y2023/constants/constants_generated.h"
-#include "y2023/vision/april_debug_generated.h"
DECLARE_int32(team_number);
@@ -36,7 +35,7 @@
void UndistortDetection(apriltag_detection_t *det) const;
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
- cv::Mat image);
+ cv::Mat image, aos::monotonic_clock::time_point eof);
const std::optional<cv::Mat> extrinsics() const { return extrinsics_; }
const cv::Mat intrinsics() const { return intrinsics_; }
@@ -64,7 +63,7 @@
frc971::vision::ImageCallback image_callback_;
aos::Sender<frc971::vision::TargetMap> target_map_sender_;
- aos::Sender<y2023::vision::AprilDebug> april_debug_sender_;
+ aos::Sender<foxglove::ImageAnnotations> image_annotations_sender_;
};
} // namespace vision
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 7877a57..4fb96bc 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -10,7 +10,6 @@
#include "frc971/vision/vision_generated.h"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
-#include "y2023/vision/april_debug_generated.h"
#include "y2023/vision/vision_util.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -28,10 +27,8 @@
using frc971::vision::CameraImage;
bool DisplayLoop(const cv::Mat intrinsics, const cv::Mat dist_coeffs,
- aos::Fetcher<CameraImage> *image_fetcher,
- aos::Fetcher<AprilDebug> *april_debug_fetcher) {
+ aos::Fetcher<CameraImage> *image_fetcher) {
const CameraImage *image;
- std::optional<const AprilDebug *> april_debug = std::nullopt;
// Read next image
if (!image_fetcher->Fetch()) {
@@ -41,12 +38,6 @@
image = image_fetcher->get();
CHECK(image != nullptr) << "Couldn't read image";
- if (april_debug_fetcher->Fetch()) {
- april_debug = april_debug_fetcher->get();
- } else {
- VLOG(2) << "Couldn't fetch next target map";
- }
-
// Create color image:
cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
(void *)image->data()->data());
@@ -66,17 +57,6 @@
cv::Mat undistorted_image;
cv::undistort(bgr_image, undistorted_image, intrinsics, dist_coeffs);
-
- if (april_debug.has_value() && april_debug.value()->corners()->size() > 0) {
- for (const auto *corners : *april_debug.value()->corners()) {
- std::vector<cv::Point> points;
- for (const auto *point_fbs : *corners->points()) {
- points.emplace_back(point_fbs->x(), point_fbs->y());
- }
- cv::polylines(undistorted_image, points, true, cv::Scalar(255, 0, 0), 10);
- }
- }
-
cv::imshow("Display", undistorted_image);
int keystroke = cv::waitKey(1);
@@ -109,14 +89,11 @@
aos::Fetcher<CameraImage> image_fetcher =
event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
- aos::Fetcher<AprilDebug> april_debug_fetcher =
- event_loop.MakeFetcher<AprilDebug>("/camera");
// Run the display loop
event_loop.AddPhasedLoop(
[&](int) {
- if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher,
- &april_debug_fetcher)) {
+ if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher)) {
LOG(INFO) << "Calling event_loop Exit";
event_loop.Exit();
};
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index b6eaf04..9abc08d 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -217,15 +217,6 @@
]
},
{
- "name": "/pi{{ NUM }}/camera",
- "type": "y2023.vision.AprilDebug",
- "source_node": "pi{{ NUM }}",
- "frequency": 40,
- "num_senders": 2,
- "max_size": 40000,
- "logger": "LOCAL_LOGGER"
- },
- {
"name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,