From 10cf8336747045406d589a006d737771d7f2340e Mon Sep 17 00:00:00 2001
From: Christian Rauch <Rauch.Christian@gmx.de>
Date: Wed, 13 Dec 2023 22:00:15 +0100
Subject: [PATCH] rename

---
 CMakeLists.txt          |  2 +-
 src/AprilTagNode.cpp    | 10 ++++++----
 src/pose_estimation.cpp |  6 +++---
 src/pose_estimation.hpp |  4 ++--
 4 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f75858c..0adf765 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,7 +60,7 @@ ament_target_dependencies(pose_estimation tf2_ros)
 
 add_library(AprilTagNode SHARED src/AprilTagNode.cpp)
 ament_target_dependencies(AprilTagNode rclcpp rclcpp_components sensor_msgs apriltag_msgs tf2_ros image_transport cv_bridge)
-target_link_libraries(AprilTagNode apriltag::apriltag tags pose_estimation Eigen3::Eigen ${OpenCV_LIBS})
+target_link_libraries(AprilTagNode apriltag::apriltag tags pose_estimation Eigen3::Eigen)
 rclcpp_components_register_node(AprilTagNode PLUGIN "AprilTagNode" EXECUTABLE "apriltag_node")
 
 ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH})
diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp
index 7002eb4..bd20084 100644
--- a/src/AprilTagNode.cpp
+++ b/src/AprilTagNode.cpp
@@ -82,7 +82,7 @@ class AprilTagNode : public rclcpp::Node {
     const rclcpp::Publisher<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr pub_detections;
     tf2_ros::TransformBroadcaster tf_broadcaster;
 
-    estim_pose_f estimate_pose;
+    pose_estimation_f estimate_pose = nullptr;
 
     void onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci);
 
@@ -111,8 +111,8 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
     const auto frames = declare_parameter("tag.frames", std::vector<std::string>{}, descr("tag frame names per id", true));
     const auto sizes = declare_parameter("tag.sizes", std::vector<double>{}, descr("tag sizes per id", true));
 
-    // get method for estimating tag pose from homography
-    estimate_pose = estim_pose_fun.at(declare_parameter("pose_method", "pnp", descr("pose estimation method", true)));
+    // get method for estimating tag pose
+    estimate_pose = pose_estimation_methods.at(declare_parameter("pose_estimation_method", "pnp", descr("pose estimation method", true)));
 
     // detector parameters in "detector" namespace
     declare_parameter("detector.threads", td->nthreads, descr("number of threads"));
@@ -213,7 +213,9 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i
         // set child frame name by generic tag name or configured tag name
         tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id);
         const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size;
-        tf.transform = estimate_pose(det, intrinsics, size);
+        if(estimate_pose != nullptr) {
+            tf.transform = estimate_pose(det, intrinsics, size);
+        }
 
         tfs.push_back(tf);
     }
diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp
index 1c7b813..4088265 100644
--- a/src/pose_estimation.cpp
+++ b/src/pose_estimation.cpp
@@ -79,7 +79,7 @@ geometry_msgs::msg::Transform tf_from_cv(const cv::Mat_<double>& tvec, const cv:
 //     return tf_from_eigen(Eigen::Map<const Eigen::Vector3d>(pose.t->data), Eigen::Map<const Mat3>(pose.R->data));
 // };
 
-estim_pose_f apriltag_homography = [](apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize) -> geometry_msgs::msg::Transform {
+pose_estimation_f apriltag_homography = [](apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize) -> geometry_msgs::msg::Transform {
     // apriltag_detection_info_t info;
     // info.det = detection;
     // info.tagsize = tagsize;
@@ -102,7 +102,7 @@ estim_pose_f apriltag_homography = [](apriltag_detection_t* const detection, con
     return tf_from_apriltag_pose(pose);
 };
 
-estim_pose_f solve_pnp = [](apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize) -> geometry_msgs::msg::Transform {
+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;
     // std::vector<cv::Point3d> objectPoints;
     // objectPoints.emplace_back(-half_tagsize, -half_tagsize, 0);
@@ -137,7 +137,7 @@ estim_pose_f solve_pnp = [](apriltag_detection_t* const detection, const std::ar
     return tf_from_cv(tvec, rvec);
 };
 
-const std::unordered_map<std::string, estim_pose_f> estim_pose_fun{
+const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods{
     // {"from_homography", from_homography},
     // {"apriltag_orthogonal_iteration", apriltag_orthogonal_iteration},
     {"homography", apriltag_homography},
diff --git a/src/pose_estimation.hpp b/src/pose_estimation.hpp
index d3e3992..7ba1654 100644
--- a/src/pose_estimation.hpp
+++ b/src/pose_estimation.hpp
@@ -5,6 +5,6 @@
 #include <unordered_map>
 
 
-typedef std::function<geometry_msgs::msg::Transform(apriltag_detection_t* const, const std::array<double, 4>&, const double&)> estim_pose_f;
+typedef std::function<geometry_msgs::msg::Transform(apriltag_detection_t* const, const std::array<double, 4>&, const double&)> pose_estimation_f;
 
-extern const std::unordered_map<std::string, estim_pose_f> estim_pose_fun;
+extern const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods;