From 306185aafed72b2742011ea2f9917e0117160398 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 22 Feb 2019 11:11:19 +0300
Subject: [PATCH] =?UTF-8?q?aruco=5Fdetect:=20add=20length=5Foverride=20par?=
 =?UTF-8?q?ameter=20for=20overriding=20individual=20marker=E2=80=99s=20len?=
 =?UTF-8?q?gth?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 aruco_pose/src/aruco_detect.cpp | 47 +++++++++++++++++++++++++++++++--
 1 file changed, 45 insertions(+), 2 deletions(-)

diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp
index 42d4f64f..4281b233 100644
--- a/aruco_pose/src/aruco_detect.cpp
+++ b/aruco_pose/src/aruco_detect.cpp
@@ -17,6 +17,8 @@
 #include <math.h>
 #include <vector>
 #include <string>
+#include <map>
+#include <unordered_map>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -62,6 +64,7 @@ private:
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	bool estimate_poses_, send_tf_;
 	double length_;
+	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, snap_orientation_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
@@ -81,6 +84,8 @@ public:
 			ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
 		}
+		readLengthOverride();
+
 		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
 		nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
 
@@ -128,6 +133,24 @@ private:
 				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
 				                                     rvecs, tvecs);
 
+				// process length override, TODO: efficiency
+				if (!length_override_.empty()) {
+					for (unsigned int i = 0; i < ids.size(); i++) {
+						int id = ids[i];
+						auto item = length_override_.find(id);
+						if (item != length_override_.end()) { // found override
+							vector<cv::Vec3d> rvecs_current, tvecs_current;
+							vector<vector<cv::Point2f>> corners_current;
+							corners_current.push_back(corners[i]);
+							cv::aruco::estimatePoseSingleMarkers(corners_current, item->second,
+							                                     camera_matrix_, dist_coeffs_,
+										                         rvecs_current, tvecs_current);
+							rvecs[i] = rvecs_current[0];
+							tvecs[i] = tvecs_current[0];
+						}
+					}
+				}
+
 				if (!snap_orientation_.empty()) {
 					try {
 						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, snap_orientation_,
@@ -181,7 +204,7 @@ private:
 
 			for (unsigned int i = 0; i < ids.size(); i++)
 				pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
-				               length_, ids[i], i);
+				               getMarkerLength(ids[i]), ids[i], i);
 
 			vis_markers_pub_.publish(vis_array_);
 		}
@@ -192,7 +215,8 @@ private:
 			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
 			if (estimate_poses_)
 				for (unsigned int i = 0; i < ids.size(); i++)
-					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_);
+					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
+					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
 
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = msg->header.frame_id;
@@ -279,6 +303,25 @@ private:
 	{
 		return frame_id_prefix_ + std::to_string(id);
 	}
+
+	void readLengthOverride()
+	{
+		std::map<std::string, double> length_override;
+		nh_priv_.getParam("length_override", length_override);
+		for (auto const& item : length_override) {
+			length_override_[std::stoi(item.first)] = item.second;
+		}
+	}
+
+	inline double getMarkerLength(int id)
+	{
+		auto item = length_override_.find(id);
+		if (item != length_override_.end()) {
+			return item->second;
+		} else {
+			return length_;
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)