Switch to quaternions in target poses
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I46efa10d0069336b520113c4233c2936e18d83d9
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 57627e8..cee2b07 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -1,5 +1,18 @@
namespace frc971.vision;
+table Position {
+ x:double (id: 0);
+ y:double (id: 1);
+ z:double (id: 2);
+}
+
+table Quaternion {
+ w:double (id: 0);
+ x:double (id: 1);
+ y:double (id: 2);
+ z:double (id: 3);
+}
+
// Represents 3d pose of an april tag on the field.
table TargetPoseFbs {
// AprilTag ID of this target
@@ -7,16 +20,10 @@
// Pose of target relative to either the field origin or robot.
// To get the pose of the target, do:
- // Translation3d(x, y, z) *
- // (AngleAxisd(yaw) * AngleAxisd(pitch) * AngleAxisd(roll))
- x:double (id: 1);
- y:double (id: 2);
- z:double (id: 3);
-
- // Orientation of the target.
- roll:double (id: 4);
- pitch:double (id: 5);
- yaw:double (id: 6);
+ // Translation3d(position.x(), position.y(), position.z()) *
+ // Quaterniond(orientation.w(), orientation.x(), orientation.y(), orientation.z())
+ position:Position (id: 1);
+ orientation:Quaternion (id: 2);
}
// Map of all target poses on a field.
diff --git a/frc971/vision/target_map.json b/frc971/vision/target_map.json
index 5f256fa..3f6eb54 100644
--- a/frc971/vision/target_map.json
+++ b/frc971/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/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 5ce62a0..ddd86e8 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -66,6 +66,32 @@
return Eigen::Vector3d(roll, pitch, yaw);
}
+flatbuffers::Offset<TargetPoseFbs> PoseUtils::TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto position_offset =
+ CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
+ target_pose.pose.p(2));
+ const auto orientation_offset =
+ CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
+ target_pose.pose.q.y(), target_pose.pose.q.z());
+ return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
+ orientation_offset);
+}
+
+TargetMapper::TargetPose PoseUtils::TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs) {
+ return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
+ .pose = ceres::examples::Pose3d{
+ Eigen::Vector3d(target_pose_fbs.position()->x(),
+ target_pose_fbs.position()->y(),
+ target_pose_fbs.position()->z()),
+ Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
+ target_pose_fbs.orientation()->x(),
+ target_pose_fbs.orientation()->y(),
+ target_pose_fbs.orientation()->z())}};
+}
+
ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
const std::vector<DataAdapter::TimestampedDetection>
×tamped_target_detections,
@@ -177,12 +203,8 @@
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
- target_poses_[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()))};
+ target_poses_[target_pose_fbs->id()] =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
}
}
@@ -253,12 +275,12 @@
}
// The pose graph optimization problem has six DOFs that are not fully
- // constrained. This is typically referred to as gauge freedom. You can apply
- // a rigid body transformation to all the nodes and the optimization problem
- // will still have the exact same cost. The Levenberg-Marquardt algorithm has
- // internal damping which mitigates this issue, but it is better to properly
- // constrain the gauge freedom. This can be done by setting one of the poses
- // as constant so the optimizer cannot change it.
+ // constrained. This is typically referred to as gauge freedom. You can
+ // apply a rigid body transformation to all the nodes and the optimization
+ // problem will still have the exact same cost. The Levenberg-Marquardt
+ // algorithm has internal damping which mitigates this issue, but it is
+ // better to properly constrain the gauge freedom. This can be done by
+ // setting one of the poses as constant so the optimizer cannot change it.
ceres::examples::MapOfPoses::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
@@ -306,19 +328,8 @@
// Convert poses to flatbuffers
std::vector<flatbuffers::Offset<TargetPoseFbs>> target_poses_fbs;
for (const auto &[id, pose] : target_poses_) {
- TargetPoseFbs::Builder target_pose_builder(fbb);
- target_pose_builder.add_id(id);
-
- target_pose_builder.add_x(pose.p(0));
- target_pose_builder.add_y(pose.p(1));
- target_pose_builder.add_z(pose.p(2));
-
- auto rpy = PoseUtils::QuaternionToEulerAngles(pose.q);
- target_pose_builder.add_roll(rpy.x());
- target_pose_builder.add_pitch(rpy.y());
- target_pose_builder.add_yaw(rpy.z());
-
- target_poses_fbs.emplace_back(target_pose_builder.Finish());
+ target_poses_fbs.emplace_back(
+ PoseUtils::TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
}
const auto field_name_offset = fbb.CreateString(field_name);
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 7e2c323..7701fd1 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -89,6 +89,14 @@
static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
// Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
+
+ // Builds a TargetPoseFbs from a TargetPose
+ static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb);
+ // Converts a TargetPoseFbs to a TargetPose
+ static TargetMapper::TargetPose TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs);
};
// Transforms robot position and target detection data into target constraints
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;
}
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)});
}
}
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 1b31e91..45ee066 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -182,7 +182,7 @@
"name": "/pi{{ NUM }}/camera",
"type": "frc971.vision.TargetMap",
"source_node": "pi{{ NUM }}",
- "frequency": 30,
+ "frequency": 40,
"num_senders": 2,
"max_size": 40000,
"logger": "LOCAL_AND_REMOTE_LOGGER",