cleanup

CMakeLists.txt

@@ -52,16 +52,15 @@ add_library(tags OBJECT src/tag_functions.cpp)
 target_link_libraries(tags apriltag::apriltag)
 set_property(TARGET tags PROPERTY POSITION_INDEPENDENT_CODE ON)
 
-add_library(homography_to_pose OBJECT src/homography_to_pose.cpp)
-target_link_libraries(homography_to_pose apriltag::apriltag Eigen3::Eigen ${OpenCV_LIBS})
-set_property(TARGET homography_to_pose PROPERTY POSITION_INDEPENDENT_CODE ON)
-ament_target_dependencies(homography_to_pose tf2_ros)
-
+add_library(pose_estimation OBJECT src/pose_estimation.cpp)
+target_link_libraries(pose_estimation apriltag::apriltag Eigen3::Eigen ${OpenCV_LIBS})
+set_property(TARGET pose_estimation PROPERTY POSITION_INDEPENDENT_CODE ON)
+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_include_directories(AprilTagNode PRIVATE ${OpenCV_INCLUDE_DIRS})
-target_link_libraries(AprilTagNode apriltag::apriltag tags homography_to_pose Eigen3::Eigen ${OpenCV_LIBS})
+target_link_libraries(AprilTagNode apriltag::apriltag tags pose_estimation Eigen3::Eigen ${OpenCV_LIBS})
 rclcpp_components_register_node(AprilTagNode PLUGIN "AprilTagNode" EXECUTABLE "apriltag_node")
 
 ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH})

src/AprilTagNode.cpp

@@ -1,5 +1,5 @@
 // ros
-#include "homography_to_pose.hpp"
+#include "pose_estimation.hpp"
 #include <apriltag_msgs/msg/april_tag_detection.hpp>
 #include <apriltag_msgs/msg/april_tag_detection_array.hpp>
 #ifdef cv_bridge_HPP
@@ -19,9 +19,6 @@
 #include "tag_functions.hpp"
 #include <apriltag.h>
 
-//#include <Eigen/Dense>
-
-//#define _GLIBCXX_USE_CXX11_ABI 1
 
 
 #define IF(N, V) \
@@ -60,8 +57,6 @@ descr(const std::string& description, const bool& read_only = false)
     return descr;
 }
 
-//...
-
 class AprilTagNode : public rclcpp::Node {
 public:
     AprilTagNode(const rclcpp::NodeOptions& options);
@@ -118,7 +113,7 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
     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("TODO", true)));
+    estimate_pose = estim_pose_fun.at(declare_parameter("pose_method", "pnp", descr("pose estimation method", true)));
 
     // detector parameters in "detector" namespace
     declare_parameter("detector.threads", td->nthreads, descr("number of threads"));
@@ -165,9 +160,7 @@ AprilTagNode::~AprilTagNode()
 void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img,
                             const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci)
 {
-    // camera intrinsic matrix for rectified images
-    // const Mat3 P = Eigen::Map<const Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(msg_ci->p.data()).leftCols<3>();
-
+    // camera intrinsics for raw images
     const std::array<double, 4> intrinsics = {msg_ci->p.data()[0], msg_ci->p.data()[5], msg_ci->p.data()[2], msg_ci->p.data()[6]};
 
     // convert to 8bit monochrome image

src/homography_to_pose.cpp → src/pose_estimation.cpp

@@ -1,4 +1,4 @@
-#include "homography_to_pose.hpp"
+#include "pose_estimation.hpp"
 #include <apriltag/apriltag_pose.h>
 #include <apriltag/common/homography.h>
 #include <Eigen/Dense>