Adding a few more calibration files
Identified, but haven't fully fixed the issue that was hanging the
calibration-- drawAxis unhappy when board origin is near z=0 in camera
frame
Also, gave some more indication to user of what needs to be done during
calibration
Change-Id: I992ca8b52902163e11e43e82c042768d2dfccb7b
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index f14c9fd..fde5394 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -262,22 +262,31 @@
Eigen::Vector3d result = eigen_camera_matrix_ * camera_projection *
board_to_camera * Eigen::Vector3d::Zero();
- result /= result.z();
- cv::circle(rgb_image, cv::Point(result.x(), result.y()), 4,
- cv::Scalar(255, 255, 255), 0, cv::LINE_8);
+ // Found that drawAxis hangs if you try to draw with z values too small
+ // (trying to draw axes at inifinity)
+ // TODO<Jim>: Explore what real thresholds for this should be; likely
+ // Don't need to get rid of negative values
+ if (result.z() < 0.01) {
+ LOG(INFO) << "Skipping, due to z value too small: " << result.z();
+ valid = false;
+ } else {
+ result /= result.z();
+ cv::circle(rgb_image, cv::Point(result.x(), result.y()), 4,
+ cv::Scalar(255, 255, 255), 0, cv::LINE_8);
- cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec, tvec,
- 0.1);
+ cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec,
+ tvec, 0.1);
+ }
} else {
LOG(INFO) << "Age: " << age_double << ", invalid pose";
}
} else {
- LOG(INFO) << "Age: " << age_double << ", not enough charuco IDs, got "
- << charuco_ids.size() << ", needed " << FLAGS_min_targets;
+ VLOG(2) << "Age: " << age_double << ", not enough charuco IDs, got "
+ << charuco_ids.size() << ", needed " << FLAGS_min_targets;
}
} else {
- LOG(INFO) << "Age: " << age_double << ", not enough marker IDs, got "
- << marker_ids.size() << ", needed " << FLAGS_min_targets;
+ VLOG(2) << "Age: " << age_double << ", not enough marker IDs, got "
+ << marker_ids.size() << ", needed " << FLAGS_min_targets;
cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
}
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 3f08be9..b5d1b32 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -72,6 +72,12 @@
cv::imshow("Display undist", undistorted_rgb_image);
}
+ int keystroke = cv::waitKey(1);
+
+ // If we haven't got a valid pose estimate, don't use these points
+ if (!valid) {
+ return;
+ }
// Calibration calculates rotation and translation delta from last image
// stored to automatically capture next image
@@ -89,6 +95,10 @@
double t_norm = delta_t.norm();
bool store_image = false;
+ double percent_motion =
+ std::max<double>(r_norm / kDeltaRThreshold, t_norm / kDeltaTThreshold);
+ LOG(INFO) << all_charuco_ids_.size() << ": Moved " << percent_motion
+ << "% of what's needed";
// Verify that camera has moved enough from last stored image
if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
// frame_ refers to deltas between current and last captured image
@@ -106,11 +116,14 @@
// Make sure camera has stopped moving before storing image
store_image =
frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
+ double percent_stop = std::max<double>(frame_r_norm / kFrameDeltaRLimit,
+ frame_t_norm / kFrameDeltaTLimit);
+ LOG(INFO) << all_charuco_ids_.size() << ": Moved enough ("
+ << percent_motion << "%); Need to stop (last motion was "
+ << percent_stop << "%";
}
-
prev_image_H_camera_board_ = H_camera_board_;
- int keystroke = cv::waitKey(1);
if (store_image) {
if (valid) {
prev_H_camera_board_ = H_camera_board_;
@@ -126,20 +139,16 @@
LOG(INFO) << "Triggered by translation delta = " << t_norm << " > "
<< kDeltaTThreshold;
}
-
- LOG(INFO) << "Image count " << all_charuco_corners_.size();
}
- if (all_charuco_ids_.size() >= 50) {
- LOG(INFO) << "Got enough images to calibrate";
- event_loop_->Exit();
- }
} else if ((keystroke & 0xFF) == static_cast<int>('q')) {
event_loop_->Exit();
}
}
void MaybeCalibrate() {
+ // TODO: This number should depend on coarse vs. fine pattern
+ // Maybe just on total # of ids found, not just images
if (all_charuco_ids_.size() >= 50) {
LOG(INFO) << "Beginning calibration on " << all_charuco_ids_.size()
<< " images";
diff --git a/y2022/vision/calib_files/calibration_pi-9971-1_cam-21-01_2022-01-28_07-06-07.187479188.json b/y2022/vision/calib_files/calibration_pi-9971-1_cam-21-01_2022-01-28_07-06-07.187479188.json
new file mode 100755
index 0000000..01935b8
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-1_cam-21-01_2022-01-28_07-06-07.187479188.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 9971,
+ "intrinsics": [
+ 390.600739,
+ 0.0,
+ 346.584473,
+ 0.0,
+ 390.302429,
+ 234.478241,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.134674,
+ -0.239105,
+ -0.000435,
+ 0.000515,
+ 0.087243
+ ],
+ "calibration_timestamp": 1643353567187479188,
+ "camera_id": "21-01"
+}
\ No newline at end of file
diff --git a/y2022/vision/calib_files/calibration_pi-9971-2_cam-21-02_2022-01-28_06-56-21.541962185.json b/y2022/vision/calib_files/calibration_pi-9971-2_cam-21-02_2022-01-28_06-56-21.541962185.json
new file mode 100755
index 0000000..3025298
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-2_cam-21-02_2022-01-28_06-56-21.541962185.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi2",
+ "team_number": 9971,
+ "intrinsics": [
+ 392.045471,
+ 0.0,
+ 299.976349,
+ 0.0,
+ 391.858948,
+ 240.437485,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.140083,
+ -0.243777,
+ -0.000455,
+ 0.00004,
+ 0.084187
+ ],
+ "calibration_timestamp": 1643352981541962185,
+ "camera_id": "21-02"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-3_cam-21-03_2022-01-28_06-40-19.061887367.json b/y2022/vision/calib_files/calibration_pi-9971-3_cam-21-03_2022-01-28_06-40-19.061887367.json
new file mode 100755
index 0000000..22ad253
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-3_cam-21-03_2022-01-28_06-40-19.061887367.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi3",
+ "team_number": 9971,
+ "intrinsics": [
+ 390.202698,
+ 0.0,
+ 323.895874,
+ 0.0,
+ 390.134705,
+ 240.809784,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.128804,
+ -0.215377,
+ -0.000073,
+ -0.000593,
+ 0.062635
+ ],
+ "calibration_timestamp": 1643352019061887367,
+ "camera_id": "21-03"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-4_cam-21-04_2022-01-28_06-51-31.012263392.json b/y2022/vision/calib_files/calibration_pi-9971-4_cam-21-04_2022-01-28_06-51-31.012263392.json
new file mode 100644
index 0000000..d867cd7
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-4_cam-21-04_2022-01-28_06-51-31.012263392.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi4",
+ "team_number": 9971,
+ "intrinsics": [
+ 391.82019,
+ 0.0,
+ 351.66806,
+ 0.0,
+ 391.380554,
+ 261.502167,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.147662,
+ -0.258165,
+ 0.000671,
+ -0.00027,
+ 0.09542
+ ],
+ "calibration_timestamp": 1643352691012263392,
+ "camera_id": "21-03"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-5_cam-21-05_2022-01-28_06-35-06.089551572.json b/y2022/vision/calib_files/calibration_pi-9971-5_cam-21-05_2022-01-28_06-35-06.089551572.json
new file mode 100755
index 0000000..d089faa
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-5_cam-21-05_2022-01-28_06-35-06.089551572.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi5",
+ "team_number": 9971,
+ "intrinsics": [
+ 393.222015,
+ 0.0,
+ 283.454346,
+ 0.0,
+ 393.15332,
+ 257.545563,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.145289,
+ -0.26649,
+ 0.000127,
+ 0.000057,
+ 0.106988
+ ],
+ "calibration_timestamp": 1643351706089551572,
+ "camera_id": "21-05"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-6_cam-21-06_2022-01-28_06-34-47.705289435.json b/y2022/vision/calib_files/calibration_pi-9971-6_cam-21-06_2022-01-28_06-34-47.705289435.json
new file mode 100644
index 0000000..bb08ce2
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-6_cam-21-06_2022-01-28_06-34-47.705289435.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi6",
+ "team_number": 9971,
+ "intrinsics": [
+ 392.598694,
+ 0.0,
+ 338.288269,
+ 0.0,
+ 392.689697,
+ 244.718338,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.138265,
+ -0.232579,
+ -0.000478,
+ 0.000089,
+ 0.075434
+ ],
+ "calibration_timestamp": 1643351687705289435,
+ "camera_id": "21-06"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-1_cam-22-07_2022-02-16_21-20-00.000000000.json b/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
similarity index 93%
rename from y2022/vision/calib_files/calibration_pi-9971-1_cam-22-07_2022-02-16_21-20-00.000000000.json
rename to y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
index 35efa45..6dbd3b2 100755
--- a/y2022/vision/calib_files/calibration_pi-9971-1_cam-22-07_2022-02-16_21-20-00.000000000.json
+++ b/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi1",
+ "node_name": "pi7",
"team_number": 9971,
"intrinsics": [
388.062378,