Undistort points before pose estimation
That way it actually makes a difference...
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib1ac8542ad91916d369a1bf0577db808db518d43
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 4c07034..0d803fc 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -177,10 +177,11 @@
info.cx = intrinsics_.at<double>(0, 2);
info.cy = intrinsics_.at<double>(1, 2);
+ UndistortDetection(det);
+
apriltag_pose_t pose;
double err = estimate_tag_pose(&info, &pose);
- UndistortDetection(det);
VLOG(1) << "err: " << err;
results.emplace_back(*det, pose);