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_