commit f41b094529ed9d61a8d909285f8b2783cc585ed6
author Jim Ostrowski <yimmy13@gmail.com> Sun Mar 24 18:05:02 2024 -0700
committer Jim Ostrowski <yimmy13@gmail.com> Sun Mar 24 18:05:02 2024 -0700
tree 7a850e3c02dd0b118d8679450af2ec01df81277d
parent adecedf89440185c664ad5af8eb70fdb94792ad7

Print out the diffs between ideal and solved

For each target in the target map, print the difference between
the solved map and the ideal (given target map), from the perpsective
of the ideal reference frame

Change-Id: I1469a39cf29558fc800f7a026865184ea70637c4
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 7f40578..b9b3712 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -818,6 +818,26 @@
   fout.close();
 }
 
+void TargetMapper::PrintDiffs() const {
+  for (int id = FLAGS_min_target_id; id <= FLAGS_max_target_id; id++) {
+    Eigen::Affine3d H_world_ideal =
+        PoseUtils::Pose3dToAffine3d(ideal_target_poses_.at(id));
+    Eigen::Affine3d H_world_solved =
+        PoseUtils::Pose3dToAffine3d(target_poses_.at(id));
+    Eigen::Affine3d H_ideal_solved = H_world_ideal.inverse() * H_world_solved;
+    Eigen::Vector3d rpy = PoseUtils::RotationMatrixToEulerAngles(
+                              H_ideal_solved.rotation().matrix()) *
+                          180.0 / M_PI;
+    Eigen::Vector3d trans = H_ideal_solved.translation();
+
+    LOG(INFO) << "\nOffset from ideal to solved for target " << id
+              << " (in m, deg)"
+              << "\n  x: " << trans(0) << ", y: " << trans(1)
+              << ", z: " << trans(2) << ", \n  roll: " << rpy(0)
+              << ", pitch: " << rpy(1) << ", yaw: " << rpy(2) << "\n";
+  }
+}
+
 }  // namespace frc971::vision
 
 std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) {

frc971/vision/target_mapper.h

diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index a4c12b2..d1d122e 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -61,6 +61,7 @@
 
   void DumpConstraints(std::string_view path) const;
   void DumpStats(std::string_view path) const;
+  void PrintDiffs() const;
 
  private:
   // Error in an estimated pose

y2024/vision/target_mapping.cc

diff --git a/y2024/vision/target_mapping.cc b/y2024/vision/target_mapping.cc
index 4727814..269b1b9 100644
--- a/y2024/vision/target_mapping.cc
+++ b/y2024/vision/target_mapping.cc
@@ -454,6 +454,7 @@
     if (!FLAGS_dump_stats_to.empty()) {
       mapper.DumpStats(FLAGS_dump_stats_to);
     }
+    mapper.PrintDiffs();
   }
 }