From 998796045c2e7aaf70001fb8208606cb11c5c8c8 Mon Sep 17 00:00:00 2001 From: Alexey Rogachevskiy Date: Sat, 30 May 2020 01:59:51 +0300 Subject: [PATCH] aruco_pose nodelet cleanup (#239) * aruco_pose: Unhardcode contour refinement Besides, this was basically a no-op anyway, since dynamic parameters overwrote that anyway. * aruco_pose: Late-construct objects that use ROS * aruco_map: Don't create/store node handle * aruco_pose: Don't assume dist_coeffs size * aruco_pose: more const == more better * aruco_pose: Be more obvious about changing variables * aruco_pose: Fix building for Kinetic * aruco_pose: Remove global add_definitions --- aruco_pose/CMakeLists.txt | 2 -- aruco_pose/src/aruco_detect.cpp | 41 +++++++++++++++--------------- aruco_pose/src/aruco_map.cpp | 44 ++++++++++++++++----------------- aruco_pose/src/utils.h | 4 +-- 4 files changed, 43 insertions(+), 48 deletions(-) diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index e72ce8fa5..30fe1e4bd 100644 --- a/aruco_pose/CMakeLists.txt +++ b/aruco_pose/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.0) project(aruco_pose) -add_definitions(-std=c++11 -Wall -g) - ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 65953e53a..7d8d1d99b 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -58,10 +58,9 @@ using cv::Mat; class ArucoDetect : public nodelet::Nodelet { private: - ros::NodeHandle nh_, nh_priv_; - tf2_ros::TransformBroadcaster br_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_{tf_buffer_}; + std::unique_ptr br_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; std::shared_ptr> dyn_srv_; cv::Ptr dictionary_; cv::Ptr parameters_; @@ -81,30 +80,32 @@ class ArucoDetect : public nodelet::Nodelet { public: virtual void onInit() { - nh_ = getNodeHandle(); - nh_priv_ = getPrivateNodeHandle(); + ros::NodeHandle& nh_ = getNodeHandle(); + ros::NodeHandle& nh_priv_ = getPrivateNodeHandle(); + + br_.reset(new tf2_ros::TransformBroadcaster()); + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh_)); int dictionary; - nh_priv_.param("dictionary", dictionary, 2); - nh_priv_.param("estimate_poses", estimate_poses_, true); - nh_priv_.param("send_tf", send_tf_, true); + dictionary = nh_priv_.param("dictionary", 2); + estimate_poses_ = nh_priv_.param("estimate_poses", true); + send_tf_ = nh_priv_.param("send_tf", true); if (estimate_poses_ && !nh_priv_.getParam("length", length_)) { NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined"); ros::shutdown(); } - readLengthOverride(); + readLengthOverride(nh_priv_); - nh_priv_.param("known_tilt", known_tilt_, ""); - nh_priv_.param("auto_flip", auto_flip_, false); + known_tilt_ = nh_priv_.param("known_tilt", ""); + auto_flip_ = nh_priv_.param("auto_flip", false); - nh_priv_.param("frame_id_prefix", frame_id_prefix_, "aruco_"); + frame_id_prefix_ = nh_priv_.param("frame_id_prefix", "aruco_"); camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); - dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F); dictionary_ = cv::aruco::getPredefinedDictionary(static_cast(dictionary)); parameters_ = cv::aruco::DetectorParameters::create(); - parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; image_transport::ImageTransport it(nh_); image_transport::ImageTransport it_priv(nh_priv_); @@ -170,8 +171,8 @@ class ArucoDetect : public nodelet::Nodelet { if (!known_tilt_.empty()) { try { - snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_, - msg->header.stamp, ros::Duration(0.02)); + snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_, + msg->header.stamp, ros::Duration(0.02)); } catch (const tf2::TransformException& e) { NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what()); } @@ -205,7 +206,7 @@ class ArucoDetect : public nodelet::Nodelet { if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) { transform.transform.rotation = marker.pose.orientation; fillTranslation(transform.transform.translation, tvecs[i]); - br_.sendTransform(transform); + br_->sendTransform(transform); } } } @@ -326,10 +327,10 @@ class ArucoDetect : public nodelet::Nodelet { return frame_id_prefix_ + std::to_string(id); } - void readLengthOverride() + void readLengthOverride(ros::NodeHandle& nh) { std::map length_override; - nh_priv_.getParam("length_override", length_override); + nh.getParam("length_override", length_override); for (auto const& item : length_override) { length_override_[std::stoi(item.first)] = item.second; } diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 4896af1e7..1193b9bb3 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -58,7 +58,6 @@ typedef message_filters::sync_policies::ExactTime image_sub_; @@ -83,8 +82,8 @@ class ArucoMap : public nodelet::Nodelet { public: virtual void onInit() { - nh_ = getNodeHandle(); - nh_priv_ = getPrivateNodeHandle(); + ros::NodeHandle &nh_ = getNodeHandle(); + ros::NodeHandle &nh_priv_ = getPrivateNodeHandle(); image_transport::ImageTransport it_priv(nh_priv_); @@ -96,19 +95,18 @@ class ArucoMap : public nodelet::Nodelet { board_->dictionary = cv::aruco::getPredefinedDictionary( static_cast(nh_priv_.param("dictionary", 2))); camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); - dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F); std::string type, map; - nh_priv_.param("type", type, "map"); - nh_priv_.param("frame_id", transform_.child_frame_id, "aruco_map"); - nh_priv_.param("known_tilt", known_tilt_, ""); - nh_priv_.param("auto_flip", auto_flip_, false); - nh_priv_.param("image_width", image_width_, 2000); - nh_priv_.param("image_height", image_height_, 2000); - nh_priv_.param("image_margin", image_margin_, 200); - nh_priv_.param("image_axis", image_axis_, true); - nh_priv_.param("markers/frame_id", markers_parent_frame_, transform_.child_frame_id); - nh_priv_.param("markers/child_frame_id_prefix", markers_frame_, ""); + type = nh_priv_.param("type", "map"); + transform_.child_frame_id = nh_priv_.param("frame_id", "aruco_map"); + known_tilt_ = nh_priv_.param("known_tilt", ""); + auto_flip_ = nh_priv_.param("auto_flip", false); + image_width_ = nh_priv_.param("image_width" , 2000); + image_height_ = nh_priv_.param("image_height", 2000); + image_margin_ = nh_priv_.param("image_margin", 200); + image_axis_ = nh_priv_.param("image_axis", true); + markers_parent_frame_ = nh_priv_.param("markers/frame_id", transform_.child_frame_id); + markers_frame_ = nh_priv_.param("markers/child_frame_id_prefix", ""); // createStripLine(); @@ -116,7 +114,7 @@ class ArucoMap : public nodelet::Nodelet { param(nh_priv_, "map", map); loadMap(map); } else if (type == "gridboard") { - createGridBoard(); + createGridBoard(nh_priv_); } else { NODELET_FATAL("unknown type: %s", type.c_str()); ros::shutdown(); @@ -331,7 +329,7 @@ class ArucoMap : public nodelet::Nodelet { NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast(board_->ids.size())); } - void createGridBoard() + void createGridBoard(ros::NodeHandle& nh) { NODELET_INFO("generate gridboard"); NODELET_WARN("gridboard maps are deprecated"); @@ -339,15 +337,15 @@ class ArucoMap : public nodelet::Nodelet { int markers_x, markers_y, first_marker; double markers_side, markers_sep_x, markers_sep_y; std::vector marker_ids; - nh_priv_.param("markers_x", markers_x, 10); - nh_priv_.param("markers_y", markers_y, 10); - nh_priv_.param("first_marker", first_marker, 0); + markers_x = nh.param("markers_x", 10); + markers_y = nh.param("markers_y", 10); + first_marker = nh.param("first_marker", 0); - param(nh_priv_, "markers_side", markers_side); - param(nh_priv_, "markers_sep_x", markers_sep_x); - param(nh_priv_, "markers_sep_y", markers_sep_y); + param(nh, "markers_side", markers_side); + param(nh, "markers_sep_x", markers_sep_x); + param(nh, "markers_sep_y", markers_sep_y); - if (nh_priv_.getParam("marker_ids", marker_ids)) { + if (nh.getParam("marker_ids", marker_ids)) { if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) { NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y"); ros::shutdown(); diff --git a/aruco_pose/src/utils.h b/aruco_pose/src/utils.h index 3f6ccd160..b7ae04321 100644 --- a/aruco_pose/src/utils.h +++ b/aruco_pose/src/utils.h @@ -35,9 +35,7 @@ static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Ma for (unsigned int i = 0; i < 3; ++i) for (unsigned int j = 0; j < 3; ++j) matrix.at(i, j) = cinfo->K[3 * i + j]; - - for (unsigned int k = 0; k < cinfo->D.size(); k++) - dist.at(k) = cinfo->D[k]; + dist = cv::Mat(cinfo->D, true); } inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)