Merge "Adding a camera_id to our intrinsic calibration file"
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index 793d8cd..850fc7f 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -15,7 +15,7 @@
V4L2Reader::V4L2Reader(aos::EventLoop *event_loop,
const std::string &device_name)
: fd_(open(device_name.c_str(), O_RDWR | O_NONBLOCK)) {
- PCHECK(fd_.get() != -1);
+ PCHECK(fd_.get() != -1) << " Failed to open device " << device_name;
// First, clean up after anybody else who left the device streaming.
StreamOff();
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index ec5bf40..9293d50 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -2,6 +2,7 @@
#include <opencv2/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
+#include <regex>
#include "Eigen/Dense"
#include "Eigen/Geometry"
@@ -13,21 +14,24 @@
#include "aos/util/file.h"
#include "y2020/vision/charuco_lib.h"
-DEFINE_string(config, "config.json", "Path to the config file to use.");
-DEFINE_string(pi, "pi-7971-1", "Pi name to calibrate.");
DEFINE_string(calibration_folder, ".", "Folder to place calibration files.");
+DEFINE_string(camera_id, "", "Camera ID in format YY-NN-- year and number.");
+DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_bool(display_undistorted, false,
"If true, display the undistorted image.");
+DEFINE_string(pi, "", "Pi name to calibrate.");
namespace frc971 {
namespace vision {
class Calibration {
public:
- Calibration(aos::ShmEventLoop *event_loop, std::string_view pi)
+ Calibration(aos::ShmEventLoop *event_loop, std::string_view pi,
+ std::string_view camera_id)
: event_loop_(event_loop),
pi_(pi),
pi_number_(aos::network::ParsePiNumber(pi)),
+ camera_id_(camera_id),
H_camera_board_(Eigen::Affine3d()),
prev_H_camera_board_(Eigen::Affine3d()),
charuco_extractor_(
@@ -37,11 +41,15 @@
std::vector<int> charuco_ids,
std::vector<cv::Point2f> charuco_corners, bool valid,
Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
- HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners,
- valid, rvec_eigen, tvec_eigen);
+ HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
+ rvec_eigen, tvec_eigen);
}) {
CHECK(pi_number_) << ": Invalid pi number " << pi
<< ", failed to parse pi number";
+ std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
+ CHECK(std::regex_match(camera_id_, re))
+ << ": Invalid camera_id '" << camera_id_
+ << "', should be of form YY-NN";
}
void HandleCharuco(cv::Mat rgb_image,
@@ -111,6 +119,8 @@
void MaybeCalibrate() {
if (all_charuco_ids_.size() >= 50) {
+ LOG(INFO) << "Beginning calibration on " << all_charuco_ids_.size()
+ << " images";
cv::Mat cameraMatrix, distCoeffs;
std::vector<cv::Mat> rvecs, tvecs;
cv::Mat stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors;
@@ -129,6 +139,8 @@
flatbuffers::FlatBufferBuilder fbb;
flatbuffers::Offset<flatbuffers::String> name_offset =
fbb.CreateString(absl::StrFormat("pi%d", pi_number_.value()));
+ flatbuffers::Offset<flatbuffers::String> camera_id_offset =
+ fbb.CreateString(camera_id_);
flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
fbb.CreateVector<float>(9u, [&cameraMatrix](size_t i) {
return static_cast<float>(
@@ -152,6 +164,7 @@
aos::realtime_clock::now();
camera_calibration_builder.add_node_name(name_offset);
camera_calibration_builder.add_team_number(team_number.value());
+ camera_calibration_builder.add_camera_id(camera_id_offset);
camera_calibration_builder.add_calibration_timestamp(
realtime_now.time_since_epoch().count());
camera_calibration_builder.add_intrinsics(intrinsics_offset);
@@ -166,8 +179,9 @@
const std::string calibration_filename =
FLAGS_calibration_folder +
- absl::StrFormat("/calibration_pi-%d-%d_%s.json", team_number.value(),
- pi_number_.value(), time_ss.str());
+ absl::StrFormat("/calibration_pi-%d-%d_cam-%s_%s.json",
+ team_number.value(), pi_number_.value(), camera_id_,
+ time_ss.str());
LOG(INFO) << calibration_filename << " -> "
<< aos::FlatbufferToJson(camera_calibration,
@@ -188,6 +202,7 @@
aos::ShmEventLoop *event_loop_;
std::string pi_;
const std::optional<uint16_t> pi_number_;
+ const std::string camera_id_;
std::vector<std::vector<int>> all_charuco_ids_;
std::vector<std::vector<cv::Point2f>> all_charuco_corners_;
@@ -206,7 +221,12 @@
aos::ShmEventLoop event_loop(&config.message());
- Calibration extractor(&event_loop, FLAGS_pi);
+ std::string hostname = FLAGS_pi;
+ if (hostname == "") {
+ hostname = aos::network::GetHostname();
+ LOG(INFO) << "Using pi name from hostname as " << hostname;
+ }
+ Calibration extractor(&event_loop, hostname, FLAGS_camera_id);
event_loop.Run();
diff --git a/y2020/vision/charuco_lib.cc b/y2020/vision/charuco_lib.cc
index 0df820b..8677cbb 100644
--- a/y2020/vision/charuco_lib.cc
+++ b/y2020/vision/charuco_lib.cc
@@ -137,7 +137,7 @@
const double age_double =
std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
if (age > std::chrono::milliseconds(100)) {
- LOG(INFO) << "Age: " << age_double << ", getting behind, skipping";
+ VLOG(2) << "Age: " << age_double << ", getting behind, skipping";
return;
}
// Create color image:
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 3336d0c..0598497 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -104,6 +104,10 @@
// Timestamp for when the calibration was taken on the realtime clock.
calibration_timestamp:int64 (id: 6);
+
+ // The id of the camera which this calibration data applies to.
+ // Expected to be formatted as Year-Number (YY-##).
+ camera_id:string (id: 7);
}
// Contains the information the EKF wants from an image matched against a single
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index ce257a9..96d7ffd 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -336,9 +336,9 @@
blob_result->filtered_blobs = filtered_pair.first;
blob_result->centroid = filtered_pair.second;
auto end = aos::monotonic_clock::now();
- LOG(INFO) << "Blob detection elapsed time: "
- << std::chrono::duration<double, std::milli>(end - start).count()
- << " ms";
+ VLOG(2) << "Blob detection elapsed time: "
+ << std::chrono::duration<double, std::milli>(end - start).count()
+ << " ms";
}
} // namespace vision
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_cam-22-01_2022-02-12_14-35_00.000000000.json b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-01_2022-02-12_14-35_00.000000000.json
new file mode 100644
index 0000000..3c30b15
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-01_2022-02-12_14-35_00.000000000.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 391.63916,
+ 0.0,
+ 312.691162,
+ 0.0,
+ 391.535889,
+ 267.138672,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.121374,
+ -0.203352,
+ 0.000325,
+ 0.002694,
+ 0.054089
+ ],
+ "calibration_timestamp": 1635611589630802881,
+ "camera_id": "22-01"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_cam-22-02_2022-01-28_05-35-16.002911868.json b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-02_2022-01-28_05-35-16.002911868.json
new file mode 100644
index 0000000..b147867
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-02_2022-01-28_05-35-16.002911868.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 390.833618,
+ 0.0,
+ 298.229218,
+ 0.0,
+ 390.547882,
+ 251.143417,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.120491,
+ -0.190643,
+ 0.000534,
+ -0.000345,
+ 0.029808
+ ],
+ "calibration_timestamp": 1643348116002911868,
+ "camera_id": "22-02"
+}
\ No newline at end of file
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_cam-22-03_2022-02-12_16-53-00.000000000.json b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-03_2022-02-12_16-53-00.000000000.json
new file mode 100644
index 0000000..a107065
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-03_2022-02-12_16-53-00.000000000.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 388.182281,
+ 0.0,
+ 306.279083,
+ 0.0,
+ 388.440582,
+ 224.480484,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.129443,
+ -0.225948,
+ 0.001234,
+ -0.000004,
+ 0.068937
+ ],
+ "calibration_timestamp": 1643342760319632865,
+ "camera_id": "22-03"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_cam-22-04_2022-01-28_05-26-43.135661745.json b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-04_2022-01-28_05-26-43.135661745.json
new file mode 100755
index 0000000..8c19c46
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-04_2022-01-28_05-26-43.135661745.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 386.619232,
+ 0.0,
+ 335.525116,
+ 0.0,
+ 386.309601,
+ 225.775742,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.130693,
+ -0.238688,
+ 0.002466,
+ -0.00017,
+ 0.083145
+ ],
+ "calibration_timestamp": 1643347603135661745,
+ "camera_id": "22-04"
+}
\ No newline at end of file
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_cam-22-05_2022-02-16_20-40-00.000000000.json b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-05_2022-02-16_20-40-00.000000000.json
new file mode 100755
index 0000000..a5ebf82
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-05_2022-02-16_20-40-00.000000000.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 387.791046,
+ 0.0,
+ 360.276276,
+ 0.0,
+ 387.214264,
+ 235.913925,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.132322,
+ -0.247507,
+ 0.001326,
+ 0.002151,
+ 0.098543
+ ],
+ "calibration_timestamp": 1643348271146848848,
+ "camera_id": "22-05"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_cam-22-06_2022-02-16_20-54-00.000000000.json b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-06_2022-02-16_20-54-00.000000000.json
new file mode 100755
index 0000000..71aaf02
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-06_2022-02-16_20-54-00.000000000.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 389.730774,
+ 0.0,
+ 329.825134,
+ 0.0,
+ 389.599243,
+ 205.222931,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.12185,
+ -0.214579,
+ -0.00013,
+ 0.000629,
+ 0.066571
+ ],
+ "calibration_timestamp": 1643348049373070054,
+ "camera_id": "22-06"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-971-1_cam-22-07_2022-02-16_21-20-00.000000000.json b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-07_2022-02-16_21-20-00.000000000.json
new file mode 100755
index 0000000..27ed863
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-1_cam-22-07_2022-02-16_21-20-00.000000000.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 971,
+ "intrinsics": [
+ 388.062378,
+ 0.0,
+ 333.890381,
+ 0.0,
+ 388.149048,
+ 212.644363,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.123399,
+ -0.21765,
+ -0.00085,
+ 0.000694,
+ 0.067006
+ ],
+ "calibration_timestamp": 1643348078845387555,
+ "camera_id": "22-07"
+}
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index a45c704..30057a5 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -48,6 +48,7 @@
self.turret_ext = None
self.node_name = ""
self.team_number = -1
+ self.camera_id = ""
self.timestamp = 0
@@ -144,6 +145,10 @@
team_number = calib_dict["team_number"]
node_name = calib_dict["node_name"]
+ camera_id = "UNKNOWN"
+ if "camera_id" in calib_dict:
+ camera_id = calib_dict["camera_id"]
+
camera_matrix = np.asarray(calib_dict["intrinsics"]).reshape(
(3, 3))
dist_coeffs = np.asarray(calib_dict["dist_coeffs"]).reshape((1, 5))
@@ -158,6 +163,7 @@
camera_params.node_name = node_name
camera_params.team_number = team_number
+ camera_params.camera_id = camera_id
camera_params.camera_int.camera_matrix = copy.copy(camera_matrix)
camera_params.camera_int.dist_coeffs = copy.copy(dist_coeffs)
camera_list.append(camera_params)
diff --git a/y2022/vision/camera_reader_main.cc b/y2022/vision/camera_reader_main.cc
index f1c5fdf..52ee51a 100644
--- a/y2022/vision/camera_reader_main.cc
+++ b/y2022/vision/camera_reader_main.cc
@@ -6,7 +6,8 @@
// bazel run //y2022/vision:camera_reader -- --config y2022/config.json
// --override_hostname pi-7971-1 --ignore_timestamps true
DEFINE_string(config, "config.json", "Path to the config file to use.");
-DEFINE_uint32(exposure, 5, "Exposure time, in 100us increments");
+DEFINE_uint32(exposure, 5,
+ "Exposure time, in 100us increments; 0 implies auto exposure");
namespace y2022 {
namespace vision {
@@ -33,7 +34,9 @@
}
V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
- v4l2_reader.SetExposure(FLAGS_exposure);
+ if (FLAGS_exposure > 0) {
+ v4l2_reader.SetExposure(FLAGS_exposure);
+ }
CameraReader camera_reader(&event_loop, &calibration_data.message(),
&v4l2_reader);
diff --git a/y2022/vision/create_calib_file.py b/y2022/vision/create_calib_file.py
index de9f65d..6b24d28 100644
--- a/y2022/vision/create_calib_file.py
+++ b/y2022/vision/create_calib_file.py
@@ -44,13 +44,9 @@
def main():
-
- camera_calib_list = None
-
- output_path = sys.argv[1]
-
camera_calib_list = camera_definition.load_camera_definitions()
+ output_path = sys.argv[1]
glog.debug("Writing file to %s", output_path)
fbb = flatbuffers.Builder(0)