def bayes2spencer(self, bayes_msg):
spencer_msg = TrackedPersons()
spencer_msg.header = bayes_msg.header
for i, pose in enumerate(bayes_msg.poses):
track = TrackedPerson()
track.track_id = self.string2uint64(bayes_msg.uuids[i])
# PoseWithCovariance
track.pose.pose.position = pose.position
track.pose.pose.orientation = pose.orientation
# TwistWithCovariance
track.twist.twist.linear = bayes_msg.velocities[i]
# Data not in bayes. Not sure about these ...
track.pose.covariance = (np.random.normal(0.3, 0.1) *
np.identity(6)).flatten().tolist()
track.twist.covariance = (np.random.normal(0.3, 0.1) *
np.identity(6)).flatten().tolist()
# we assume 0 here
# track.twist.twist.angular
track.is_occluded = False
track.is_matched = False
track.detection_id = self.string2uint64(bayes_msg.uuids[i])
track.age = 0
spencer_msg.tracks.append(track)
return spencer_msg
def do_3d_tracking(self, detections_2d, detections_3d):
start = time.time()
#rospy.loginfo("Tracking frame")
# convert_detections
boxes_2d = []
boxes_3d = []
valid_3d = []
features_2d = []
features_3d = []
dets_2d = sorted(detections_2d.detection2d_with_features,
key=lambda x: x.frame_det_id)
dets_3d = sorted(detections_3d.detection3d_with_features,
key=lambda x: x.frame_det_id)
i, j = 0, 0
while i < len(dets_2d) and j < len(dets_3d):
det_2d = dets_2d[i]
det_3d = dets_3d[j]
if det_2d.frame_det_id == det_3d.frame_det_id:
i += 1
j += 1
valid_3d.append(det_3d.valid)
boxes_2d.append(
np.array([
det_2d.x1, det_2d.y1, det_2d.x2, det_2d.y2, 1, -1, -1
]))
features_2d.append(torch.Tensor(det_2d.feature).to('cuda:0'))
if det_3d.valid:
boxes_3d.append(
np.array([
det_3d.x, det_3d.y, det_3d.z, det_3d.l, det_3d.h,
det_3d.w, det_3d.theta
]))
features_3d.append(
torch.Tensor(det_3d.feature).to('cuda:0'))
else:
boxes_3d.append(None)
features_3d.append(None)
elif det_2d.frame_det_id < det_3d.frame_det_id:
i += 1
else:
j += 1
if not boxes_3d:
boxes_3d = None
features_3d, features_2d = combine_features(
features_2d,
features_3d,
valid_3d,
self.combination_model,
depth_weight=self.depth_weight)
detections = convert_detections(boxes_2d, features_3d, features_2d,
boxes_3d)
self.tracker.predict()
self.tracker.update(None, detections)
tracked_array = TrackedPersons()
tracked_array.header.stamp = detections_3d.header.stamp
tracked_array.header.frame_id = 'occam'
for track in self.tracker.tracks:
if not track.is_confirmed():
continue
#print('Confirmed track!')
pose_msg = Pose()
tracked_person_msg = TrackedPerson()
tracked_person_msg.header.stamp = detections_3d.header.stamp
tracked_person_msg.header.frame_id = 'occam'
tracked_person_msg.track_id = track.track_id
if track.time_since_update < 2:
tracked_person_msg.is_matched = True
else:
tracked_person_msg.is_matched = False
bbox = track.to_tlwh3d()
covariance = track.get_cov().reshape(-1).tolist()
pose_msg.position.x = bbox[0]
pose_msg.position.y = bbox[1] - bbox[4] / 2
pose_msg.position.z = bbox[2]
pose_msg = PoseWithCovariance(pose=pose_msg, covariance=covariance)
tracked_person_msg.pose = pose_msg
tracked_array.tracks.append(tracked_person_msg)
self.tracker_output_pub.publish(tracked_array)
def newSegmentationReceived(self, laserscan, laserscanSegmentation, occludedTracks):
if (laserscan.header.stamp != laserscanSegmentation.header.stamp):
rospy.logwarn("Different timestamps laser: {} segmentation:{}".format(laserscan.header.stamp, laserscanSegmentation.header.stamp))
currentStamp = laserscanSegmentation.header.stamp
pointCount = len(laserscan.ranges)
cartesianCoordinates = []
rospy.logdebug("Currently occluded track labels: {}".format(occludedTracks))
# Required for velocity calculations
if self._lastDataStamp is None:
self._lastDataStamp = laserscanSegmentation.header.stamp
# Build lookup of cartesian coordinates per laser point
for pointIndex in xrange(0, pointCount):
cartesianCoordinates.append( self.calculateCartesianCoordinates(laserscan, pointIndex) )
# For each labelled segment, create and append one TrackedPerson and DetectedPerson message
trackedPersons = TrackedPersons(header=laserscanSegmentation.header)
detectedPersons = DetectedPersons(header=laserscanSegmentation.header)
for segment in laserscanSegmentation.segments:
# Calculate centroid of tracked person
centroid = numpy.array([0.0, 0.0, 0.0])
for pointIndex in segment.measurement_indices:
centroid += cartesianCoordinates[pointIndex]
centroid /= float(len(segment.measurement_indices))
# Lookup previous centroid (for velocity/twist calculation), assume zero velocity at track initialization
if not segment.label in self._previousCentroidLookup:
self._previousCentroidLookup[segment.label] = collections.deque()
# Maintain centroid history
centroidHistory = self._previousCentroidLookup[segment.label]
while len(centroidHistory) > 20:
centroidHistory.popleft()
# Calculate average velocity over past few frames
dt = 0
velocity = accumulatedVelocity = numpy.array([0.0, 0.0, 0.0])
if centroidHistory:
previousCentroid = centroid
previousStamp = currentStamp
for historyStamp, historyCentroid in reversed(centroidHistory):
accumulatedVelocity += previousCentroid - historyCentroid
dt += abs((previousStamp - historyStamp).to_sec())
previousCentroid = historyCentroid
previousStamp = historyStamp
if dt > 0:
velocity = accumulatedVelocity / dt
else:
velocity = 0
centroidHistory.append( (currentStamp, centroid) )
# Remember age of track
if not segment.label in self._firstTrackEncounterLookup:
self._firstTrackEncounterLookup[segment.label] = currentStamp
# Initialize TrackedPerson message
trackedPerson = TrackedPerson()
trackedPerson.track_id = segment.label
trackedPerson.age = currentStamp - self._firstTrackEncounterLookup[segment.label]
trackedPerson.detection_id = self._detectionIdCounter
trackedPerson.is_occluded = False
trackedPerson.is_matched = True
# Set position
LARGE_VARIANCE = 99999999
trackedPerson.pose.pose.position.x = centroid[0]
trackedPerson.pose.pose.position.y = centroid[1]
trackedPerson.pose.pose.position.z = centroid[2]
# Set orientation
if dt > 0:
yaw = math.atan2(velocity[1], velocity[0])
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
trackedPerson.pose.pose.orientation = Quaternion(x=quaternion[0], y=quaternion[1], z=quaternion[2], w=quaternion[3])
trackedPerson.pose.covariance[2 * 6 + 2] = trackedPerson.pose.covariance[3 * 6 + 3] = trackedPerson.pose.covariance[4 * 6 + 4] = LARGE_VARIANCE # z pos, roll, pitch
# Set velocity
if dt > 0:
trackedPerson.twist.twist.linear.x = velocity[0]
trackedPerson.twist.twist.linear.y = velocity[1]
trackedPerson.twist.twist.linear.z = velocity[2]
trackedPerson.twist.covariance[2 * 6 + 2] = trackedPerson.twist.covariance[3 * 6 + 3] = trackedPerson.twist.covariance[4 * 6 + 4] = trackedPerson.twist.covariance[5 * 6 + 5] = LARGE_VARIANCE # linear z, angular x, y, z
# Append to list of tracked persons
trackedPersons.tracks.append(trackedPerson)
# Initialize DetectedPerson message by copying data from TrackedPerson
detectedPerson = DetectedPerson()
detectedPerson.detection_id = trackedPerson.detection_id
detectedPerson.confidence = 1.0
detectedPerson.pose = copy.deepcopy(trackedPerson.pose)
detectedPerson.pose.pose.orientation = Quaternion()
for i in xrange(0, 2):
detectedPerson.pose.covariance[i * 6 + i] = 0.17 * 0.17
detectedPerson.pose.covariance[5 * 6 + 5] = LARGE_VARIANCE # yaw
detectedPersons.detections.append(detectedPerson)
self._detectionIdCounter += 1
for occludedTrackID in occludedTracks:
centroidHistory = self._previousCentroidLookup[occludedTrackID]
last_centroid = centroidHistory[-1][1]
# Initialize TrackedPerson message
occludedPerson = TrackedPerson()
occludedPerson.track_id = occludedTrackID
occludedPerson.age = currentStamp - self._firstTrackEncounterLookup[occludedTrackID]
occludedPerson.detection_id = 0
occludedPerson.is_occluded = True
occludedPerson.is_matched = False
# Set position
LARGE_VARIANCE = 99999999
occludedPerson.pose.pose.position.x = last_centroid[0]
occludedPerson.pose.pose.position.y = last_centroid[1]
occludedPerson.pose.pose.position.z = last_centroid[2]
occludedPerson.pose.covariance[2 * 6 + 2] = occludedPerson.pose.covariance[3 * 6 + 3] = occludedPerson.pose.covariance[4 * 6 + 4] = LARGE_VARIANCE # z pos, roll, pitch
trackedPersons.tracks.append(occludedPerson)
# Publish tracked persons
self.trackedPersonsPublisher.publish(trackedPersons)
self.detectedPersonsPublisher.publish(detectedPersons)
self._lastDataStamp = laserscanSegmentation.header.stamp
def newSegmentationReceived(self, laserscan, laserscanSegmentation, occludedTracks):
if (laserscan.header.stamp != laserscanSegmentation.header.stamp):
rospy.logwarn("Different timestamps laser: {} segmentation:{}".format(laserscan.header.stamp, laserscanSegmentation.header.stamp))
currentStamp = laserscanSegmentation.header.stamp
pointCount = len(laserscan.ranges)
cartesianCoordinates = []
rospy.logdebug("Currently occluded track labels: {}".format(occludedTracks))
# Required for velocity calculations
if self._lastDataStamp is None:
self._lastDataStamp = laserscanSegmentation.header.stamp
# Build lookup of cartesian coordinates per laser point
for pointIndex in xrange(0, pointCount):
cartesianCoordinates.append( self.calculateCartesianCoordinates(laserscan, pointIndex) )
# For each labelled segment, create and append one TrackedPerson and DetectedPerson message
trackedPersons = TrackedPersons(header=laserscanSegmentation.header)
detectedPersons = DetectedPersons(header=laserscanSegmentation.header)
for segment in laserscanSegmentation.segments:
# Calculate centroid of tracked person
centroid = numpy.array([0.0, 0.0, 0.0])
for pointIndex in segment.measurement_indices:
centroid += cartesianCoordinates[pointIndex]
centroid /= float(len(segment.measurement_indices))
# Lookup previous centroid (for velocity/twist calculation), assume zero velocity at track initialization
if not segment.label in self._previousCentroidLookup:
self._previousCentroidLookup[segment.label] = collections.deque()
# Maintain centroid history
centroidHistory = self._previousCentroidLookup[segment.label]
while len(centroidHistory) > 20:
centroidHistory.popleft()
# Calculate average velocity over past few frames
dt = 0
velocity = accumulatedVelocity = numpy.array([0.0, 0.0, 0.0])
if centroidHistory:
previousCentroid = centroid
previousStamp = currentStamp
for historyStamp, historyCentroid in reversed(centroidHistory):
accumulatedVelocity += previousCentroid - historyCentroid
dt += abs((previousStamp - historyStamp).to_sec())
previousCentroid = historyCentroid
previousStamp = historyStamp
if dt > 0:
velocity = accumulatedVelocity / dt
else:
velocity = 0
centroidHistory.append( (currentStamp, centroid) )
# Remember age of track
if not segment.label in self._firstTrackEncounterLookup:
self._firstTrackEncounterLookup[segment.label] = currentStamp
# Initialize TrackedPerson message
trackedPerson = TrackedPerson()
trackedPerson.track_id = segment.label
trackedPerson.age = currentStamp - self._firstTrackEncounterLookup[segment.label]
trackedPerson.detection_id = self._detectionIdCounter
trackedPerson.is_occluded = False
trackedPerson.is_matched = True
# Set position
LARGE_VARIANCE = 99999999
trackedPerson.pose.pose.position.x = centroid[0]
trackedPerson.pose.pose.position.y = centroid[1]
trackedPerson.pose.pose.position.z = centroid[2]
# Set orientation
if dt > 0:
yaw = math.atan2(velocity[1], velocity[0])
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
trackedPerson.pose.pose.orientation = Quaternion(x=quaternion[0], y=quaternion[1], z=quaternion[2], w=quaternion[3])
trackedPerson.pose.covariance[2 * 6 + 2] = trackedPerson.pose.covariance[3 * 6 + 3] = trackedPerson.pose.covariance[4 * 6 + 4] = LARGE_VARIANCE # z pos, roll, pitch
# Set velocity
if dt > 0:
trackedPerson.twist.twist.linear.x = velocity[0]
trackedPerson.twist.twist.linear.y = velocity[1]
trackedPerson.twist.twist.linear.z = velocity[2]
trackedPerson.twist.covariance[2 * 6 + 2] = trackedPerson.twist.covariance[3 * 6 + 3] = trackedPerson.twist.covariance[4 * 6 + 4] = trackedPerson.twist.covariance[5 * 6 + 5] = LARGE_VARIANCE # linear z, angular x, y, z
# Append to list of tracked persons
trackedPersons.tracks.append(trackedPerson)
# Initialize DetectedPerson message by copying data from TrackedPerson
detectedPerson = DetectedPerson()
detectedPerson.detection_id = trackedPerson.detection_id
detectedPerson.confidence = 1.0
detectedPerson.pose = copy.deepcopy(trackedPerson.pose)
detectedPerson.pose.pose.orientation = Quaternion()
for i in xrange(0, 2):
detectedPerson.pose.covariance[i * 6 + i] = 0.17 * 0.17
detectedPerson.pose.covariance[5 * 6 + 5] = LARGE_VARIANCE # yaw
detectedPersons.detections.append(detectedPerson)
self._detectionIdCounter += 1
for occludedTrackID in occludedTracks:
centroidHistory = self._previousCentroidLookup[occludedTrackID]
last_centroid = centroidHistory[-1][1]
# Initialize TrackedPerson message
occludedPerson = TrackedPerson()
occludedPerson.track_id = occludedTrackID
occludedPerson.age = currentStamp - self._firstTrackEncounterLookup[occludedTrackID]
occludedPerson.detection_id = 0
occludedPerson.is_occluded = True
occludedPerson.is_matched = False
# Set position
LARGE_VARIANCE = 99999999
occludedPerson.pose.pose.position.x = last_centroid[0]
occludedPerson.pose.pose.position.y = last_centroid[1]
occludedPerson.pose.pose.position.z = last_centroid[2]
occludedPerson.pose.covariance[2 * 6 + 2] = occludedPerson.pose.covariance[3 * 6 + 3] = occludedPerson.pose.covariance[4 * 6 + 4] = LARGE_VARIANCE # z pos, roll, pitch
trackedPersons.tracks.append(occludedPerson)
# Publish tracked persons
self.trackedPersonsPublisher.publish(trackedPersons)
self.detectedPersonsPublisher.publish(detectedPersons)
self._lastDataStamp = laserscanSegmentation.header.stamp
def publish_obstacles(self):
with self.lock:
if self.transport_data is None:
return
# non-leg obstacles
timestamp, xx, yy, clusters, is_legs, cogs, radii, tf_rob_in_fix = self.transport_data
# tracks
track_ids = []
tracks_latest_pos, tracks_color = [], []
tracks_in_frame, tracks_velocities = [], []
tracks_radii = []
for trackid in self.tracker.active_tracks:
track = self.tracker.active_tracks[trackid]
xy = np.array(track.pos_history[-1])
is_track_in_frame = True
if trackid in self.tracker.latest_matches:
color = (0.,1.,0.,1.) # green
elif trackid in self.tracker.new_tracks:
color = (0.,0.,1.,1.) # blue
else:
color = (0.7, 0.7, 0.7, 1.)
is_track_in_frame = False
track_ids.append(trackid)
tracks_latest_pos.append(xy)
tracks_color.append(color)
tracks_in_frame.append(is_track_in_frame)
tracks_velocities.append(track.estimate_velocity())
tracks_radii.append(track.avg_radius())
# publish trackedpersons
from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson
pub = rospy.Publisher("/tracked_persons", TrackedPersons, queue_size=1)
tp_msg = TrackedPersons()
tp_msg.header.frame_id = self.kFixedFrame
tp_msg.header.stamp = timestamp
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
# if not in_frame:
# continue
tp = TrackedPerson()
tp.track_id = trackid
tp.is_occluded = False
tp.is_matched = in_frame
tp.detection_id = trackid
tp.pose.pose.position.x = xy[0]
tp.pose.pose.position.y = xy[1]
heading_angle = np.arctan2(vel[1], vel[0]) # guess heading from velocity
from geometry_msgs.msg import Quaternion
tp.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, heading_angle))
tp.twist.twist.linear.x = vel[0]
tp.twist.twist.linear.y = vel[1]
tp.twist.twist.angular.z = 0 # unknown
tp_msg.tracks.append(tp)
pub.publish(tp_msg)
pub = rospy.Publisher('/obstacles', ObstacleArrayMsg, queue_size=1)
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
if not in_frame:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = trackid
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = xy[0]
obst.polygon.points[0].y = xy[1]
obst.polygon.points[0].z = 0
obst.radius = radius
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = vel[0]
obst.velocities.twist.linear.y = vel[1]
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/close_nonleg_obstacles', ObstacleArrayMsg, queue_size=1)
MAX_DIST_STATIC_CLUSTERS_M = 3.
cogs_in_fix = []
for i in range(len(cogs)):
cogs_in_fix.append(apply_tf(cogs[i], Pose2D(tf_rob_in_fix)))
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for c, cog_in_fix, cog_in_rob, r, is_leg in zip(clusters, cogs_in_fix, cogs, radii, is_legs):
if np.linalg.norm(cog_in_rob) < self.kRobotRadius:
continue
if len(c) < self.kMinClusterSize:
continue
# leg obstacles are already published in the tracked obstacles topic
if is_leg:
continue
# close obstacles only
if np.linalg.norm(cog_in_rob) > MAX_DIST_STATIC_CLUSTERS_M:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = 0
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = cog_in_fix[0]
obst.polygon.points[0].y = cog_in_fix[1]
obst.polygon.points[0].z = 0
obst.radius = r
yaw = 0
q = tf.transformations.quaternion_from_euler(0,0,yaw)
from geometry_msgs.msg import Quaternion
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = 0
obst.velocities.twist.linear.y = 0
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/obstacle_markers', MarkerArray, queue_size=1)
# delete all markers
ma = MarkerArray()
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "obstacles"
mk.id = 0
mk.type = 0 # ARROW
mk.action = 3 # deleteall
ma.markers.append(mk)
pub.publish(ma)
# publish tracks
ma = MarkerArray()
id_ = 0
# track endpoint
for trackid, xy, in_frame, vel in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities):
if not in_frame:
continue
normvel = np.linalg.norm(vel)
if normvel == 0:
continue
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = trackid
mk.type = 0 # ARROW
mk.action = 0
mk.scale.x = np.linalg.norm(vel)
mk.scale.y = 0.1
mk.scale.z = 0.1
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
mk.pose.position.x = xy[0]
mk.pose.position.y = xy[1]
mk.pose.position.z = 0.03
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
mk.pose.orientation = Quaternion(*q)
ma.markers.append(mk)
pub.publish(ma)