commit 6d242d57fe4a55bcc0fef768edd85b46b2aa5eec
author Jim Ostrowski <yimmy13@gmail.com> Wed Apr 03 20:39:03 2024 -0700
committer Jim Ostrowski <yimmy13@gmail.com> Sat Sep 21 14:40:23 2024 -0700
tree 25e6ff742fe539c05a0cc92ca23b34687229f7fc
parent 9e6951254f935c2a6379c2140a071a8ae7552f20

Changes to camera extrinsic calibration to improve accuracy

Adds Outlier Rejection

Do proper rotation (Quaternion) averaging

When camera sees two targets at once, store that, and store
a single observation to match with other cameras.  This leads
to a lot more good connections

Removed dead code, and refactored a couple pieces, e.g., writing extrinsic file

Also refactored some of the utilities to use quaternion averaging from utils,
and move other utility functions to vision_utils_lib

Change-Id: I918ce9c937d80717daa6659abbb139006506d4cc
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>

y2023/vision/april_detection_test.cc

diff --git a/y2023/vision/april_detection_test.cc b/y2023/vision/april_detection_test.cc
index 9f4f8b7..7e50b09 100644
--- a/y2023/vision/april_detection_test.cc
+++ b/y2023/vision/april_detection_test.cc
@@ -11,6 +11,7 @@
 #include "frc971/constants/constants_sender_lib.h"
 #include "frc971/vision/target_mapper.h"
 #include "frc971/vision/vision_generated.h"
+#include "frc971/vision/vision_util_lib.h"
 #include "y2023/constants/constants_generated.h"
 #include "y2023/constants/constants_list_generated.h"
 #include "y2023/vision/aprilrobotics.h"
@@ -61,7 +62,7 @@
     ASSERT_EQ(april_pose_fetcher_->target_poses()->size(), 1);
 
     frc971::vision::TargetMapper::TargetPose target_pose =
-        frc971::vision::PoseUtils::TargetPoseFromFbs(
+        frc971::vision::TargetMapper::TargetPoseFromFbs(
             *april_pose_fetcher_->target_poses()->Get(0));
 
     ASSERT_EQ(april_pose_fetcher_->target_poses()->Get(0)->id(), 8);

y2023/vision/calibrate_multi_cameras.cc

diff --git a/y2023/vision/calibrate_multi_cameras.cc b/y2023/vision/calibrate_multi_cameras.cc
index 8032c764..5c29797 100644
--- a/y2023/vision/calibrate_multi_cameras.cc
+++ b/y2023/vision/calibrate_multi_cameras.cc
@@ -13,6 +13,7 @@
 #include "frc971/vision/charuco_lib.h"
 #include "frc971/vision/extrinsics_calibration.h"
 #include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_util_lib.h"
 #include "frc971/vision/visualize_robot.h"
 // clang-format off
 // OpenCV eigen files must be included after Eigen includes
@@ -362,7 +363,7 @@
     }
 
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
 
     target_poses.emplace_back(target_pose);
 

y2023/vision/target_mapping.cc

diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index f33a83a..325ad90 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -15,6 +15,7 @@
 #include "frc971/control_loops/pose.h"
 #include "frc971/vision/calibration_generated.h"
 #include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_util_lib.h"
 #include "frc971/vision/visualize_robot.h"
 #include "y2023/constants/simulated_constants_sender.h"
 #include "y2023/vision/aprilrobotics.h"
@@ -239,7 +240,7 @@
     }
 
     const TargetMapper::TargetPose target_pose =
-        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+        TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
 
     Eigen::Affine3d H_camera_target =
         Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;