Adding a camera_id to our intrinsic calibration file
We'll need to manually label and track these, since the pi cameras
don't have an internal serial number (as best I can tell).
I think this is important, since we can swap cameras between pi's
and it's hard to be sure we've got the right calibration based on
pi hostame.
Including calibration files for cameras 22-01 to 22-07
Added a little detail to the CHECK, since it wasn't very clear
Change-Id: I6dd6f8e19355bb0fefce2fcb7fd79ba5c95fb374
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
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)