Esempio n. 1
0
    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
Esempio n. 2
0
    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)