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_mapper.cc b/frc971/vision/target_mapper.cc
index 8077f16..c14b032 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -104,7 +104,8 @@
Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
target_pose_fbs.orientation()->x(),
target_pose_fbs.orientation()->y(),
- target_pose_fbs.orientation()->z())}};
+ target_pose_fbs.orientation()->z())
+ .normalized()}};
}
ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(