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_;