commit 2f2685fa00a19134e63c7def062cc18b4d4b31fd
author Jim Ostrowski <yimmy13@gmail.com> Sat Mar 25 11:57:54 2023 -0700
committer Jim Ostrowski <yimmy13@gmail.com> Sat Oct 21 11:35:44 2023 -0700
tree 540ef4b540511130e5dc3663d6dd0da0371cbcca
parent 7289bdbbf78df0cd69a8ad70891d5e89addcffc9

Adding multi-camera extrinsic calibration between cameras

Uses pair of Aruco Diamonds to build extrinsics between neighboring cameras

Small refactor of charuco_lib to allow construction without requiring event_loop

Change max_pose_error to 1e-7 based on data from SFR match
Also loosen decision_margin to 50 based on the same data

Small cleanup/typos in target_mapping

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

frc971/vision/target_mapper.cc

diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index c4af163..8077f16 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -303,6 +303,9 @@
   ceres::LocalParameterization *quaternion_local_parameterization =
       new ceres::EigenQuaternionParameterization;
 
+  int min_constraint_id = std::numeric_limits<int>::max();
+  int max_constraint_id = std::numeric_limits<int>::min();
+
   for (ceres::examples::VectorOfConstraints::const_iterator constraints_iter =
            constraints.begin();
        constraints_iter != constraints.end(); ++constraints_iter) {
@@ -325,6 +328,15 @@
         << "Should have counted constraints for " << id_pair.first << "->"
         << id_pair.second;
 
+    VLOG(1) << "Adding constraint pair: " << id_pair.first << " and "
+            << id_pair.second;
+    // Store min & max id's; assumes first id < second id
+    if (id_pair.first < min_constraint_id) {
+      min_constraint_id = id_pair.first;
+    }
+    if (id_pair.second > max_constraint_id) {
+      max_constraint_id = id_pair.second;
+    }
     // Normalize constraint cost by occurances
     size_t constraint_count = constraint_counts_[id_pair];
     // Scale all costs so the total cost comes out to more reasonable numbers
@@ -358,6 +370,12 @@
   // setting one of the poses as constant so the optimizer cannot change it.
   CHECK_NE(poses->count(FLAGS_frozen_target_id), 0ul)
       << "Got no poses for frozen target id " << FLAGS_frozen_target_id;
+  CHECK_GE(FLAGS_frozen_target_id, min_constraint_id)
+      << "target to freeze index " << FLAGS_frozen_target_id
+      << " must be in range of constraints, > " << min_constraint_id;
+  CHECK_LE(FLAGS_frozen_target_id, max_constraint_id)
+      << "target to freeze index " << FLAGS_frozen_target_id
+      << " must be in range of constraints, < " << max_constraint_id;
   ceres::examples::MapOfPoses::iterator pose_start_iter =
       poses->find(FLAGS_frozen_target_id);
   CHECK(pose_start_iter != poses->end()) << "There are no poses.";
@@ -479,8 +497,8 @@
           PoseUtils::Pose3dToAffine3d(target_poses_.at(id_start)).inverse() *
           PoseUtils::Pose3dToAffine3d(target_poses_.at(id_end));
       auto constraint = PoseUtils::Affine3dToPose3d(H_start_end);
-      LOG(INFO) << id_start << "->" << id_end << ": " << constraint.p.norm()
-                << " meters";
+      VLOG(1) << id_start << "->" << id_end << ": " << constraint.p.norm()
+              << " meters";
     }
   }
 }