Add a skeleton extrinsics extractor
This is meant to be a starting place for the extrinsics code. The
process will be something like the following when all done:
1) Extract all IMU readings and board positions and give them good times.
2) Run an EKF over the readings with a set of both board and camera
extrinsics and measure the error
3) Wrap an optimizer around this error measurement to solve for the
extrinsics.
4) Figure out how to seed this all with decent initial guesses so
everything converges easily.
This only makes progress on 1) and probably has some extra debug code in
the middle.
Change-Id: Ide129e7fd7280f19c26df1aa42cf5f1b55ab7300
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 4c98d93..b43829d 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -160,3 +160,27 @@
"//third_party:opencv",
],
)
+
+cc_binary(
+ name = "extrinsics_calibration",
+ srcs = [
+ "extrinsics_calibration.cc",
+ ],
+ data = [
+ "//y2020:config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2020:__subpackages__"],
+ deps = [
+ ":charuco_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//third_party:opencv",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)