Switch to quaternions in target poses
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I46efa10d0069336b520113c4233c2936e18d83d9
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index fc9d799..d20a247 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -91,10 +91,15 @@
flatbuffers::FlatBufferBuilder *fbb) {
const auto T =
Eigen::Translation3d(pose.t->data[0], pose.t->data[1], pose.t->data[2]);
- const auto rpy = frc971::vision::PoseUtils::RotationMatrixToEulerAngles(
- Eigen::Matrix3d(pose.R->data));
- return frc971::vision::CreateTargetPoseFbs(*fbb, target_id, T.x(), T.y(),
- T.z(), rpy(0), rpy(1), rpy(2));
+ const auto position_offset =
+ frc971::vision::CreatePosition(*fbb, T.x(), T.y(), T.z());
+
+ const auto orientation = Eigen::Quaterniond(Eigen::Matrix3d(pose.R->data));
+ const auto orientation_offset = frc971::vision::CreateQuaternion(
+ *fbb, orientation.w(), orientation.x(), orientation.y(), orientation.z());
+
+ return frc971::vision::CreateTargetPoseFbs(*fbb, target_id, position_offset,
+ orientation_offset);
}
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
diff --git a/y2023/vision/target_map.json b/y2023/vision/target_map.json
index 5f256fa..3f6eb54 100644
--- a/y2023/vision/target_map.json
+++ b/y2023/vision/target_map.json
@@ -2,75 +2,123 @@
"target_poses": [
{
"id": 1,
- "x": 7.244,
- "y": -2.938,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 2,
- "x": 7.244,
- "y": -1.262,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 3,
- "x": 7.244,
- "y": 0.414,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 4,
- "x": 7.909,
- "y": 2.740,
- "z": 0.695,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.909,
+ "y": 2.740,
+ "z": 0.695
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 5,
- "x": -7.908,
- "y": 2.740,
- "z": 0.695,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.908,
+ "y": 2.740,
+ "z": 0.695
+ },
+ "orientation": {
+ /* yaw of 0 */
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 6,
- "x": -7.243,
- "y": 0.414,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 7,
- "x": -7.243,
- "y": -1.262,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 8,
- "x": -7.243,
- "y": -2.938,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
}
]
}
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)});
}
}