commit e2f40d7d59c43b2c827a39569930810af66e8850
author Milind Upadhyay <milind.upadhyay@gmail.com> Thu Feb 24 13:55:53 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Fri Feb 25 23:18:44 2022 -0800
tree 2a9c55e1a771cc33ed66a7ae81d1b24939bd7116
parent c75a0ad2171ead315e82cecd06bc8f5fbee9b420

Add camera calibration to target estimate

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Ica30bd4c98f4d10c0bf1dc9e1f4cebea3ae2225c

y2022/vision/BUILD

diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 284dd66..4f1c7ff 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -151,6 +151,7 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2022:__subpackages__"],
     deps = [
+        ":calibration_fbs",
         ":target_estimate_fbs",
         "//third_party:opencv",
     ],
@@ -160,6 +161,9 @@
     name = "target_estimate_fbs",
     srcs = ["target_estimate.fbs"],
     gen_reflections = 1,
+    includes = [
+        ":calibration_fbs_includes",
+    ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2022:__subpackages__"],
 )

y2022/vision/camera_reader.cc

diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index abce18c..b8e666e 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -105,11 +105,15 @@
     blob_result_offset = blob_result_builder.Finish();
   }
 
+  const auto camera_calibration_offset =
+      aos::RecursiveCopyFlatBuffer(camera_calibration_, builder.fbb());
+
   auto target_estimate_builder = builder.MakeBuilder<TargetEstimate>();
   TargetEstimator::EstimateTargetLocation(
       blob_result.centroid, CameraIntrinsics(), CameraExtrinsics(),
       &target_estimate_builder);
   target_estimate_builder.add_blob_result(blob_result_offset);
+  target_estimate_builder.add_camera_calibration(camera_calibration_offset);
 
   builder.CheckOk(builder.Send(target_estimate_builder.Finish()));
 }

y2022/vision/target_estimate.fbs

diff --git a/y2022/vision/target_estimate.fbs b/y2022/vision/target_estimate.fbs
index 707014c..2621634 100644
--- a/y2022/vision/target_estimate.fbs
+++ b/y2022/vision/target_estimate.fbs
@@ -1,3 +1,5 @@
+include "y2022/vision/calibration.fbs";
+
 namespace y2022.vision;
 
 struct Point {
@@ -39,7 +41,8 @@
 
   blob_result:BlobResultFbs (id: 2);
 
-  // TODO(milind): add confidence
+  // Information about the camera which took this image.
+  camera_calibration:frc971.vision.calibration.CameraCalibration (id: 3);
 }
 
 root_type TargetEstimate;