From 09dd32a6ce9c1a3e9de79e365ba0d584ca135309 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sun, 10 Dec 2017 03:08:39 +0300
Subject: [PATCH] =?UTF-8?q?Continue=20=C2=ABfixing=C2=BB=20weird=20bugs=20?=
 =?UTF-8?q?in=20aruco=20functions?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 aruco_pose/src/aruco_pose.cpp |  6 +--
 aruco_pose/src/fix.cpp        | 84 +++++++++++++++++++++++++++++++++++
 2 files changed, 87 insertions(+), 3 deletions(-)

diff --git a/aruco_pose/src/aruco_pose.cpp b/aruco_pose/src/aruco_pose.cpp
index 54eeaf5d..7723dd35 100644
--- a/aruco_pose/src/aruco_pose.cpp
+++ b/aruco_pose/src/aruco_pose.cpp
@@ -91,6 +91,8 @@ cv::Ptr<cv::aruco::Board> createCustomBoard(int markersX, int markersY, float ma
     return res;
 }
 
+#include "fix.cpp"
+
 void ArucoPose::createBoard()
 {
     static auto map_image_pub = nh_priv_.advertise<sensor_msgs::Image>("map_image", 1, true);
@@ -129,7 +131,7 @@ void ArucoPose::createBoard()
         }
 
         // Publish map image for debugging
-        cv::aruco::drawPlanarBoard(board, cv::Size(2000, 2000), map_image, 50, 1);
+        _drawPlanarBoard(board,  cv::Size(2000, 2000), map_image, 50, 1);
 
         cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
 
@@ -162,8 +164,6 @@ cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
     return res;
 }
 
-#include "fix.cpp"
-
 void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) {
     cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
 
diff --git a/aruco_pose/src/fix.cpp b/aruco_pose/src/fix.cpp
index 294ff569..c15bf3b7 100644
--- a/aruco_pose/src/fix.cpp
+++ b/aruco_pose/src/fix.cpp
@@ -1,4 +1,5 @@
 using namespace cv;
+using namespace cv::aruco;
 
 // Temporal fix!
 // TODO: remove
@@ -59,3 +60,86 @@ int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<a
     // divide by four since all the four corners are concatenated in the array for each marker
     return (int)objPoints.total() / 4;
 }
+
+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
+        dictionary.drawMarker(_board->ids[m], dst_sz.width, marker, borderBits);
+
+        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);
+    }
+}