Add april tag corner display to viewer
Until we have this working with foxglove...
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I6a73d32eeecb4d1ac9359215c96d82827f627ac7
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 3186f44..839f11e 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -30,8 +30,11 @@
"//aos:init",
"//aos:json_to_flatbuffer",
"//aos/events:shm_event_loop",
+ "//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",
],
)
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 68495b1..7877a57 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -6,7 +6,12 @@
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/time/time.h"
+#include "frc971/constants/constants_sender_lib.h"
#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.");
DEFINE_string(channel, "/camera", "Channel name for the image.");
@@ -16,23 +21,32 @@
DEFINE_int32(rate, 100, "Time in milliseconds to wait between images");
-namespace frc971 {
+namespace y2023 {
namespace vision {
namespace {
-aos::Fetcher<CameraImage> image_fetcher;
-bool DisplayLoop() {
+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) {
const CameraImage *image;
+ std::optional<const AprilDebug *> april_debug = std::nullopt;
// Read next image
- if (!image_fetcher.Fetch()) {
+ if (!image_fetcher->Fetch()) {
VLOG(2) << "Couldn't fetch next image";
return true;
}
-
- image = image_fetcher.get();
+ 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());
@@ -41,7 +55,8 @@
if (!FLAGS_capture.empty()) {
if (absl::EndsWith(FLAGS_capture, ".bfbs")) {
- aos::WriteFlatbufferToFile(FLAGS_capture, image_fetcher.CopyFlatBuffer());
+ aos::WriteFlatbufferToFile(FLAGS_capture,
+ image_fetcher->CopyFlatBuffer());
} else {
cv::imwrite(FLAGS_capture, bgr_image);
}
@@ -49,7 +64,21 @@
return false;
}
- cv::imshow("Display", bgr_image);
+ 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);
if ((keystroke & 0xFF) == static_cast<int>('c')) {
// Convert again, to get clean image
@@ -68,14 +97,26 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
+ frc971::constants::WaitForConstants<Constants>(&config.message());
+
aos::ShmEventLoop event_loop(&config.message());
- image_fetcher = event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
+ const auto *calibration_data = FindCameraCalibration(
+ constants_fetcher.constants(), event_loop.node()->name()->string_view());
+ const cv::Mat intrinsics = CameraIntrinsics(calibration_data);
+ const cv::Mat dist_coeffs = CameraDistCoeffs(calibration_data);
+
+ 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(
- [&event_loop](int) {
- if (!DisplayLoop()) {
+ [&](int) {
+ if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher,
+ &april_debug_fetcher)) {
LOG(INFO) << "Calling event_loop Exit";
event_loop.Exit();
};
@@ -89,9 +130,9 @@
} // namespace
} // namespace vision
-} // namespace frc971
+} // namespace y2023
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
- frc971::vision::ViewerMain();
+ y2023::vision::ViewerMain();
}