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>
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) {