From 90acaba523fbe4b206bfc54723b5d76502725d59 Mon Sep 17 00:00:00 2001
From: Christian Rauch <Rauch.Christian@gmx.de>
Date: Wed, 13 Dec 2023 22:25:24 +0100
Subject: [PATCH] cleanup

---
 src/pose_estimation.cpp | 51 ++++++++++++++++++++++++++---------------
 1 file changed, 33 insertions(+), 18 deletions(-)

diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp
index a902104..4dd5018 100644
--- a/src/pose_estimation.cpp
+++ b/src/pose_estimation.cpp
@@ -40,28 +40,43 @@ geometry_msgs::msg::Transform tf_from_cv(const cv::Mat_<double>& tvec, const cv:
     return t;
 }
 
-pose_estimation_f apriltag_homography = [](apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize) -> geometry_msgs::msg::Transform {
+geometry_msgs::msg::Transform
+homography(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
+{
     apriltag_detection_info_t info = {detection, tagsize, intr[0], intr[1], intr[2], intr[3]};
 
     apriltag_pose_t pose;
     estimate_pose_for_tag_homography(&info, &pose);
 
     return tf_from_apriltag_pose(pose);
-};
-
-pose_estimation_f solve_pnp = [](apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize) -> geometry_msgs::msg::Transform {
-    const double half_tagsize = 0.5 * tagsize;
-    const std::vector<cv::Point3d> objectPoints{{-half_tagsize, -half_tagsize, 0}, {+half_tagsize, -half_tagsize, 0}, {+half_tagsize, +half_tagsize, 0}, {-half_tagsize, +half_tagsize, 0}};
+}
 
-    std::vector<cv::Point2d> imagePoints;
-    constexpr double tag_x[4] = {-1, 1, 1, -1};
-    constexpr double tag_y[4] = {1, 1, -1, -1};
-    for(int i = 0; i < 4; i++) {
-        // Homography projection taking tag local frame coordinates to image pixels
-        double im_x, im_y;
-        homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y);
-        imagePoints.push_back(cv::Point2d(im_x, im_y));
-    }
+geometry_msgs::msg::Transform
+pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
+{
+    const std::vector<cv::Point3d> objectPoints{
+        {-tagsize / 2, -tagsize / 2, 0},
+        {+tagsize / 2, -tagsize / 2, 0},
+        {+tagsize / 2, +tagsize / 2, 0},
+        {-tagsize / 2, +tagsize / 2, 0},
+    };
+
+    // std::vector<cv::Point2d> imagePoints;
+    // constexpr double tag_x[4] = {-1, 1, 1, -1};
+    // constexpr double tag_y[4] = {1, 1, -1, -1};
+    // for(int i = 0; i < 4; i++) {
+    //     // Homography projection taking tag local frame coordinates to image pixels
+    //     double im_x, im_y;
+    //     homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y);
+    //     imagePoints.push_back(cv::Point2d(im_x, im_y));
+    // }
+
+    const std::vector<cv::Point2d> imagePoints{
+        {detection->p[0][0], detection->p[0][1]},
+        {detection->p[1][0], detection->p[1][1]},
+        {detection->p[2][0], detection->p[2][1]},
+        {detection->p[3][0], detection->p[3][1]},
+    };
 
     cv::Mat rvec, tvec;
     cv::Matx33d cameraMatrix;
@@ -73,9 +88,9 @@ pose_estimation_f solve_pnp = [](apriltag_detection_t* const detection, const st
     cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec);
 
     return tf_from_cv(tvec, rvec);
-};
+}
 
 const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods{
-    {"homography", apriltag_homography},
-    {"pnp", solve_pnp},
+    {"homography", homography},
+    {"pnp", pnp},
 };