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/geometry.h b/y2022/vision/geometry.h
index 31e8629..c27eb4d 100644
--- a/y2022/vision/geometry.h
+++ b/y2022/vision/geometry.h
@@ -1,3 +1,6 @@
+#ifndef Y2022_VISION_GEOMETRY_H_
+#define Y2022_VISION_GEOMETRY_H_
+
#include "aos/util/math.h"
#include "glog/logging.h"
#include "opencv2/core/types.hpp"
@@ -129,3 +132,5 @@
};
} // namespace y2022::vision
+
+#endif // Y2022_VISION_GEOMETRY_H_
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