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

@@ -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)
 

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<tf2_ros::TransformBroadcaster> br_;
+	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
@@ -81,30 +80,32 @@ private:
 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<std::string>("known_tilt", known_tilt_, "");
-		nh_priv_.param("auto_flip", auto_flip_, false);
+		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
+		auto_flip_ = nh_priv_.param("auto_flip", false);
 
-		nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
+		frame_id_prefix_ = nh_priv_.param<std::string>("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<cv::aruco::PREDEFINED_DICTIONARY_NAME>(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 @@ private:
 
 				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 @@ private:
 						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 @@ private:
 		return frame_id_prefix_ + std::to_string(id);
 	}
 
-	void readLengthOverride()
+	void readLengthOverride(ros::NodeHandle& nh)
 	{
 		std::map<std::string, double> 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;
 		}

aruco_pose/src/aruco_map.cpp

@@ -58,7 +58,6 @@ typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray
 
 class ArucoMap : public nodelet::Nodelet {
 private:
-	ros::NodeHandle nh_, nh_priv_;
 	ros::Publisher img_pub_, pose_pub_, markers_pub_, vis_markers_pub_;
 	image_transport::Publisher debug_pub_;
 	message_filters::Subscriber<Image> image_sub_;
@@ -83,8 +82,8 @@ private:
 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 @@ public:
 		board_->dictionary = cv::aruco::getPredefinedDictionary(
 			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(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<std::string>("type", type, "map");
-		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
-		nh_priv_.param<std::string>("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<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
-		nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");
+		type = nh_priv_.param<std::string>("type", "map");
+		transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
+		known_tilt_ = nh_priv_.param<std::string>("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<std::string>("markers/frame_id", transform_.child_frame_id);
+		markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");
 
 		// createStripLine();
 
@@ -116,7 +114,7 @@ public:
 			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 @@ publish_debug:
 		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}
 
-	void createGridBoard()
+	void createGridBoard(ros::NodeHandle& nh)
 	{
 		NODELET_INFO("generate gridboard");
 		NODELET_WARN("gridboard maps are deprecated");
@@ -339,15 +337,15 @@ publish_debug:
 		int markers_x, markers_y, first_marker;
 		double markers_side, markers_sep_x, markers_sep_y;
 		std::vector<int> marker_ids;
-		nh_priv_.param<int>("markers_x", markers_x, 10);
-		nh_priv_.param<int>("markers_y", markers_y, 10);
-		nh_priv_.param<int>("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();

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<double>(i, j) = cinfo->K[3 * i + j];
-
-	for (unsigned int k = 0; k < cinfo->D.size(); k++)
-		dist.at<double>(k) = cinfo->D[k];
+	dist = cv::Mat(cinfo->D, true);
 }
 
 inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)