CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-29 14:29:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_map: add image_axis parameter for drawing axis on ~image topic

Browse Source
This commit is contained in:
Oleg Kalachev
2019-08-01 04:06:18 +03:00
parent 47bc3b90da
commit 60fd4f9c0f
3 changed files with 24 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
aruco_pose/src/aruco_map.cpp
View File

@@ -77,7 +77,7 @@ private:
visualization_msgs::MarkerArray vis_array_;
std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
int image_width_, image_height_, image_margin_;
bool auto_flip_;
bool auto_flip_, image_axis_;
public:
virtual void onInit()
@@ -104,6 +104,7 @@ public:
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_, "");
@@ -481,14 +482,15 @@ publish_debug:
cv_bridge::CvImage msg;
if (!board_->ids.empty()) {
_drawPlanarBoard(board_, size, image, image_margin_, 1);
_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
} else {
// empty map
image.create(size, CV_8UC1);
image.setTo(cv::Scalar::all(255));
msg.encoding = sensor_msgs::image_encodings::MONO8;
}
msg.encoding = sensor_msgs::image_encodings::MONO8;
msg.image = image;
img_pub_.publish(msg.toImageMsg());
}

19
aruco_pose/src/draw.cpp
View File

@@ -23,12 +23,12 @@ static void _projectPoints( InputArray objectPoints,
void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
int borderBits) {
int borderBits, bool drawAxis) {
CV_Assert(outSize.area() > 0);
CV_Assert(marginSize >= 0);
_img.create(outSize, CV_8UC1);
_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
Mat out = _img.getMat();
out.setTo(Scalar::all(255));
out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
@@ -90,6 +90,9 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
side = std::max(side, 10);
dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
if (drawAxis) {
cvtColor(marker, marker, COLOR_GRAY2RGB);
}
// interpolate tiny marker to marker position in markerZone
inCorners[0] = Point2f(-0.5f, -0.5f);
@@ -101,6 +104,18 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
BORDER_TRANSPARENT);
}
// draw axis
if (drawAxis) {
Size wholeSize; Point ofs;
out.locateROI(wholeSize, ofs);
auto out_copy = _img.getMat();
cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
line(out_copy, center, center + Point(300, 0), Scalar(255, 0, 0), 10); // x axis
line(out_copy, center, center + Point(0, -300), Scalar(0, 255, 0), 10); // y axis
line(out_copy, center, center + Point(-200, 200), Scalar(0, 0, 255), 10); // z axis
}
}
/* Draw a (potentially partially visible) line. */

3
aruco_pose/src/draw.h
View File

@@ -3,6 +3,7 @@
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img,
int marginSize, int borderBits, bool drawAxis);
void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
cv::InputArray rvec, cv::InputArray tvec, float length);