Add field for image channel in charuco lib
Allows us to easily send it to /camera/decimated for mapping, which is
what we're using until we start logging at full frequency.
Also, move some command line flags out of charuco lib to avoid manually setting flags.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Iae4d9ed06260154a0fb4d4c67ea03b06a28e4411
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index f1ab4fc..eee22f5 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -114,6 +114,7 @@
Calibration::Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
aos::EventLoop *image_event_loop,
aos::EventLoop *imu_event_loop, std::string_view pi,
+ TargetType target_type, std::string_view image_channel,
CalibrationData *data)
: image_event_loop_(image_event_loop),
image_factory_(event_loop_factory->GetNodeEventLoopFactory(
@@ -122,7 +123,7 @@
imu_factory_(
event_loop_factory->GetNodeEventLoopFactory(imu_event_loop_->node())),
charuco_extractor_(
- image_event_loop_, pi,
+ image_event_loop_, pi, target_type, image_channel,
[this](cv::Mat rgb_image, monotonic_clock::time_point eof,
std::vector<cv::Vec4i> charuco_ids,
std::vector<std::vector<cv::Point2f>> charuco_corners,
@@ -135,7 +136,7 @@
image_event_loop_,
absl::StrCat("/pi",
std::to_string(aos::network::ParsePiNumber(pi).value()),
- "/camera"),
+ image_channel),
[this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
}),
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index d21f4c6..d9f6065 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -1,5 +1,5 @@
-#ifndef Y2020_VISION_CALIBRATION_ACCUMULATOR_H_
-#define Y2020_VISION_CALIBRATION_ACCUMULATOR_H_
+#ifndef FRC971_VISION_CALIBRATION_ACCUMULATOR_H_
+#define FRC971_VISION_CALIBRATION_ACCUMULATOR_H_
#include <vector>
@@ -82,7 +82,8 @@
public:
Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
- std::string_view pi, CalibrationData *data);
+ std::string_view pi, TargetType target_type,
+ std::string_view image_channel, CalibrationData *data);
// Processes a charuco detection that is returned from charuco_lib.
// For a valid detection(s), it stores camera observation
@@ -115,4 +116,4 @@
} // namespace vision
} // namespace frc971
-#endif // Y2020_VISION_CALIBRATION_ACCUMULATOR_H_
+#endif // FRC971_VISION_CALIBRATION_ACCUMULATOR_H_
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 82cf56b..65ee4ad 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -25,8 +25,6 @@
DEFINE_uint32(
min_charucos, 10,
"The mininum number of aruco targets in charuco board required to match.");
-DEFINE_string(target_type, "charuco",
- "Type of target: april_tag|aruco|charuco|charuco_diamond");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
namespace frc971 {
@@ -163,11 +161,12 @@
// Only charuco board has a board associated with it
board_ = static_cast<cv::Ptr<cv::aruco::CharucoBoard>>(NULL);
- if (FLAGS_target_type == "charuco" || FLAGS_target_type == "aruco") {
+ if (target_type_ == TargetType::kCharuco ||
+ target_type_ == TargetType::kAruco) {
dictionary_ = cv::aruco::getPredefinedDictionary(
FLAGS_large_board ? cv::aruco::DICT_5X5_250 : cv::aruco::DICT_6X6_250);
- if (FLAGS_target_type == "charuco") {
+ if (target_type_ == TargetType::kCharuco) {
LOG(INFO) << "Using " << (FLAGS_large_board ? " large " : " small ")
<< " charuco board with "
<< (FLAGS_coarse_pattern ? "coarse" : "fine") << " pattern";
@@ -189,12 +188,12 @@
cv::imwrite(FLAGS_board_template_path, board_image);
}
}
- } else if (FLAGS_target_type == "charuco_diamond") {
+ } else if (target_type_ == TargetType::kCharucoDiamond) {
// TODO<Jim>: Measure this
marker_length_ = 0.15;
square_length_ = 0.1651;
dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
- } else if (FLAGS_target_type == "april_tag") {
+ } else if (target_type_ == TargetType::kAprilTag) {
// Tag will be 6 in (15.24 cm) according to
// https://www.firstinspires.org/robotics/frc/blog/2022-2023-approved-devices-rules-preview-and-vision-target-update
square_length_ = 0.1524;
@@ -202,8 +201,8 @@
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
} else {
// Bail out if it's not a supported target
- LOG(FATAL) << "Target type undefined: " << FLAGS_target_type
- << " vs. april_tag|aruco|charuco|charuco_diamond";
+ LOG(FATAL) << "Target type undefined: "
+ << static_cast<uint8_t>(target_type_);
}
}
@@ -238,7 +237,7 @@
LOG(INFO) << "Skipping, due to z value too small: " << result.z();
} else {
result /= result.z();
- if (FLAGS_target_type == "charuco") {
+ if (target_type_ == TargetType::kCharuco) {
cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvecs[i],
tvecs[i], 0.1);
} else {
@@ -277,7 +276,8 @@
}
CharucoExtractor::CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi,
+ aos::EventLoop *event_loop, std::string_view pi, TargetType target_type,
+ std::string_view image_channel,
std::function<void(cv::Mat, monotonic_clock::time_point,
std::vector<cv::Vec4i>,
std::vector<std::vector<cv::Point2f>>, bool,
@@ -285,6 +285,8 @@
std::vector<Eigen::Vector3d>)> &&handle_charuco_fn)
: event_loop_(event_loop),
calibration_(SiftTrainingData(), pi),
+ target_type_(target_type),
+ image_channel_(image_channel),
camera_matrix_(calibration_.CameraIntrinsics()),
eigen_camera_matrix_(calibration_.CameraIntrinsicsEigen()),
dist_coeffs_(calibration_.CameraDistCoeffs()),
@@ -298,7 +300,7 @@
CHECK(pi_number_) << ": Invalid pi number " << pi
<< ", failed to parse pi number";
- LOG(INFO) << "Connecting to channel /pi" << pi_number_.value() << "/camera";
+ LOG(INFO) << "Connecting to channel " << image_channel_;
}
void CharucoExtractor::HandleImage(cv::Mat rgb_image,
@@ -328,13 +330,14 @@
cv::aruco::detectMarkers(rgb_image, dictionary_, marker_corners, marker_ids);
cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
- VLOG(2) << "Handle Image, with target type = " << FLAGS_target_type << " and "
- << marker_ids.size() << " markers detected initially";
+ VLOG(2) << "Handle Image, with target type = "
+ << static_cast<uint8_t>(target_type_) << " and " << marker_ids.size()
+ << " markers detected initially";
if (marker_ids.size() == 0) {
VLOG(2) << "Didn't find any markers";
} else {
- if (FLAGS_target_type == "charuco") {
+ if (target_type_ == TargetType::kCharuco) {
std::vector<int> charuco_ids;
std::vector<cv::Point2f> charuco_corners;
@@ -395,8 +398,8 @@
<< ", not enough marker IDs for charuco board, got "
<< marker_ids.size() << ", needed " << FLAGS_min_charucos;
}
- } else if (FLAGS_target_type == "april_tag" ||
- FLAGS_target_type == "aruco") {
+ } else if (target_type_ == TargetType::kAprilTag ||
+ target_type_ == TargetType::kAruco) {
// estimate pose for april tags doesn't return valid, so marking true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
@@ -410,7 +413,7 @@
result_ids.emplace_back(cv::Vec4i{marker_ids[i], 0, 0, 0});
}
result_corners = marker_corners;
- } else if (FLAGS_target_type == "charuco_diamond") {
+ } else if (target_type_ == TargetType::kCharucoDiamond) {
// Extract the diamonds associated with the markers
std::vector<cv::Vec4i> diamond_ids;
std::vector<std::vector<cv::Point2f>> diamond_corners;
@@ -438,7 +441,8 @@
VLOG(2) << "Found aruco markers, but no charuco diamond targets";
}
} else {
- LOG(FATAL) << "Unknown target type: " << FLAGS_target_type;
+ LOG(FATAL) << "Unknown target type: "
+ << static_cast<uint8_t>(target_type_);
}
}
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 362bb7d..02bad18 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -2,13 +2,12 @@
#define Y2020_VISION_CHARUCO_LIB_H_
#include <functional>
-#include <string_view>
-
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>
+#include <string_view>
+
#include "Eigen/Dense"
#include "Eigen/Geometry"
-
#include "absl/types/span.h"
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
@@ -16,7 +15,6 @@
#include "y2020/vision/sift/sift_training_generated.h"
DECLARE_bool(visualize);
-DECLARE_string(target_type);
namespace frc971 {
namespace vision {
@@ -62,6 +60,14 @@
std::function<void(cv::Mat, aos::monotonic_clock::time_point)> handle_image_;
};
+// Types of targets that a CharucoExtractor can detect in images
+enum class TargetType : uint8_t {
+ kAprilTag = 0,
+ kAruco = 1,
+ kCharuco = 2,
+ kCharucoDiamond = 3
+};
+
// Class which calls a callback each time an image arrives with the information
// extracted from it.
class CharucoExtractor {
@@ -80,7 +86,8 @@
// multiple targets in an image; for charuco boards, there should be just one
// element
CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi,
+ aos::EventLoop *event_loop, std::string_view pi, TargetType target_type,
+ std::string_view image_channel,
std::function<void(cv::Mat, aos::monotonic_clock::time_point,
std::vector<cv::Vec4i>,
std::vector<std::vector<cv::Point2f>>, bool,
@@ -122,7 +129,12 @@
cv::Ptr<cv::aruco::Dictionary> dictionary_;
cv::Ptr<cv::aruco::CharucoBoard> board_;
- // Length of a side of the aruco marker
+ // Type of targets to detect
+ TargetType target_type_;
+ // Channel to listen on for images
+ std::string_view image_channel_;
+
+ // Length of a side of the target marker
double marker_length_;
// Length of a side of the checkerboard squares (around the marker)
double square_length_;
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index c6bacdb..6145452 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -35,7 +35,7 @@
prev_H_camera_board_(Eigen::Affine3d()),
prev_image_H_camera_board_(Eigen::Affine3d()),
charuco_extractor_(
- event_loop, pi,
+ event_loop, pi, TargetType::kCharuco, "/camera",
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof,
std::vector<cv::Vec4i> charuco_ids,
@@ -60,8 +60,6 @@
CHECK(std::regex_match(camera_id_, re))
<< ": Invalid camera_id '" << camera_id_
<< "', should be of form YY-NN";
- CHECK_EQ(FLAGS_target_type, "charuco")
- << "Intrinsic calibration only works with Charuco board";
}
void HandleCharuco(cv::Mat rgb_image,
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index cf0c8f2..3702355 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -65,7 +65,7 @@
// Now, hook Calibration up to everything.
Calibration extractor(&factory, pi_event_loop.get(), imu_event_loop.get(),
- FLAGS_pi, &data);
+ FLAGS_pi, TargetType::kCharuco, "/camera", &data);
if (FLAGS_turret) {
aos::NodeEventLoopFactory *roborio_factory =
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 521992c..05d3481 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -18,6 +18,9 @@
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
DEFINE_bool(turret, true, "If true, the camera is on the turret");
+DEFINE_string(target_type, "charuco",
+ "Type of target: april_tag|aruco|charuco|charuco_diamond");
+DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
namespace frc971 {
namespace vision {
@@ -64,9 +67,23 @@
std::unique_ptr<aos::EventLoop> pi_event_loop =
factory.MakeEventLoop("calibration", pi_node);
+ TargetType target_type = TargetType::kCharuco;
+ if (FLAGS_target_type == "april_tag") {
+ target_type = TargetType::kAprilTag;
+ } else if (FLAGS_target_type == "aruco") {
+ target_type = TargetType::kAruco;
+ } else if (FLAGS_target_type == "charuco") {
+ target_type = TargetType::kCharuco;
+ } else if (FLAGS_target_type == "charuco_diamond") {
+ target_type = TargetType::kCharucoDiamond;
+ } else {
+ LOG(FATAL) << "Unknown target type: " << FLAGS_target_type
+ << ", expected: april_tag|aruco|charuco|charuco_diamond";
+ }
+
// Now, hook Calibration up to everything.
Calibration extractor(&factory, pi_event_loop.get(), imu_event_loop.get(),
- FLAGS_pi, &data);
+ FLAGS_pi, target_type, FLAGS_image_channel, &data);
if (FLAGS_turret) {
aos::NodeEventLoopFactory *roborio_factory =
@@ -224,7 +241,7 @@
if (FLAGS_plot) {
Plot(data, calibration_parameters);
}
-}
+} // namespace vision
} // namespace vision
} // namespace frc971
diff --git a/y2022/vision/target_mapping.cc b/y2022/vision/target_mapping.cc
index 2b10798..112696e 100644
--- a/y2022/vision/target_mapping.cc
+++ b/y2022/vision/target_mapping.cc
@@ -26,6 +26,8 @@
"times of target detections. Currently does not work reliably on the box "
"of pis.");
+DECLARE_string(image_channel);
+
namespace y2022 {
namespace vision {
using frc971::vision::DataAdapter;
@@ -151,10 +153,13 @@
const auto turret_extrinsics = CameraTurretExtrinsics(calibration);
const auto fixed_extrinsics = CameraFixedExtrinsics(calibration);
+ // TODO(milind): change to /camera once we log at full frequency
+ static constexpr std::string_view kImageChannel = "/camera/decimated";
charuco_extractors->emplace_back(std::make_unique<CharucoExtractor>(
pi_event_loop,
"pi-" + std::to_string(FLAGS_team_number) + "-" +
std::to_string(pi_number),
+ TargetType::kAprilTag, kImageChannel,
[=](cv::Mat /*rgb_image*/, aos::monotonic_clock::time_point eof,
std::vector<cv::Vec4i> april_ids,
std::vector<std::vector<cv::Point2f>> /*april_corners*/, bool valid,
@@ -173,11 +178,8 @@
}
}));
- std::string channel =
- absl::StrCat("/pi", std::to_string(pi_number), "/camera");
-
image_callbacks->emplace_back(std::make_unique<ImageCallback>(
- pi_event_loop, "/pi" + std::to_string(pi_number) + "/camera",
+ pi_event_loop, kImageChannel,
[&, charuco_extractor =
charuco_extractors->at(charuco_extractors->size() - 1).get()](
cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
@@ -233,9 +235,6 @@
"/superstructure");
}
- // Override target_type to AprilTag, since that's what we're using here
- FLAGS_target_type = "april_tag";
-
std::vector<std::unique_ptr<CharucoExtractor>> charuco_extractors;
std::vector<std::unique_ptr<ImageCallback>> image_callbacks;