Switch to quaternions in target poses
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I46efa10d0069336b520113c4233c2936e18d83d9
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 9440813..271f787 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -408,15 +408,9 @@
std::vector<TargetMapper::TargetPose> actual_target_poses;
ceres::examples::MapOfPoses target_poses;
- for (auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
- auto target_pose = TargetMapper::TargetPose{
- static_cast<int>(target_pose_fbs->id()),
- ceres::examples::Pose3d{
- Eigen::Vector3d(target_pose_fbs->x(), target_pose_fbs->y(),
- target_pose_fbs->z()),
- PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(
- target_pose_fbs->roll(), target_pose_fbs->pitch(),
- target_pose_fbs->yaw()))}};
+ for (const auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
actual_target_poses.emplace_back(target_pose);
target_poses[target_pose.id] = target_pose.pose;
}