From 82f9b9d6c16c21a6f20b7a2756b4532a879aeb71 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 14 Feb 2019 05:46:00 +0300 Subject: [PATCH] aruco_map: enable rotating (yaw, pitch, roll) each marker in the map --- aruco_pose/CMakeLists.txt | 1 + aruco_pose/src/aruco_map.cpp | 49 ++++++++++--------- aruco_pose/src/draw.cpp | 94 ++++++++++++++++++++++++++++++++++++ aruco_pose/src/draw.h | 9 ++++ 4 files changed, 131 insertions(+), 22 deletions(-) create mode 100644 aruco_pose/src/draw.cpp create mode 100644 aruco_pose/src/draw.h diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index 7c7b0f24..92662447 100644 --- a/aruco_pose/CMakeLists.txt +++ b/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) diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 911aac91..4d2cf2e7 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -40,6 +40,7 @@ #include #include +#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 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 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& 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; diff --git a/aruco_pose/src/draw.cpp b/aruco_pose/src/draw.cpp new file mode 100644 index 00000000..a2d5f35f --- /dev/null +++ b/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); + } + } +} diff --git a/aruco_pose/src/draw.h b/aruco_pose/src/draw.h new file mode 100644 index 00000000..c0cc8297 --- /dev/null +++ b/aruco_pose/src/draw.h @@ -0,0 +1,9 @@ +#include +#include +#include +#include + +using namespace cv; +using namespace cv::aruco; + +void drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize, int borderBits);