aruco_map: enable rotating (yaw, pitch, roll) each marker in the map

aruco_pose/CMakeLists.txt

@@ -139,6 +139,7 @@ include_directories(
 add_library(aruco_pose
   src/aruco_detect.cpp
   src/aruco_map.cpp
+  src/draw.cpp
 )
 
 add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp)

aruco_pose/src/aruco_map.cpp

@@ -40,6 +40,7 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
 
+#include "draw.h"
 #include "utils.h"
 #include "gridboard.h"
 
@@ -289,17 +290,31 @@ public:
 				   double yaw, double pitch, double roll)
 	{
 		// Create transform
-		// geometry_msgs::TransformStamped t;
-		// t.transform.translation.x = x;
-		// t.transform.translation.y = y;
-		// t.transform.translation.z = z;
-		// tf::Quaternion q;
-		// q.setRPY(roll, pitch, yaw);
-		// tf::quaternionTFToMsg(q, t.transform.rotation);
+		tf::Quaternion q;
+		q.setRPY(roll, pitch, yaw);
+		tf::Transform transform(q, tf::Vector3(x, y, z));
+
+		/* marker's corners:
+			0    1
+			3    2
+		*/
+		double halflen = length / 2;
+		tf::Point p0(-halflen, halflen, 0);
+		tf::Point p1(halflen, halflen, 0);
+		tf::Point p2(halflen, -halflen, 0);
+		tf::Point p3(-halflen, -halflen, 0);
+		p0 = transform * p0;
+		p1 = transform * p1;
+		p2 = transform * p2;
+		p3 = transform * p3;
+
+		vector<cv::Point3f> obj_points = {
+			cv::Point3f(p0.x(), p0.y(), p0.z()),
+			cv::Point3f(p1.x(), p1.y(), p1.z()),
+			cv::Point3f(p2.x(), p2.y(), p2.z()),
+			cv::Point3f(p3.x(), p3.y(), p3.z())
+		};
 
-		// TODO: process roll pitch yaw
-		vector<cv::Point3f> obj_points(4);
-		setMarkerObjectPoints(x, y, z, yaw, length, obj_points);
 		board_->ids.push_back(id);
 		board_->objPoints.push_back(obj_points);
 
@@ -321,8 +336,6 @@ public:
 		marker.pose.position.x = x;
 		marker.pose.position.y = y;
 		marker.pose.position.z = z;
-		tf::Quaternion q;
-		q.setRPY(roll, pitch, yaw);
 		tf::quaternionTFToMsg(q, marker.pose.orientation);
 		marker.frame_locked = true;
 		vis_array_.markers.push_back(marker);
@@ -335,20 +348,12 @@ public:
 		vis_array_.markers.at(0).points.push_back(p);
 	}
 
-	void setMarkerObjectPoints(float x, float y, float z, float yaw, float length,
-	                           vector<cv::Point3f>& obj_points)
-	{
-		obj_points[0] = cv::Point3f(x - length / 2, y + length / 2, 0);
-		obj_points[1] = obj_points[0] + cv::Point3f(length, 0, 0);
-		obj_points[2] = obj_points[0] + cv::Point3f(length, -length, 0);
-		obj_points[3] = obj_points[0] + cv::Point3f(0, -length, 0);
-	}
-
 	void publishMapImage()
 	{
 		cv::Mat image;
 		cv_bridge::CvImage msg;
-		cv::aruco::drawPlanarBoard(board_, cv::Size(2000, 2000), image, 50, 1);
+		drawPlanarBoard(board_, cv::Size(2000, 2000), image, 200, 1);
+
 		cv::cvtColor(image, image, CV_GRAY2BGR);
 		msg.encoding = sensor_msgs::image_encodings::BGR8;
 		msg.image = image;

aruco_pose/src/draw.cpp

@@ -0,0 +1,94 @@
+// This code is basically taken from https://github.com/opencv/opencv_contrib/blob/master/modules/aruco/src/aruco.cpp
+// with some improvements and fixes
+
+#include "draw.h"
+
+void drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
+                     int borderBits) {
+
+    CV_Assert(outSize.area() > 0);
+    CV_Assert(marginSize >= 0);
+
+    _img.create(outSize, CV_8UC1);
+    Mat out = _img.getMat();
+    out.setTo(Scalar::all(255));
+    out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
+
+    // calculate max and min values in XY plane
+    CV_Assert(_board->objPoints.size() > 0);
+    float minX, maxX, minY, maxY;
+    minX = maxX = _board->objPoints[0][0].x;
+    minY = maxY = _board->objPoints[0][0].y;
+
+    for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+        for(int j = 0; j < 4; j++) {
+            minX = min(minX, _board->objPoints[i][j].x);
+            maxX = max(maxX, _board->objPoints[i][j].x);
+            minY = min(minY, _board->objPoints[i][j].y);
+            maxY = max(maxY, _board->objPoints[i][j].y);
+        }
+    }
+
+    float sizeX = maxX - minX;
+    float sizeY = maxY - minY;
+
+    // proportion transformations
+    float xReduction = sizeX / float(out.cols);
+    float yReduction = sizeY / float(out.rows);
+
+    // determine the zone where the markers are placed
+    if(xReduction > yReduction) {
+        int nRows = int(sizeY / xReduction);
+        int rowsMargins = (out.rows - nRows) / 2;
+        out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+    } else {
+        int nCols = int(sizeX / yReduction);
+        int colsMargins = (out.cols - nCols) / 2;
+        out.adjustROI(0, 0, -colsMargins, -colsMargins);
+    }
+
+    // now paint each marker
+    Dictionary &dictionary = *(_board->dictionary);
+    Mat marker;
+    Point2f outCorners[3];
+    Point2f inCorners[3];
+    for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+        // transform corners to markerZone coordinates
+        for(int j = 0; j < 3; j++) {
+            Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+            // move top left to 0, 0
+            pf -= Point2f(minX, minY);
+            pf.x = pf.x / sizeX * float(out.cols);
+            pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+            outCorners[j] = pf;
+        }
+
+        // get marker
+        Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+        // dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
+		int side = std::round(diag / std::sqrt(2));
+
+		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
+
+		try {
+			if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
+				// marker is aligned to image axes
+				marker.copyTo(out(Rect(outCorners[0], dst_sz)));
+				continue;
+			}
+
+			// interpolate tiny marker to marker position in markerZone
+			inCorners[0] = Point2f(-0.5f, -0.5f);
+			inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+			inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
+
+			// remove perspective
+			Mat transformation = getAffineTransform(inCorners, outCorners);
+			warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+							BORDER_TRANSPARENT);
+		} catch(cv::Exception& e) {
+			ROS_INFO("Skip drawing marker %d", m);
+		}
+	}
+}

aruco_pose/src/draw.h

@@ -0,0 +1,9 @@
+#include <cmath>
+#include <ros/ros.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+using namespace cv;
+using namespace cv::aruco;
+
+void drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize, int borderBits);