Solve mapping problem in 3d
Solving for z, pitch, and roll will help us get better estimates for
the 2d pose that we actually care about.
Also, since we are only using the box of pis for mapping, deleted
functionality for mapping with robot position so I didn't have to
support that in 3d.
Change-Id: I87e99a148c4953e2e67b0b0e9b07aa9abe1cd158
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 50a9d7d..38bab6d 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -6,12 +6,14 @@
id:uint64 (id: 0);
// Pose of target relative to field origin.
- // NOTE: As of now, we only solve for the 2d pose (x, y, yaw)
- // and all other values will be 0.
+ // To get the pose of the target in the field frame, 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);