Adding multi-camera extrinsic calibration between cameras
Uses pair of Aruco Diamonds to build extrinsics between neighboring cameras
Small refactor of charuco_lib to allow construction without requiring event_loop
Change max_pose_error to 1e-7 based on data from SFR match
Also loosen decision_margin to 50 based on the same data
Small cleanup/typos in target_mapping
Change-Id: If3b902612cb99e4f6e8a20291561fac766e6bacc
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 82cc6b3..b8edcae 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -25,8 +25,10 @@
DEFINE_uint32(
min_charucos, 10,
"The mininum number of aruco targets in charuco board required to match.");
-DEFINE_uint32(min_id, 12, "Minimum valid charuco id");
-DEFINE_uint32(max_id, 15, "Minimum valid charuco id");
+DEFINE_uint32(min_id, 0, "Minimum valid charuco id");
+DEFINE_uint32(max_diamonds, 0,
+ "Maximum number of diamonds to see. Set to 0 for no limit");
+DEFINE_uint32(max_id, 15, "Maximum valid charuco id");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
DEFINE_bool(
draw_axes, false,
@@ -280,9 +282,21 @@
}
CharucoExtractor::CharucoExtractor(
+ const calibration::CameraCalibration *calibration,
+ const TargetType target_type)
+ : event_loop_(nullptr),
+ target_type_(target_type),
+ calibration_(CHECK_NOTNULL(calibration)) {
+ VLOG(2) << "Configuring CharucoExtractor without event_loop";
+ SetupTargetData();
+ VLOG(2) << "Camera matrix:\n" << calibration_.CameraIntrinsics();
+ VLOG(2) << "Distortion Coefficients:\n" << calibration_.CameraDistCoeffs();
+}
+
+CharucoExtractor::CharucoExtractor(
aos::EventLoop *event_loop,
- const calibration::CameraCalibration *calibration, TargetType target_type,
- std::string_view image_channel,
+ const calibration::CameraCalibration *calibration,
+ const 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,
@@ -303,9 +317,34 @@
void CharucoExtractor::HandleImage(cv::Mat rgb_image,
const monotonic_clock::time_point eof) {
+ // Set up the variables we'll use in the callback function
+ bool valid = false;
+ // ids and corners for final, refined board / marker detections
+ // Using Vec4i type since it supports Charuco Diamonds
+ // And overloading it using 1st int in Vec4i for others target types
+ std::vector<cv::Vec4i> result_ids;
+ std::vector<std::vector<cv::Point2f>> result_corners;
+
+ // Return a list of poses; for Charuco Board there will be just one
+ std::vector<Eigen::Vector3d> rvecs_eigen;
+ std::vector<Eigen::Vector3d> tvecs_eigen;
+ ProcessImage(rgb_image, eof, event_loop_->monotonic_now(), result_ids,
+ result_corners, valid, rvecs_eigen, tvecs_eigen);
+
+ handle_charuco_(rgb_image, eof, result_ids, result_corners, valid,
+ rvecs_eigen, tvecs_eigen);
+}
+
+void CharucoExtractor::ProcessImage(
+ cv::Mat rgb_image, const monotonic_clock::time_point eof,
+ const monotonic_clock::time_point current_time,
+ std::vector<cv::Vec4i> &result_ids,
+ std::vector<std::vector<cv::Point2f>> &result_corners, bool &valid,
+ std::vector<Eigen::Vector3d> &rvecs_eigen,
+ std::vector<Eigen::Vector3d> &tvecs_eigen) {
const double age_double =
- std::chrono::duration_cast<std::chrono::duration<double>>(
- event_loop_->monotonic_now() - eof)
+ std::chrono::duration_cast<std::chrono::duration<double>>(current_time -
+ eof)
.count();
// Have found this useful if there is blurry / noisy images
@@ -318,22 +357,10 @@
cv::cvtColor(thresh, rgb_image, cv::COLOR_GRAY2RGB);
}
- // Set up the variables we'll use in the callback function
- bool valid = false;
- // Return a list of poses; for Charuco Board there will be just one
- std::vector<Eigen::Vector3d> rvecs_eigen;
- std::vector<Eigen::Vector3d> tvecs_eigen;
-
// ids and corners for initial aruco marker detections
std::vector<int> marker_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
- // ids and corners for final, refined board / marker detections
- // Using Vec4i type since it supports Charuco Diamonds
- // And overloading it using 1st int in Vec4i for others target types
- std::vector<cv::Vec4i> result_ids;
- std::vector<std::vector<cv::Point2f>> result_corners;
-
// Do initial marker detection; this is the same for all target types
cv::aruco::detectMarkers(rgb_image, dictionary_, marker_corners, marker_ids);
cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
@@ -429,11 +456,14 @@
square_length_ / marker_length_,
diamond_corners, diamond_ids);
- // Check that we have exactly one charuco diamond. For calibration, we
- // can constrain things so that this is the case
- if (diamond_ids.size() == 1) {
- // TODO<Jim>: Could probably make this check more general than requiring
- // range of ids
+ // Check that we have an acceptable number of diamonds detected.
+ // Should be at least one, and no more than FLAGS_max_diamonds.
+ // Different calibration routines will require different values for this
+ if (diamond_ids.size() > 0 &&
+ (FLAGS_max_diamonds == 0 ||
+ diamond_ids.size() <= FLAGS_max_diamonds)) {
+ // TODO<Jim>: Could probably make this check more general than
+ // requiring range of ids
bool all_valid_ids = true;
for (uint i = 0; i < 4; i++) {
uint id = diamond_ids[0][i];
@@ -446,14 +476,15 @@
cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
diamond_ids);
- // estimate pose for diamonds doesn't return valid, so marking true
+ // estimate pose for diamonds doesn't return valid, so marking
+ // true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(
diamond_corners, square_length_, calibration_.CameraIntrinsics(),
calibration_.CameraDistCoeffs(), rvecs, tvecs);
- DrawTargetPoses(rgb_image, rvecs, tvecs);
+ DrawTargetPoses(rgb_image, rvecs, tvecs);
PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
result_ids = diamond_ids;
result_corners = diamond_corners;
@@ -463,12 +494,12 @@
} else {
if (diamond_ids.size() == 0) {
// OK to not see any markers sometimes
- VLOG(2)
- << "Found aruco markers, but no valid charuco diamond targets";
+ VLOG(2) << "Found aruco markers, but no valid charuco diamond "
+ "targets";
} else {
- // But should never detect multiple
- LOG(FATAL) << "Found multiple charuco diamond markers. Should only "
- "be one";
+ VLOG(2) << "Found too many number of diamond markers, which likely "
+ "means false positives were detected: "
+ << diamond_ids.size() << " > " << FLAGS_max_diamonds;
}
}
} else {
@@ -476,9 +507,6 @@
<< static_cast<uint8_t>(target_type_);
}
}
-
- handle_charuco_(rgb_image, eof, result_ids, result_corners, valid,
- rvecs_eigen, tvecs_eigen);
}
flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 8af7b8c..2f274bb 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -93,6 +93,11 @@
// extracted from it.
class CharucoExtractor {
public:
+ // Setting up a constructor that doesn't require an event_loop, so we can call
+ // and get results back from ProcessImage directly
+ CharucoExtractor(const calibration::CameraCalibration *calibration,
+ const TargetType target_type);
+
// The callback takes the following arguments:
// cv::Mat -> image with overlays drawn on it.
// monotonic_clock::time_point -> Time on this node when this image was
@@ -117,7 +122,16 @@
std::vector<Eigen::Vector3d>)> &&handle_charuco_fn);
// Handles the image by detecting the charuco board in it.
- void HandleImage(cv::Mat rgb_image, aos::monotonic_clock::time_point eof);
+ void HandleImage(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof);
+
+ void ProcessImage(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof,
+ const aos::monotonic_clock::time_point current_time,
+ std::vector<cv::Vec4i> &result_ids,
+ std::vector<std::vector<cv::Point2f>> &result_corners,
+ bool &valid, std::vector<Eigen::Vector3d> &rvecs_eigen,
+ std::vector<Eigen::Vector3d> &tvecs_eigen);
// Returns the aruco dictionary in use.
cv::Ptr<cv::aruco::Dictionary> dictionary() const { return dictionary_; }
@@ -140,8 +154,8 @@
void DrawTargetPoses(cv::Mat rgb_image, std::vector<cv::Vec3d> rvecs,
std::vector<cv::Vec3d> tvecs);
- // Helper function to convert rotation (rvecs) and translation (tvecs) vectors
- // into Eigen vectors and store in corresponding vectors
+ // Helper function to convert rotation (rvecs) and translation (tvecs)
+ // vectors into Eigen vectors and store in corresponding vectors
void PackPoseResults(std::vector<cv::Vec3d> &rvecs,
std::vector<cv::Vec3d> &tvecs,
std::vector<Eigen::Vector3d> *rvecs_eigen,
@@ -184,7 +198,8 @@
const foxglove::PointsAnnotationType line_type =
foxglove::PointsAnnotationType::POINTS);
-// Creates a PointsAnnotation to build up ImageAnnotations with different types
+// Creates a PointsAnnotation to build up ImageAnnotations with different
+// types
flatbuffers::Offset<foxglove::PointsAnnotation> BuildPointsAnnotation(
flatbuffers::FlatBufferBuilder *fbb,
const aos::monotonic_clock::time_point monotonic_now,
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index c4af163..8077f16 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -303,6 +303,9 @@
ceres::LocalParameterization *quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
+ int min_constraint_id = std::numeric_limits<int>::max();
+ int max_constraint_id = std::numeric_limits<int>::min();
+
for (ceres::examples::VectorOfConstraints::const_iterator constraints_iter =
constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {
@@ -325,6 +328,15 @@
<< "Should have counted constraints for " << id_pair.first << "->"
<< id_pair.second;
+ VLOG(1) << "Adding constraint pair: " << id_pair.first << " and "
+ << id_pair.second;
+ // Store min & max id's; assumes first id < second id
+ if (id_pair.first < min_constraint_id) {
+ min_constraint_id = id_pair.first;
+ }
+ if (id_pair.second > max_constraint_id) {
+ max_constraint_id = id_pair.second;
+ }
// Normalize constraint cost by occurances
size_t constraint_count = constraint_counts_[id_pair];
// Scale all costs so the total cost comes out to more reasonable numbers
@@ -358,6 +370,12 @@
// setting one of the poses as constant so the optimizer cannot change it.
CHECK_NE(poses->count(FLAGS_frozen_target_id), 0ul)
<< "Got no poses for frozen target id " << FLAGS_frozen_target_id;
+ CHECK_GE(FLAGS_frozen_target_id, min_constraint_id)
+ << "target to freeze index " << FLAGS_frozen_target_id
+ << " must be in range of constraints, > " << min_constraint_id;
+ CHECK_LE(FLAGS_frozen_target_id, max_constraint_id)
+ << "target to freeze index " << FLAGS_frozen_target_id
+ << " must be in range of constraints, < " << max_constraint_id;
ceres::examples::MapOfPoses::iterator pose_start_iter =
poses->find(FLAGS_frozen_target_id);
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
@@ -479,8 +497,8 @@
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_start)).inverse() *
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_end));
auto constraint = PoseUtils::Affine3dToPose3d(H_start_end);
- LOG(INFO) << id_start << "->" << id_end << ": " << constraint.p.norm()
- << " meters";
+ VLOG(1) << id_start << "->" << id_end << ": " << constraint.p.norm()
+ << " meters";
}
}
}