Jim Ostrowski | c560cbe | 2020-03-07 00:29:30 -0800 | [diff] [blame] | 1 | #include <map> |
Philipp Schrader | 790cb54 | 2023-07-05 21:06:52 -0700 | [diff] [blame] | 2 | #include <random> |
| 3 | |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 4 | #include "absl/flags/flag.h" |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 5 | #include <opencv2/calib3d.hpp> |
| 6 | #include <opencv2/features2d.hpp> |
| 7 | #include <opencv2/highgui/highgui.hpp> |
| 8 | #include <opencv2/imgproc.hpp> |
| 9 | |
| 10 | #include "aos/events/shm_event_loop.h" |
| 11 | #include "aos/init.h" |
Jim Ostrowski | baa4369 | 2020-03-08 16:25:10 -0700 | [diff] [blame] | 12 | #include "aos/time/time.h" |
Jim Ostrowski | 977850f | 2022-01-22 21:04:22 -0800 | [diff] [blame] | 13 | #include "frc971/vision/v4l2_reader.h" |
| 14 | #include "frc971/vision/vision_generated.h" |
Jim Ostrowski | c560cbe | 2020-03-07 00:29:30 -0800 | [diff] [blame] | 15 | #include "y2020/vision/sift/sift_generated.h" |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 16 | |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 17 | ABSL_FLAG(std::string, config, "aos_config.json", |
| 18 | "Path to the config file to use."); |
| 19 | ABSL_FLAG(bool, show_features, true, "Show the SIFT features that matched."); |
| 20 | ABSL_FLAG(std::string, channel, "/camera", "Channel name for the image."); |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 21 | |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 22 | ABSL_FLAG(std::string, capture, "", |
| 23 | "If set, capture a single image and save it to this filename."); |
Austin Schuh | 5776312 | 2021-11-06 20:49:18 -0700 | [diff] [blame] | 24 | |
Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 25 | namespace frc971::vision { |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 26 | namespace { |
| 27 | |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 28 | aos::Fetcher<CameraImage> image_fetcher; |
| 29 | aos::Fetcher<sift::ImageMatchResult> match_fetcher; |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 30 | cv::Mat palette_; |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 31 | |
| 32 | bool DisplayLoop() { |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 33 | // Try to get target match data |
| 34 | int64_t match_timestamp = 0; |
| 35 | const sift::ImageMatchResult *match; |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 36 | if (match_fetcher.Fetch()) { |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 37 | match = match_fetcher.get(); |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 38 | CHECK(match != nullptr) << "Got null when trying to fetch match result"; |
| 39 | |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 40 | match_timestamp = match->image_monotonic_timestamp_ns(); |
| 41 | if (match->camera_poses() != nullptr && match->camera_poses()->size() > 0) { |
| 42 | VLOG(2) << "Got matches for timestamp " << match_timestamp << "\n"; |
| 43 | } |
| 44 | } else { |
| 45 | VLOG(2) << "Didn't get match this cycle"; |
| 46 | } |
| 47 | |
| 48 | int64_t image_timestamp = 0; |
| 49 | bool matching_image_found = false; |
| 50 | const CameraImage *image; |
| 51 | while (!matching_image_found) { |
| 52 | // Read next image |
| 53 | if (!image_fetcher.FetchNext()) { |
| 54 | VLOG(2) << "Couldn't fetch next image"; |
| 55 | return true; |
| 56 | } |
| 57 | |
| 58 | image = image_fetcher.get(); |
| 59 | CHECK(image != nullptr) << "Couldn't read image"; |
| 60 | image_timestamp = image->monotonic_timestamp_ns(); |
| 61 | VLOG(2) << "Got image at timestamp: " << image_timestamp; |
| 62 | |
| 63 | if (match_timestamp != 0 && image_timestamp == match_timestamp) { |
| 64 | matching_image_found = true; |
| 65 | } else if (image_timestamp > match_timestamp) { |
| 66 | LOG(INFO) << "Image timestamp went past match_timestamp"; |
| 67 | return true; |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 68 | } |
| 69 | } |
| 70 | |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 71 | // Create color image: |
| 72 | cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2, |
| 73 | (void *)image->data()->data()); |
| 74 | cv::Mat rgb_image(cv::Size(image->cols(), image->rows()), CV_8UC3); |
Brian Silverman | 4c7235a | 2021-11-17 19:04:37 -0800 | [diff] [blame] | 75 | cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV); |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 76 | |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 77 | if (!absl::GetFlag(FLAGS_capture).empty()) { |
| 78 | cv::imwrite(absl::GetFlag(FLAGS_capture), rgb_image); |
Austin Schuh | 5776312 | 2021-11-06 20:49:18 -0700 | [diff] [blame] | 79 | return false; |
| 80 | } |
| 81 | |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 82 | if (matching_image_found) { |
| 83 | // Draw whatever matches we have |
| 84 | if (match->camera_poses() != nullptr && match->camera_poses()->size() > 0) { |
| 85 | // Draw target point in image |
| 86 | float x = match->camera_poses()->Get(0)->query_target_point_x(); |
| 87 | float y = match->camera_poses()->Get(0)->query_target_point_y(); |
| 88 | float radius = match->camera_poses()->Get(0)->query_target_point_radius(); |
| 89 | cv::circle(rgb_image, cv::Point2f(x, y), radius, cv::Scalar(0, 255, 0), |
| 90 | 5); |
| 91 | } |
| 92 | |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 93 | if (absl::GetFlag(FLAGS_show_features) && |
| 94 | match->image_matches() != nullptr && |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 95 | match->image_matches()->size() > 0) { |
| 96 | // Iterate through matches and draw matched keypoints |
| 97 | for (uint model_match_ind = 0; |
| 98 | model_match_ind < match->image_matches()->size(); |
| 99 | model_match_ind++) { |
| 100 | auto match_list = |
| 101 | match->image_matches()->Get(model_match_ind)->matches(); |
| 102 | if (match_list != nullptr && match_list->size() > 0) { |
| 103 | int train_image_ind = |
| 104 | match->image_matches()->Get(model_match_ind)->train_image(); |
| 105 | VLOG(2) << "Got " << match_list->size() << " matches to model " |
| 106 | << train_image_ind; |
| 107 | |
| 108 | // Picking color from palette and drawing |
| 109 | auto color = palette_.at<cv::Vec3b>(train_image_ind % palette_.cols); |
| 110 | LOG(INFO) << "Using color " << color; |
| 111 | for (uint i = 0; i < match_list->size(); i++) { |
| 112 | uint query_feature_ind = match_list->Get(i)->query_feature(); |
| 113 | float kp_x = match->features()->Get(query_feature_ind)->x(); |
| 114 | float kp_y = match->features()->Get(query_feature_ind)->y(); |
| 115 | cv::circle(rgb_image, cv::Point2f(kp_x, kp_y), 5, color, 2); |
| 116 | } |
| 117 | } |
| 118 | } |
| 119 | } |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 120 | } |
| 121 | |
| 122 | cv::imshow("Display", rgb_image); |
| 123 | int keystroke = cv::waitKey(1); |
| 124 | if ((keystroke & 0xFF) == static_cast<int>('c')) { |
| 125 | // Convert again, to get clean image |
Brian Silverman | 4c7235a | 2021-11-17 19:04:37 -0800 | [diff] [blame] | 126 | cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV); |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 127 | std::stringstream name; |
| 128 | name << "capture-" << aos::realtime_clock::now() << ".png"; |
| 129 | cv::imwrite(name.str(), rgb_image); |
| 130 | LOG(INFO) << "Saved image file: " << name.str(); |
| 131 | } else if ((keystroke & 0xFF) == static_cast<int>('q')) { |
| 132 | return false; |
| 133 | } |
| 134 | return true; |
| 135 | } |
| 136 | |
| 137 | void ViewerMain() { |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 138 | // Create random color palette for distinguishing multiple models |
| 139 | uchar colors[5][3] = { |
| 140 | {0, 0, 255}, {0, 165, 255}, {0, 255, 255}, {255, 0, 0}, {128, 0, 128}}; |
| 141 | palette_ = cv::Mat(3, 5, CV_8U, &colors); |
Jim Ostrowski | c560cbe | 2020-03-07 00:29:30 -0800 | [diff] [blame] | 142 | |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 143 | aos::FlatbufferDetachedBuffer<aos::Configuration> config = |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 144 | aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config)); |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 145 | |
| 146 | aos::ShmEventLoop event_loop(&config.message()); |
| 147 | |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 148 | image_fetcher = |
| 149 | event_loop.MakeFetcher<CameraImage>(absl::GetFlag(FLAGS_channel)); |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 150 | |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 151 | // If we want to show the features, we have to use the detailed channel |
Austin Schuh | 99f7c6a | 2024-06-25 22:07:44 -0700 | [diff] [blame^] | 152 | std::string result_channel = absl::GetFlag(FLAGS_channel); |
| 153 | if (absl::GetFlag(FLAGS_show_features)) { |
Jim Ostrowski | 56c0932 | 2021-10-23 16:17:21 -0700 | [diff] [blame] | 154 | result_channel += "/detailed"; |
| 155 | } |
| 156 | match_fetcher = |
| 157 | event_loop.MakeFetcher<sift::ImageMatchResult>(result_channel); |
Jim Ostrowski | baa4369 | 2020-03-08 16:25:10 -0700 | [diff] [blame] | 158 | |
Jim Ostrowski | 834dddf | 2021-04-10 14:40:19 -0700 | [diff] [blame] | 159 | // Run the display loop |
| 160 | event_loop.AddPhasedLoop( |
| 161 | [&event_loop](int) { |
| 162 | if (!DisplayLoop()) { |
| 163 | LOG(INFO) << "Calling event_loop Exit"; |
| 164 | event_loop.Exit(); |
| 165 | }; |
| 166 | }, |
| 167 | ::std::chrono::milliseconds(100)); |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 168 | |
| 169 | event_loop.Run(); |
Austin Schuh | 5776312 | 2021-11-06 20:49:18 -0700 | [diff] [blame] | 170 | |
| 171 | image_fetcher = aos::Fetcher<CameraImage>(); |
| 172 | match_fetcher = aos::Fetcher<sift::ImageMatchResult>(); |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 173 | } |
| 174 | |
| 175 | } // namespace |
Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 176 | } // namespace frc971::vision |
Jim Ostrowski | 23eb658 | 2020-03-04 23:15:32 -0800 | [diff] [blame] | 177 | |
| 178 | // Quick and lightweight grayscale viewer for images |
| 179 | int main(int argc, char **argv) { |
| 180 | aos::InitGoogle(&argc, &argv); |
| 181 | frc971::vision::ViewerMain(); |
| 182 | } |