How can I show a set of 2D coordinates detected by opencv in rviz
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()
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.
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.