def localize_detection(self, detection): """ 2d image detection -> 3d point in [map frame] """ u = detection.x + detection.width / 2 v = detection.y + detection.height / 2 camera_info = None best_time = 100 for ci in self.camera_infos: time = abs(ci.header.stamp.to_sec() - detection.header.stamp.to_sec()) if time < best_time: camera_info = ci best_time = time if not camera_info or best_time > 1: return False, None camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) point = Point( ((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) localization = self.localize(detection.header, point, self.region_scope) if not localization: return False, None point_trans = None print "Face localized ", localization.pose.position.x, localization.pose.position.y if localization.pose.position.x == 0.0: print "Ignoring this one!" return False, None try: #(pos_trans, rot) = self.tf_listener.lookupTransform('/map', detection.header.frame_id, rospy.Time(0)) point_trans = self.tf_listener.transformPoint( '/map', PointStamped(detection.header, localization.pose.position)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): pass if point_trans == None: # transformation failed return False, None print "Face detected at %d,%d" % (point_trans.point.x, point_trans.point.y) return True, point_trans.point
class coord_converter: def __init__(self): self.image_sub = rospy.Subscriber("hough_circles/circles", CircleArrayStamped, self.callback) self.camera_info_msg = None self.camera_model = PinholeCameraModel() self.image_info_sub = rospy.Subscriber("camera1/camera_info", CameraInfo, self.image_info_cb, queue_size=1) # if self.camera_info_msg is not None: # print('PinholeCameraModel parameters:\n') # print('cx: ' + str(self.camera_model.cx())) # print('cy: ' + str(self.camera_model.cy())) # print('fx: ' + str(self.camera_model.fx())) # print('fy: ' + str(self.camera_model.fy())) def callback(self,data): print('\nIMAGE COORDINATES (center):') for i in range (len(data.circles)): print(' circle' + str(i+1) + ' detected:') print(' x: ' + str(data.circles[i].center.x)) print(' y: ' + str(data.circles[i].center.y)) print('\nWORLD COORDINATES (center):') print(' camera_robot coordinates:\n x: 1.4\n y: 2.2') for i in range (len(data.circles)): print(' circle' + str(i+1) + ' detected:') # using the equations showed at the beggining of this script: x_world = ((data.circles[i].center.x) - self.camera_model.cx()) * 1.25 / self.camera_model.fx() #z_world = camera rod height = 1.25 y_world = ((data.circles[i].center.y) - self.camera_model.cy()) * 1.25 / self.camera_model.fy() print(' x: ' + str(1.4 + x_world)) #x=0 from image = 1.4 from world print(' y: ' + str(2.6 - y_world)) #y=0 from image = 2.6 from world / direction of y axis from world = -y from image def image_info_cb(self, msg): if self.camera_info_msg is None: self.camera_model.fromCameraInfo(msg) self.camera_info_msg = msg
def detections_callback(self, detection): global counter global maxDistance #the maximal distance between two points required for them to be considered the same detection global alreadyDetected global n global lastAdded global detected #global tf global numFaces u = detection.x + detection.width / 2 v = detection.y + detection.height / 2 camera_info = None best_time = 100 for ci in self.camera_infos: time = abs(ci.header.stamp.to_sec() - detection.header.stamp.to_sec()) if time < best_time: camera_info = ci best_time = time if not camera_info or best_time > 1: print("Best time is higher than 1") return camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) point = Point( ((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) localization = self.localize(detection.header, point, self.region_scope) transformedPoint = point if not localization: return now = detection.header.stamp self.tf.waitForTransform("camera_rgb_optical_frame", "map", now, rospy.Duration(5.0)) m = geometry_msgs.msg.PoseStamped() m.header.frame_id = "camera_rgb_optical_frame" m.pose = localization.pose m.header.stamp = detection.header.stamp transformedPoint = self.tf.transformPose("map", m) if (localization.pose.position.x != 0.0): #print("Localisation: ", localization) print("Transformation: ", transformedPoint) #print("Point: ", point) if counter == 0: #print("Counter is zero.") counter += 1 lastAdded = transformedPoint #print("Adding the point to the array.") #alreadyDetected.append(transformedPoint) #print(len(alreadyDetected)) else: #print("Counter: ", counter) dist = self.distance(transformedPoint, lastAdded) #print("Number of detected faces so far: ", len(detected)) print("Distance is ", dist) if dist <= maxDistance: #print("-----------Less then max----------------------------------") if counter < 2 or counter >= 2: #alreadyDetected.append(transformedPoint) #print(len(alreadyDetected)) lastAdded = transformedPoint counter += 1 beenDetected = False for p in detected: #print("Checking already detected") #print("Distance: ", self.distance(p, transformedPoint)) if self.distance(p, transformedPoint) <= maxDistance: #print("This face has already been detected") #print("Distance: ", self.distance(p, transformedPoint)) beenDetected = True break if (beenDetected == False): print( "-----------------Good to go------------------------" ) detected.append(lastAdded) marker = Marker() #print("Localisation: ", localization) marker.header.stamp = detection.header.stamp marker.header.frame_id = detection.header.frame_id marker.pose = localization.pose marker.type = Marker.CUBE marker.action = Marker.ADD marker.frame_locked = False marker.lifetime = rospy.Duration.from_sec(1) marker.id = self.marker_id_counter marker.scale = Vector3(0.1, 0.1, 0.1) marker.color = ColorRGBA(1, 0, 0, 1) self.markers.markers.append(marker) self.marker_id_counter += 1 self.soundhandle.say("I detected a face.", blocking=False) print("Number of detected faces: ", len(detected)) if len(detected) == numFaces: #publish stop #rospy.init_node('mapper', anonymous=True) #msg = String() #msg.data = "Found all faces." print("Sending shutdown") self.pub.publish("Found all faces.") else: #print("-----------------------------------------More then max----") lastAdded = transformedPoint else: print("Localisation: ", localization)
class PixelConversion(): def __init__(self): self.image_topic = "wall_camera/image_raw" self.image_info_topic = "wall_camera/camera_info" self.point_topic = "/clicked_point" self.frame_id = "/map" self.pixel_x = 350 self.pixel_y = 215 # What we do during shutdown rospy.on_shutdown(self.cleanup) # Create the OpenCV display window for the RGB image self.cv_window_name = self.image_topic cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL) cv.MoveWindow(self.cv_window_name, 25, 75) # Create the cv_bridge object self.bridge = CvBridge() self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1) self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1) self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4) self.line_pub = rospy.Publisher("line", Marker, queue_size = 10) self.inter_pub = rospy.Publisher("inter", Marker, queue_size = 10) self.plane_pub = rospy.Publisher("plane", Marker, queue_size = 10) self.points = [] self.ready_for_image = False self.has_pic = False self.camera_info_msg = None self.tf_listener = tf.TransformListener() # self.display_picture() self.camera_model = PinholeCameraModel() def point_cb(self, point_msg): if len(self.points) < 4: self.points.append(point_msg.point) if len(self.points) == 1: self.ready_for_image = True print "Point stored from click: ", point_msg.point def image_cb(self, img_msg): if self.ready_for_image: # Use cv_bridge() to convert the ROS image to OpenCV format try: frame = self.bridge.imgmsg_to_cv2(img_msg, "bgr8") except CvBridgeError, e: print e # Convert the image to a Numpy array since most cv2 functions # require Numpy arrays. frame = np.array(frame, dtype=np.uint8) print "Got array" # Process the frame using the process_image() function #display_image = self.process_image(frame) # Display the image. self.img = frame # self.has_pic = True if self.camera_info_msg is not None: print "cx ", self.camera_model.cx() print "cy ", self.camera_model.cy() print "fx ", self.camera_model.fx() print "fy ", self.camera_model.fy() # Get the point from the clicked message and transform it into the camera frame self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5)) (trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0)) self.tfros = tf.TransformerROS() tf_mat = self.tfros.fromTranslationRotation(trans, rot) point_in_camera_frame = tuple(np.dot(tf_mat, np.array([self.points[0].x, self.points[0].y, self.points[0].z, 1.0])))[:3] # Get the pixels related to the clicked point (the point must be in the camera frame) pixel_coords = self.camera_model.project3dToPixel(point_in_camera_frame) print "Pixel coords ", pixel_coords # Get the unit vector related to the pixel coords unit_vector = self.camera_model.projectPixelTo3dRay((int(pixel_coords[0]), int(pixel_coords[1]))) print "Unit vector ", unit_vector # TF the unit vector of the pixel to the frame of the clicked point from the map frame # self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5)) # (trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0)) # self.tfros = tf.TransformerROS() # tf_mat = self.tfros.fromTranslationRotation(trans, rot) # xyz = tuple(np.dot(tf_mat, np.array([unit_vector[0], unit_vector[1], unit_vector[2], 1.0])))[:3] # print "World xyz ", xyz # intersect the unit vector with the ground plane red = (0,125,255) cv2.circle(self.img, (int(pixel_coords[0]), int(pixel_coords[1])), 30, red) cv2.imshow(self.image_topic, self.img) ''' trying better calculations for going from pixel to world ''' # need plane of the map in camera frame points # (0, 0, 0), (1, 1, 0), (-1, -1, 0) = map plane in map frame coordinates p1 = [-94.0, -98.0, 0.0] p2 = [-92.0, -88.0, 0.0] p3 = [-84.0, -88.0, 0.0] # p2 = [1.0, -1.0, 0.0] # p3 = [-1.0, -1.0, 0.0] # point_marker = self.create_points_marker([p1, p2, p3], "/map") # self.plane_pub.publish(point_marker) self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5)) (trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0)) self.tfros = tf.TransformerROS() tf_mat = self.tfros.fromTranslationRotation(trans, rot) # pts in camera frame coodrds p1 = list(np.dot(tf_mat, np.array([p1[0], p1[1], p1[2], 1.0])))[:3] p2 = list(np.dot(tf_mat, np.array([p2[0], p2[1], p2[2], 1.0])))[:3] p3 = list(np.dot(tf_mat, np.array([p3[0], p3[1], p3[2], 1.0])))[:3] plane_norm = self.get_plane_normal(p1, p2, p3) ## maybe viz print "P1 ", p1 print "P2 ", p2 print "P3 ", p3 print "Plane norm: ", plane_norm point_marker = self.create_points_marker([p1, p2, p3], img_msg.header.frame_id) self.plane_pub.publish(point_marker) # need unit vector to define ray to cast onto plane line_pt = list(unit_vector) line_norm = self.get_line_normal([0.0,0.0,0.0], np.asarray(line_pt) * 10) # just scale the unit vector for another point, maybe just use both unit vector for the point and the line inter = self.line_plane_intersection(line_pt, line_norm, p1, plane_norm) ## intersection is in the camera frame... ### tf this into map frame self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5)) (trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0)) self.tfros = tf.TransformerROS() tf_mat = self.tfros.fromTranslationRotation(trans, rot) xyz = tuple(np.dot(tf_mat,np.array([inter[0], inter[1], inter[2], 1.0])))[:3] print "intersection pt: ", xyz point_marker = self.create_point_marker(xyz, "/map") self.inter_pub.publish(point_marker) line_marker = self.create_line_marker([0.0,0.0,0.0], np.asarray(unit_vector) * 30, img_msg.header.frame_id) self.line_pub.publish(line_marker) self.ready_for_image = False self.points = []
def detections_callback(self, detection): global counter global maxDistance #the maximal distance between two points required for them to be considered the same detection global alreadyDetected global n global lastAdded global detected global numFaces global brojach global avgx global avgy global avgr global flag global maxHeight global minHeight global maxDistRing if flag == 0: if brojach < 10: avgx = avgx + detection.pose.position.x avgy = avgy + detection.pose.position.y avgr = avgr + detection.pose.position.z brojach = brojach + 1 return if brojach == 10: tp = detection avgx = avgx / 10 avgy = avgy / 10 avgr = avgr / 10 camera_info = None best_time = 100 for ci in self.camera_infos: time = abs(ci.header.stamp.to_sec() - detection.header.stamp.to_sec()) if time < best_time: camera_info = ci best_time = time if not camera_info or best_time > 1: print("Best time is higher than 1") tp.pose.position.z = -1 self.pub_avg_ring.publish(tp) flag = 1 return camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) p = Point( ((avgx - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((avgy + avgr - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 0) #p2 = Point(((avgx - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),((avgy -avgr - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 0) avgx = avgy = avgr = 0 lp = self.localize(detection.header, p1, self.region_scope) #lp2 = self.localize(detection.header, p2, self.region_scope) self.tf.waitForTransform(detection.header.frame_id, "map", rospy.Time.now(), rospy.Duration(5.0)) mp = geometry_msgs.msg.PoseStamped() mp.header.frame_id = detection.header.frame_id mp.pose = lp.pose mp.header.stamp = detection.header.stamp tp = self.tf.transformPose("map", mp1) #mp2 = geometry_msgs.msg.PoseStamped() #mp2.header.frame_id = detection.header.frame_id #mp2.pose = lp2.pose #mp2.header.stamp = detection.header.stamp #tp2 = self.tf.transformPose("map", mp2) now2 = rospy.Time.now() self.tf2.waitForTransform("base_link", "map", now2, rospy.Duration(5.0)) robot = geometry_msgs.msg.PoseStamped() robot.header.frame_id = "base_link" robot.pose.position.x = 0 robot.pose.position.y = 0 robot.header.stamp = now2 robotmap = self.tf2.transformPose("map", robot) if lp1.pose.position.z != 0: dp1 = self.distance(robotmap, tp) if dp > maxDistRing: print("Distance too big: ", dp) tp.pose.position.z = -1 if tp.pose.position.z > maxHeight + 0.2 or tp.pose.position.z < minHeight: print("Height is wrong: ", tp.pose.position.z) tp.pose.position.z = -1 #if ((point1.y - point2.y)/2 > 0.7 or (point1.y - point2.y)/2 < 0.5): # tp.pose.position.z = -1 #if (point1.z > 0.5 or point1.z < 0.3): # tp.pose.position.z = -1 #if (point2.z > 0.5 or point2.z < 0.3): # visina # tp.pose.position.z = -1 else: print("Localizer failed") tp.pose.position.z = -1 self.pub_avg_ring.publish(tp) flag = 1 return if flag == 1: u = detection.pose.position.x #u = avgx+avgr v = detection.pose.position.y + detection.pose.position.z #v = avgy w = detection.pose.position.x #w = avgx-avgr q = detection.pose.position.y - detection.pose.position.z g = detection.pose.position.x #w = avgx-avgr h = detection.pose.position.y + detection.pose.position.z + 3 r = detection.pose.position.x #w = avgx-avgr t = detection.pose.position.y + detection.pose.position.z - 3 #q = avgy camera_info = None best_time = 100 for ci in self.camera_infos: time = abs(ci.header.stamp.to_sec() - detection.header.stamp.to_sec()) if time < best_time: camera_info = ci best_time = time if not camera_info or best_time > 1: print("Best time is higher than 1") return camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) point1 = Point(((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) point2 = Point(((w - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((q - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) point3 = Point(((g - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((h - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) point4 = Point(((r - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((t - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) localization1 = self.localize(detection.header, point1, self.region_scope) localization2 = self.localize(detection.header, point2, self.region_scope) localization3 = self.localize(detection.header, point3, self.region_scope) localization4 = self.localize(detection.header, point4, self.region_scope) if not (localization1 or localization2 or localization3 or localization): print("Localization failed") return now = detection.header.stamp self.tf.waitForTransform(detection.header.frame_id, "map", now, rospy.Duration(5.0)) m = geometry_msgs.msg.PoseStamped() m.header.frame_id = detection.header.frame_id m.pose = localization1.pose m.header.stamp = detection.header.stamp transformedPoint1 = self.tf.transformPose("map", m) m2 = geometry_msgs.msg.PoseStamped() m2.header.frame_id = detection.header.frame_id m2.pose = localization2.pose m2.header.stamp = detection.header.stamp transformedPoint2 = self.tf.transformPose("map", m2) m3 = geometry_msgs.msg.PoseStamped() m3.header.frame_id = detection.header.frame_id m3.pose = localization3.pose m3.header.stamp = detection.header.stamp transformedPoint3 = self.tf.transformPose("map", m3) m4 = geometry_msgs.msg.PoseStamped() m4.header.frame_id = detection.header.frame_id m4.pose = localization4.pose m4.header.stamp = detection.header.stamp transformedPoint4 = self.tf.transformPose("map", m4) now2 = rospy.Time.now() self.tf2.waitForTransform("base_link", "map", now2, rospy.Duration(5.0)) robot = geometry_msgs.msg.PoseStamped() robot.header.frame_id = "base_link" robot.pose.position.x = 0 robot.pose.position.y = 0 robot.header.stamp = now2 robotmap = self.tf2.transformPose("map", robot) if localization1.pose.position.z != 0: dist1 = self.distance(robotmap, transformedPoint1) else: dist1 = 100000 if localization2.pose.position.z != 0: dist2 = self.distance(robotmap, transformedPoint2) else: dist2 = 100000 if localization3.pose.position.z != 0: dist3 = self.distance(robotmap, transformedPoint3) else: dist3 = 100000 if localization4.pose.position.z != 0: dist4 = self.distance(robotmap, transformedPoint4) else: dist4 = 100000 # find smallest distance to a point. dist = dist1 transformedPoint = transformedPoint1 if dist2 < dist: dist = dist2 transformedPoint = transformedPoint2 if dist3 < dist: dist = dist3 transformedPoint = transformedPoint3 if dist4 < dist: dist = dist4 transformedPoint = transformedPoint4 print("distance: ", dist) if (dist < maxDistRing): radius = abs((transformedPoint1.pose.position.y - transformedPoint2.pose.position.y) / 2) print("radius: ", radius) print("height: ", transformedPoint.pose.position.z) if (dist1 < maxDistRing and dist2 < maxDistRing) or (dist1 > maxDistRing and dist2 > maxDistRing): print("Checking radius") if (radius > 0.7 or radius < 0.3): print("Wrong radius") return else: print("Cant use radius") if (transformedPoint.pose.position.z > maxHeight or transformedPoint.pose.position.z < minHeight): print("Wrong height") return print("start checking") print("Detected rings: ", len(detected)) if len(detected) == 0: beenDetected = False else: beenDetected = False for p in detected: if self.distance(p, transformedPoint) <= maxDistance: print("Already detected ring!") beenDetected = True break if (beenDetected == False): detected.append(transformedPoint) print("Publishing new ring") self.pub_ring.publish(transformedPoint) marker = Marker() marker.header.stamp = detection.header.stamp marker.header.frame_id = transformedPoint.header.frame_id marker.pose.position.x = transformedPoint.pose.position.x marker.pose.position.y = transformedPoint.pose.position.y marker.pose.position.z = transformedPoint.pose.position.z marker.type = Marker.CUBE marker.action = Marker.ADD marker.frame_locked = False marker.lifetime = rospy.Duration.from_sec(1) marker.id = self.marker_id_counter marker.scale = Vector3(0.1, 0.1, 0.1) marker.color = ColorRGBA(1, 0, 0, 1) self.markers.markers.append(marker) self.marker_id_counter += 1 print("Number of detected rings: ", len(detected)) #if len(detected) == numFaces: # print("Sending shutdown") else: print("Too far away")
class GridToImageConverter(): def __init__(self): self.image_topic = "wall_camera/image_raw" self.image_info_topic = "wall_camera/camera_info" self.point_topic = "/clicked_point" self.frame_id = "/map" self.occupancy self.pixel_x = 350 self.pixel_y = 215 # What we do during shutdown rospy.on_shutdown(self.cleanup) # Create the OpenCV display window for the RGB image self.cv_window_name = self.image_topic cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL) cv.MoveWindow(self.cv_window_name, 25, 75) # Create the cv_bridge object self.bridge = CvBridge() self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1) self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1) self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4) self.points = [] self.ready_for_image = False self.has_pic = False self.camera_info_msg = None self.tf_listener = tf.TransformListener() # self.display_picture() self.camera_model = PinholeCameraModel() def point_cb(self, point_msg): if len(self.points) < 4: self.points.append(point_msg.point) if len(self.points) == 1: self.ready_for_image = True print "Point stored from click: ", point_msg.point def image_cb(self, img_msg): if self.ready_for_image: # Use cv_bridge() to convert the ROS image to OpenCV format try: frame = self.bridge.imgmsg_to_cv2(img_msg, "bgr8") except CvBridgeError, e: print e # Convert the image to a Numpy array since most cv2 functions # require Numpy arrays. frame = np.array(frame, dtype=np.uint8) print "Got array" # Process the frame using the process_image() function #display_image = self.process_image(frame) # Display the image. self.img = frame # self.has_pic = True if self.camera_info_msg is not None: print "cx ", self.camera_model.cx() print "cy ", self.camera_model.cy() print "fx ", self.camera_model.fx() print "fy ", self.camera_model.fy() # Get the point from the clicked message and transform it into the camera frame self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5)) (trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0)) self.tfros = tf.TransformerROS() tf_mat = self.tfros.fromTranslationRotation(trans, rot) point_in_camera_frame = tuple(np.dot(tf_mat, np.array([self.points[0].x, self.points[0].y, self.points[0].z, 1.0])))[:3] # Get the pixels related to the clicked point (the point must be in the camera frame) pixel_coords = self.camera_model.project3dToPixel(point_in_camera_frame) print "Pixel coords ", pixel_coords # Get the unit vector related to the pixel coords unit_vector = self.camera_model.projectPixelTo3dRay((int(pixel_coords[0]), int(pixel_coords[1]))) print "Unit vector ", unit_vector # TF the unit vector of the pixel to the frame of the clicked point from the map frame self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5)) (trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0)) self.tfros = tf.TransformerROS() tf_mat = self.tfros.fromTranslationRotation(trans, rot) xyz = tuple(np.dot(tf_mat, np.array([unit_vector[0], unit_vector[1], unit_vector[2], 1.0])))[:3] print "World xyz ", xyz red = (0,125,255) cv2.circle(self.img, (int(pixel_coords[0]), int(pixel_coords[1])), 30, red) cv2.imshow(self.image_topic, self.img) self.ready_for_image = False self.points = [] # Process any keyboard commands self.keystroke = cv.WaitKey(5) if 32 <= self.keystroke and self.keystroke < 128: cc = chr(self.keystroke).lower() if cc == 'q': # The user has press the q key, so exit rospy.signal_shutdown("User hit q key to quit.")
def detections_callback(self, detection): global counter global maxDistance #the maximal distance between two points required for them to be considered the same detection global alreadyDetected global n global lastAdded global detected global numFaces global brojach global avgx global avgy global avgr global flag if flag == 0: if brojach < 10: avgx = avgx + detection.pose.position.x avgy = avgy + detection.pose.position.y avgr = avgr + detection.pose.position.z brojach = brojach + 1 return if brojach == 10: avgx = avgx / 10 avgy = avgy / 10 avgr = avgr / 10 p = Point(((avgx - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((avgy - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), avgr) lp = self.localize(detection.header, p, self.region_scope) self.tf.waitForTransform(detection.header.frame_id, "map", rospy.Time.now(), rospy.Duration(5.0)) mp = geometry_msgs.msg.PoseStamped() mp.header.frame_id = detection.header.frame_id mp.pose = lp.pose mp.header.stamp = detection.header.stamp tp = self.tf.transformPose("map", mp) if lp.pose.position.x != 0: dp = self.distance(robotmap, tp) if dp > 1.5: tp.pose.position.z = -1 #if ((point1.y - point2.y)/2 > 0.7 or (point1.y - point2.y)/2 < 0.5): # tp.pose.position.z = -1 #if (point1.z > 0.5 or point1.z < 0.3): # tp.pose.position.z = -1 #if (point2.z > 0.5 or point2.z < 0.3): # visina # tp.pose.position.z = -1 else: tp.pose.position.z = -1 self.pub_avg_ring.publish(tp) flag = 1 return if flag == 1: u = detection.pose.position.x #u = avgx+avgr v = detection.pose.position.y + detection.pose.position.z #v = avgy w = detection.pose.position.x #w = avgx-avgr q = detection.pose.position.y - detection.pose.position.z g = detection.pose.position.x #w = avgx-avgr h = detection.pose.position.y + detection.pose.position.z + 4 r = detection.pose.position.x #w = avgx-avgr t = detection.pose.position.y + detection.pose.position.z - 4 #q = avgy camera_info = None best_time = 100 for ci in self.camera_infos: time = abs(ci.header.stamp.to_sec() - detection.header.stamp.to_sec()) if time < best_time: camera_info = ci best_time = time if not camera_info or best_time > 1: print("Best time is higher than 1") return camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) point1 = Point(((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) point2 = Point(((w - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((q - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) point3 = Point(((g - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((h - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) point4 = Point(((r - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((t - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) localization1 = self.localize(detection.header, point1, self.region_scope) localization2 = self.localize(detection.header, point2, self.region_scope) localization3 = self.localize(detection.header, point3, self.region_scope) localization4 = self.localize(detection.header, point4, self.region_scope) if not localization1: return # calculate center center = Point(localization1.pose.position.x, localization1.pose.position.y, localization1.pose.position.z) now = detection.header.stamp self.tf.waitForTransform(detection.header.frame_id, "map", now, rospy.Duration(5.0)) m = geometry_msgs.msg.PoseStamped() m.header.frame_id = detection.header.frame_id #m.pose.position.x = center.x #m.pose.position.y = center.y m.pose = localization1.pose m.header.stamp = detection.header.stamp transformedPoint1 = self.tf.transformPose("map", m) m2 = geometry_msgs.msg.PoseStamped() m2.header.frame_id = detection.header.frame_id m2.pose = localization2.pose m2.header.stamp = detection.header.stamp transformedPoint2 = self.tf.transformPose("map", m2) m3 = geometry_msgs.msg.PoseStamped() m3.header.frame_id = detection.header.frame_id m3.pose = localization3.pose m3.header.stamp = detection.header.stamp transformedPoint3 = self.tf.transformPose("map", m3) m4 = geometry_msgs.msg.PoseStamped() m4.header.frame_id = detection.header.frame_id m4.pose = localization4.pose m4.header.stamp = detection.header.stamp transformedPoint4 = self.tf.transformPose("map", m4) now2 = rospy.Time.now() self.tf2.waitForTransform("base_link", "map", now2, rospy.Duration(5.0)) robot = geometry_msgs.msg.PoseStamped() robot.header.frame_id = "base_link" robot.pose.position.x = 0 robot.pose.position.y = 0 #robot.pose = localization1.pose robot.header.stamp = now2 robotmap = self.tf2.transformPose("map", robot) #dist = self.distance(robotmap,transformedPoint) transformedPoint = transformedPoint1 if localization1.pose.position.x != 0: dist1 = self.distance(robotmap, transformedPoint1) else: dist1 = 100000 if localization2.pose.position.x != 0: dist2 = self.distance(robotmap, transformedPoint2) transformedPoint = transformedPoint2 else: dist2 = 100000 if localization3.pose.position.x != 0: dist3 = self.distance(robotmap, transformedPoint3) transformedPoint = transformedPoint3 else: dist3 = 100000 if localization4.pose.position.x != 0: dist4 = self.distance(robotmap, transformedPoint4) transformedPoint = transformedPoint4 else: dist4 = 100000 # find smallest distance to a point. dist = dist1 if dist2 < dist: dist = dist2 if dist3 < dist: dist = dist3 if dist4 < dist: dist = dist4 print("distance: ", dist) if (dist < 1.5): rad = abs((point1.y - point2.y) / 2) print("radius: ", rad) print("height p1: ", point1.z) print("height p2: ", point2.z) #if ((point1.y - point2.y)/2 > 0.7 or (point1.y - point2.y)/2 < 0.5): # return #if (point1.z > 0.5 or point1.z < 0.3): # return #if (point2.z > 0.5 or point2.z < 0.3): # visina # return self.pub.publish(transformedPoint) print("start checking") print(len(detected)) if len(detected) == 0: detected.append(transformedPoint) beenDetected = False else: beenDetected = False for p in detected: if self.distance(p, transformedPoint) <= maxDistance: print("Already detected ring!") beenDetected = True break if (beenDetected == False): print("Publishing new ring") self.pub_ring.publish(transformedPoint) marker = Marker() marker.header.stamp = detection.header.stamp marker.header.frame_id = transformedPoint.header.frame_id marker.pose.position.x = transformedPoint.pose.position.x marker.pose.position.y = transformedPoint.pose.position.y marker.pose.position.z = transformedPoint.pose.position.z marker.type = Marker.CUBE marker.action = Marker.ADD marker.frame_locked = False marker.lifetime = rospy.Duration.from_sec(1) marker.id = self.marker_id_counter marker.scale = Vector3(0.1, 0.1, 0.1) marker.color = ColorRGBA(1, 0, 0, 1) self.markers.markers.append(marker) self.marker_id_counter += 1 print("Number of detected rings: ", len(detected)) if len(detected) == numFaces: print("Sending shutdown") else: print("Just zeros")
class Realsense: """ Class to use a Relsense camera for Hand detection """ def __init__(self): """ Constructor """ rospy.init_node("ros_yolo", anonymous=True, disable_signals=False) self.bridge = CvBridge() self.rgb_lock = threading.Lock() self.depth_lock = threading.Lock() self.cameraModel = PinholeCameraModel() self.cameraInfo = CameraInfo() self.sub_color = rospy.Subscriber("/camera/color/image_raw", Image, self.color_callback, queue_size=1) self.sub_depth = rospy.Subscriber( "/camera/aligned_depth_to_color/image_raw", Image, self.depth_callback, queue_size=1) self.sub_camera_info = rospy.Subscriber("/camera/depth/camera_info", CameraInfo, self.camera_info_callback, queue_size=1) self.target_pub = rospy.Publisher("/hand_detection/target_camera", PoseStamped) self.color_img = np.zeros((480, 640, 3), np.uint8) self.depth_img = np.zeros((480, 640), np.uint8) self.img_width = 640 self.img_height = 480 self.hand_counter = 0 self.not_hand_counter = 0 self.hand_3D_pos = np.empty((0, 3), int) def _random_centered_pixels(self, box_x, box_y, box_w, box_h, radius, samples): """ Returns an array of pixel positions in 2D """ pixels = [] center_x = int(box_w / 2) center_y = int(box_h / 2) x = box_x + center_x - radius y = box_y + center_y - radius samples_limit = radius * radius if samples < samples_limit: random_positions = random.sample(range(0, samples_limit), samples) for sample in random_positions: pixel_x = x + (sample % radius) if pixel_x > (self.img_width / 2 - 1): pixel_x = self.img_width / 2 - 1 pixel_y = y + int(sample / radius) if pixel_y > (self.img_height / 2 - 1): pixel_y = self.img_height / 2 - 1 pixels.append([pixel_x, pixel_y]) else: print("The samples number should be minor" + " than radius*radius") return pixels def _load_model(self, custom=True): if custom: model_def_path = "config/yolov3-custom-hand-arm.cfg" weights_path = "checkpoints-arm-hand/yolov3_ckpt_54.pth" class_path = "data/custom/classes.names" else: model_def_path = "config/yolov3.cfg" weights_path = "weights/yolov3.weights" class_path = "data/coco.names" self.img_size = 416 # Object confidence threshold self.conf_thres = 0.95 # IOU thresshold for non-maximum suppression self.nms_thres = 0.05 if torch.cuda.is_available(): print("---------GPU AVAILABLE---------") self.Tensor = torch.cuda.FloatTensor else: print("---------CPU AVAILABLE---------") self.Tensor = torch.FloatTensor # Create the model device = torch.device("cuda" if torch.cuda.is_available() else "cpu") self.model = Darknet(model_def_path, img_size=416).to(device) # Load the custom weights if custom: self.model.load_state_dict(torch.load(weights_path)) else: self.model.load_darknet_weights(weights_path) self.model.eval() self.classes = load_classes(class_path) cmap = plt.get_cmap("tab20b") colors = [cmap(i) for i in np.linspace(0, 1, 10000)] self.bbox_colors = random.sample(colors, len(self.classes)) def _infer(self): with self.rgb_lock: frame = self.color_img.copy() # frame = self.color_img # Display only purpose img = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) img = ImagePIL.fromarray(frame) ratio = min(self.img_size / img.size[0], self.img_size / img.size[1]) imw = round(img.size[0] * ratio) imh = round(img.size[1] * ratio) img_transforms = transforms.Compose([ transforms.Resize((imh, imw)), transforms.Pad((max(int( (imh - imw) / 2), 0), max(int( (imw - imh) / 2), 0), max(int( (imh - imw) / 2), 0), max(int((imw - imh) / 2), 0)), (128, 128, 128)), transforms.ToTensor(), ]) image_tensor = img_transforms(img).float() image_tensor = image_tensor.unsqueeze_(0) input_img = Variable(image_tensor.type(self.Tensor)) with torch.no_grad(): detections = self.model(input_img) detections = non_max_suppression(detections, self.conf_thres, self.nms_thres) if detections[0] is not None: unit_scaling = 0.001 center_x = self.cameraModel.cx() center_y = self.cameraModel.cy() constant_x = unit_scaling / self.cameraModel.fx() constant_y = unit_scaling / self.cameraModel.fy() # Rescale boxes to original image detections = rescale_boxes(detections[0], self.img_size, frame.shape[:2]) unique_labels = detections[:, -1].cpu().unique() for x1, y1, x2, y2, conf, cls_conf, cls_pred in detections: box_w = x2 - x1 box_h = y2 - y1 w_proportion = box_w / img.size[0] h_proportion = box_h / img.size[1] if ((self.classes[int(cls_pred)] == 'Human hand' or self.classes[int(cls_pred)] == 'Human arm') and w_proportion >= 0.1 and w_proportion <= 0.90 and h_proportion >= 0.1 and h_proportion <= 1.0 and box_w > 10 and box_h > 10): color = self.bbox_colors[int( np.where(unique_labels == int(cls_pred))[0])] frame = cv2.rectangle( frame, (x1, y1), (x2, y2), (color[0] * 255, color[1] * 255, color[2] * 255), 2) cv2.putText(frame, self.classes[int(cls_pred)], (x1, y1), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA) radius = 5 samples = 10 pixels = self._random_centered_pixels( float(x1), float(y1), box_w, box_h, radius, samples) depthPoint = [] arrayPoints = np.empty((0, 3), int) for i in range(0, samples - 1): x = int(pixels[i][0]) y = int(pixels[i][1]) depthPoint = np.array([ (x - center_x / 2) * self.depth_img[y, x] * constant_x, (y - center_y / 2) * self.depth_img[y, x] * constant_y, self.depth_img[y, x] * unit_scaling ]) if depthPoint.sum() > 0: arrayPoints = np.append(arrayPoints, [depthPoint], axis=0) if len(arrayPoints) > 0: mean_x = np.mean(arrayPoints[:, 0], axis=0) mean_y = np.mean(arrayPoints[:, 1], axis=0) mean_z = np.mean(arrayPoints[:, 2], axis=0) pos_3D = np.array([mean_x, mean_y, mean_z]) if self.hand_counter == 0: self.hand_3D_pos = pos_3D self.hand_counter = 1 elif mean_z > 0.25 and mean_z < 1.20: dist = np.linalg.norm(self.hand_3D_pos - pos_3D) if ((dist < 0.10) or (pos_3D[2] < self.hand_3D_pos[2])): self.hand_3D_pos = pos_3D self.hand_counter = self.hand_counter + 1 if self.hand_counter > 2: # if self.hand_counter == 3: # cv2.imwrite('hand.jpg', frame) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'RealSense_optical_frame' # publish p = PoseStamped() p.header.frame_id = 'target_camera' p.header.stamp = rospy.Time.now() p.pose.position.x = mean_x p.pose.position.y = mean_y p.pose.position.z = mean_z self.target_pub.publish(p) break else: self.not_hand_counter = self.not_hand_counter + 1 if self.not_hand_counter > 20: self.hand_counter = 0 self.not_hand_counter = 0 cv2.imshow('frame', frame) cv2.waitKey(1) def color_callback(self, data): """ Callback function for color image subscriber """ try: with self.rgb_lock: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") self.color_img = cv_image except CvBridgeError as e: print(e) def depth_callback(self, data): """ Callback function for depth image subscriber """ try: with self.depth_lock: cv_image = self.bridge.imgmsg_to_cv2(data, "passthrough") self.depth_img = np.array(cv_image, dtype=np.float32) except CvBridgeError as e: print(e) def camera_info_callback(self, data): """ Callback function for camera info subscriber """ try: self.cameraModel.fromCameraInfo(data) self.cameraInfo = data except Error as e: print(e)
def detections_callback(self, detection): global counter global maxDistance #the maximal distance between two points required for them to be considered the same detection global alreadyDetected global n global lastAdded global detected global numFaces u = detection.x + detection.width / 2 v = detection.y + detection.height / 2 camera_info = None best_time = 100 for ci in self.camera_infos: time = abs(ci.header.stamp.to_sec() - detection.header.stamp.to_sec()) if time < best_time: camera_info = ci best_time = time if not camera_info or best_time > 1: print("Best time is higher than 1") return camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) point = Point( ((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(), ((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 1) localization = self.localize(detection.header, point, self.region_scope) transformedPoint = point if not localization: return now = detection.header.stamp self.tf.waitForTransform("camera_rgb_optical_frame", "map", now, rospy.Duration(5.0)) m = geometry_msgs.msg.PoseStamped() m.header.frame_id = "camera_rgb_optical_frame" m.pose = localization.pose m.header.stamp = detection.header.stamp transformedPoint = self.tf.transformPose("map", m) if (localization.pose.position.x != 0.0): #print("Transformation: ", transformedPoint) beenDetected = False if counter == 0: lastAdded = transformedPoint else: lastAdded = transformedPoint for p in detected: if self.distance(p, transformedPoint) <= maxDistance: beenDetected = True break if (beenDetected == False): counter += 1 print("-----------------Good to go------------------------") detected.append(lastAdded) self.pub_face.publish(transformedPoint) marker = Marker() marker.header.stamp = detection.header.stamp marker.header.frame_id = detection.header.frame_id marker.pose = localization.pose marker.type = Marker.CUBE marker.action = Marker.ADD marker.frame_locked = False marker.lifetime = rospy.Duration.from_sec(1) marker.id = self.marker_id_counter marker.scale = Vector3(0.1, 0.1, 0.1) marker.color = ColorRGBA(1, 0, 0, 1) self.markers.markers.append(marker) self.marker_id_counter += 1 #self.soundhandle.say("I detected a face.", blocking = False) print("Number of detected faces: ", len(detected)) lastAdded = transformedPoint