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
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)
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")
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")
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