Manually compute euler angles from extrinsics
For pi4 (yaw of -90 deg), the euler angles extracted from the
extrinsics were offset by pi.
Compute the euler angles constraining the pitch from [-pi/2, pi/2] so we
get the correct solution and not an equivalent but incorrect one.
This is probably the reason all pi4's estimates were 0 confidence, and
we missed shots that had pi4 pointing at the target before the
main aiming pi, pi3.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I701b72c492f898dc377328965ee9a696f546e2e5
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index b495b23..ba399b7 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -187,12 +187,16 @@
&distance_, &angle_to_camera_, &camera_height_);
// Compute the estimated rotation of the camera using the robot rotation.
- const Eigen::Vector3d ypr_extrinsics =
- (Eigen::Affine3d(extrinsics_).rotation() * kHubToCameraAxes)
- .eulerAngles(2, 1, 0);
+ const Eigen::Matrix3d extrinsics_rot =
+ Eigen::Affine3d(extrinsics_).rotation() * kHubToCameraAxes;
+ // asin returns a pitch in [-pi/2, pi/2] so this will be the correct euler
+ // angles.
+ const double pitch_seed = -std::asin(extrinsics_rot(2, 0));
+ const double roll_seed =
+ std::atan2(extrinsics_rot(2, 1) / std::cos(pitch_seed),
+ extrinsics_rot(2, 2) / std::cos(pitch_seed));
+
// TODO(milind): seed with localizer output as well
- const double roll_seed = ypr_extrinsics.z();
- const double pitch_seed = ypr_extrinsics.y();
// Constrain the rotation to be around the localizer's, otherwise there can be
// multiple solutions. There shouldn't be too much roll or pitch