Switch to quaternions in target poses
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I46efa10d0069336b520113c4233c2936e18d83d9
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 38465c9..6d32dbd 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -44,14 +44,12 @@
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
Eigen::Affine3d extrinsics) {
- for (const auto *target_pose : *map.target_poses()) {
- Eigen::Translation3d T_camera_target = Eigen::Translation3d(
- target_pose->x(), target_pose->y(), target_pose->z());
- Eigen::Quaterniond R_camera_target =
- PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(
- target_pose->roll(), target_pose->pitch(), target_pose->yaw()));
+ for (const auto *target_pose_fbs : *map.target_poses()) {
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
- Eigen::Affine3d H_camcv_target = T_camera_target * R_camera_target;
+ Eigen::Affine3d H_camcv_target =
+ Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
// With X, Y, Z being robot axes and x, y, z being camera axes,
// x = -Y, y = -Z, z = X
static const Eigen::Affine3d H_camcv_camrob =
@@ -74,7 +72,7 @@
.time = pi_distributed_time,
.H_robot_target = H_robot_target,
.distance_from_camera = distance_from_camera,
- .id = static_cast<TargetMapper::TargetId>(target_pose->id())});
+ .id = static_cast<TargetMapper::TargetId>(target_pose.id)});
}
}