From c5628cecc00a034dac84a323b144c995b1dd6528 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 22 Mar 2018 23:33:50 +0300 Subject: [PATCH] aruco_pose: publish debug image even where there is no board --- aruco_pose/src/aruco_pose.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/aruco_pose/src/aruco_pose.cpp b/aruco_pose/src/aruco_pose.cpp index dddb9f89..8f181848 100644 --- a/aruco_pose/src/aruco_pose.cpp +++ b/aruco_pose/src/aruco_pose.cpp @@ -272,10 +272,12 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs: cv::Mat distCoeffs(8, 1, CV_64F); parseCameraInfo(cinfo, cameraMatrix, distCoeffs); + int valid = 0; + cv::Mat rvec, tvec, objPoints; + if (markerIds.size() > 0) { - cv::Mat rvec, tvec, objPoints; - int valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, + valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, rvec, tvec, false, objPoints); if (valid) { @@ -301,18 +303,16 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs: ref_transform.setOrigin(ref_vector3); ref_transform.setRotation(q); br.sendTransform(ref_transform); - - if(img_pub.getNumSubscribers() > 0) - { - // Publish debug image - cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); - cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); - } } } if (img_pub.getNumSubscribers() > 0) { + cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); // draw markers + if (valid) + { + cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); // draw board axis + } cv_bridge::CvImage out_msg; out_msg.header.frame_id = msg->header.frame_id; out_msg.header.stamp = msg->header.stamp;