0
$\begingroup$

Rosanswers logo

I used opencv fast detection to detect the corner in the image. I tried to use visualization_msg/Marker to publish the 2D coordinate of a set of corners to rviz. However, when I add marker topic in rviz there is nothing showed in rviz.

Thanks in advance and Here is my code

#!/usr/bin/env python

import numpy as np
import cv2
import rospy
from visualization_msgs.msg import Marker
from sensor_msgs.msg import PointCloud 

# Captures a single image from the camera and returns it in PIL format
def get_image():
    # read is the easiest way to get a full image out of a VideoCapture object.
    retval, img = camera.read()
    return img

def pub_img():
    pub = rospy.Publisher('pub_coord', Marker, queue_size=100)
    rospy.init_node('pub_coord', anonymous=True)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        img = get_image()
        # Initiate FAST object with default values
        fast = cv2.FastFeatureDetector_create()

        # find and draw the keypoints
        kp = fast.detect(img,None)
        # img2 = cv2.drawKeypoints(img, kp, img, color=(255,0,0))
        img2 = cv2.drawKeypoints(img, kp, None, color=(255,0,0))
        coord = np.array([[]])
        for i in range (0, len(kp)):
            coord = np.append(coord, kp[i].pt).reshape(i+1, 2)
            # print(coord)
        if len(kp) != 0:
            size = kp[0].size
        else:
            pass
        
        fast_marker = Marker()
        fast_marker.header.stamp = rospy.get_rostime()
        fast_marker.ns = "pub_img"
        fast_marker.type = fast_marker.POINTS # point
        fast_marker.action = fast_marker.ADD
        fast_marker.header.frame_id = "map"

        if len(kp) != 0 :
            for i in range(0, len(coord)):
                fast_marker.pose.position.x = coord[i,0]
                fast_marker.pose.position.y = coord[i,1]
                fast_marker.pose.orientation.w = 1.0

                fast_marker.scale.x = 0.1
                fast_marker.scale.y = 0.1
                fast_marker.scale.z = 0.1

                fast_marker.color.b = 1.0
                fast_marker.color.a = 1.0

                pub.publish(fast_marker)
                rospy.sleep(1)
        
        # Print all default params
        print("Threshold: ", fast.getThreshold())
        print("neighborhood: ", fast.getType())
        print("Total Keypoints with nonmaxSuppression: ", len(kp))


if __name__ == '__main__':
    try:
        camera_port = -1
        camera = cv2.VideoCapture(camera_port)
        if not camera.isOpened():
            sys.stdout.write('camera not available')
        pub_img()
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
        cv2.destroyAllWindows()

Originally posted by siweiguo on ROS Answers with karma: 1 on 2017-08-21

Post score: 0


Original comments

Comment by ahendrix on 2017-08-21:
I don't think this will do what you want, even if you do get it working. You're publishing 2D markers, but rviz is 3D, and will try to draw these markers in 3D space. Since you're setting the Z to 0, it will draw them at Z=0 with respect to the frame_id.

Comment by ahendrix on 2017-08-21:
You should probably make a copy of your image, use OpenCV to draw the markers on that copy and then publish the new image instead.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The problem is likely in the frame_id assignment. You are using fast_marker.header.frame_id = "map" when you should be using your cameras frame_id value. The camera frame_id typically comes from something like a URDF.

One potential short-term solution from within rviz is to set both the global frame and target frame to map while leaving your code the same (i.e., marker frame_id is also map).


Originally posted by Thomas D with karma: 4347 on 2017-08-21

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by siweiguo on 2017-08-22:
Thank you for your comment. I changed global frame and target frame all to map but there is still nothing in rviz.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.