Normalize target map orientations when reading them
This year we have some non-square targets, which results in quaternions
which are not perfectly normalized when in JSON.
Change-Id: I166a383a2ed7d3ed578b7eda6be7b0968231b44e
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/vision/target_map_utils.cc b/frc971/vision/target_map_utils.cc
index 232c2b6..3800628 100644
--- a/frc971/vision/target_map_utils.cc
+++ b/frc971/vision/target_map_utils.cc
@@ -8,7 +8,8 @@
return (Eigen::Translation3d(
Eigen::Vector3d(position->x(), position->y(), position->z())) *
Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
- quaternion->z()))
+ quaternion->z())
+ .normalized())
.matrix();
}
} // namespace frc971::vision