예제 #1
0
 def __init__(self, detection, trackId):
     super(Tracks, self).__init__()
     self.KF = KalmanFilter()
     self.KF.predict()
     self.KF.correct(np.matrix(detection).reshape(2, 1))
     self.trace = deque(maxlen=20)
     self.prediction = detection.reshape(1, 2)
     self.trackId = trackId
     self.skipped_frames = 0
예제 #2
0
 def updateState(self, measurements, probabilities, p0):
     x_p = self.x_p
     p_p = self.p_p
     if measurements.size > 0:
         y_u, y_i = self.__getInnovation__(x_p, measurements, probabilities)
         s_u = KalmanFilter.__getInnovationCovariance__(self, p_p)
         k_u = KalmanFilter.__getKalmanGain__(self, s_u, p_p)
         x_u = KalmanFilter.__getUpdatedState__(self, x_p, k_u, y_u)
         p_u = self.__getStateCovariance__(k_u, p_p, s_u, y_u, y_i,
                                           probabilities, p0)
     else:
         self.x_u = x_p
         self.p_u = p_p
     self.measurement = measurements
     self.probabilities = probabilities
예제 #3
0
 def __init__(self,
              A=None,
              H=None,
              Q=None,
              R=None,
              G=None,
              X_0=None,
              threshold=100):
     KalmanFilter.__init__(self, A, H, Q, R, G, X_0)
     self.threshold = threshold
     self.sphereVolume = self.__getSphereVolume__(H.shape[0])
     self.__initializeInnovationCovariance__()
     self.age = 1
     self.totalVisibleCount = 0
     self.consecutiveInvisibleCount = 0
     self.detectionProbability = 0.90
예제 #4
0
 def __getInnovation__(self, prediction, measurements, probabilities):
     y_u = 0
     y_i = []
     for nm, measurement in enumerate(measurements):
         y_u_i = np.array(
             KalmanFilter.__getInnovation__(self, prediction, measurement))
         y_u += probabilities[nm] * y_u_i
         y_i.append(y_u_i)
     return np.matrix(y_u).T, np.matrix(y_i).T
예제 #5
0
 def __init__(self,
              respond = None,
              regressors = None,
              intercept = False,
              Sigma = None,
              sigma = None,
              initBeta = None,
              initVariance = None,
              Phi = None,
              **args):
     """
     :param respond: Dependent time series
     :type respond: TimeSeriesFrame<double>
     :param regressors: Independent time serieses
     :type regressors: TimeSeriesFrame<double>
     :param intercept: include/exclude intercept in the regression
     :type intercept: boolean
     """
     KalmanFilter.__init__(self, respond, regressors, intercept, Sigma, sigma, initBeta, initVariance, Phi, **args)
예제 #6
0
 def __init__(self,
              respond=None,
              regressors=None,
              intercept=False,
              Sigma=None,
              sigma=None,
              initBeta=None,
              initVariance=None,
              Phi=None,
              **args):
     """
     :param respond: Dependent time series
     :type respond: TimeSeriesFrame<double>
     :param regressors: Independent time serieses
     :type regressors: TimeSeriesFrame<double>
     :param intercept: include/exclude intercept in the regression
     :type intercept: boolean
     """
     KalmanFilter.__init__(self, respond, regressors, intercept, Sigma,
                           sigma, initBeta, initVariance, Phi, **args)
예제 #7
0
 def __init__(self, prediction, trackIdCount):
     """Initialize variables used by Track class
     Args:
         prediction: predicted centroids of object to be tracked
         trackIdCount: identification of each track object
     Return:
         None
     """
     self.track_id = trackIdCount  # identification of each track object
     self.KF = KalmanFilter()  # KF instance to track this object
     self.prediction = np.asarray(prediction)  # predicted centroids (x,y)
     self.skipped_frames = 0  # number of frames skipped undetected
     self.trace = []  # trace path
예제 #8
0
class Tracks(object):
    """docstring for Tracks"""
    def __init__(self, detection, trackId):
        super(Tracks, self).__init__()
        self.KF = KalmanFilter()
        self.KF.predict()
        self.KF.correct(np.matrix(detection).reshape(2, 1))
        self.trace = deque(maxlen=20)
        self.prediction = detection.reshape(1, 2)
        self.trackId = trackId
        self.skipped_frames = 0

    def predict(self, detection):
        self.prediction = np.array(self.KF.predict()).reshape(1, 2)
        self.KF.correct(np.matrix(detection).reshape(2, 1))
class TrackingObject(object):
	"""
	TrackingObject: maintains and updates all the information 
	for one tracked object
	"""
	def __init__(self, detection, trackId):
		super(TrackingObject, self).__init__()
		self.KF = KalmanFilter()
		self.KF.predict()
		self.KF.correct(np.matrix(detection).reshape(2,1))
		self.trace = deque(maxlen=20)
		self.prediction = detection.reshape(1,2)
		self.trackId = trackId
		self.skipped_frames = 0

	def predict_and_correct(self, measurement):
		self.prediction = np.array(self.KF.predict()).reshape(1,2)
		self.KF.correct(np.matrix(measurement).reshape(2,1))
예제 #10
0
                x = est_rbt_x[:,
                              est_k]  # last state estimate vector from robot frame
                P = est_rbt_P[:, :, est_k]  # last covariance matrix
                Q = est_rbt_Q  # process noise covariance matrix
                R = est_rbt_R  # measurement noise covariance
                H = est_rbt_H  # observation matrix
                z = est_rbt_m[:, est_k]  # measurement vector
                u = matrix(
                    [[0], [0], [0]]
                )  # control input vector (don't give kalman filter knowledge about thruster inputs)

                A = est_rbt_A
                B = est_rbt_B

                state = KalmanFilter(x, P, z, u, A, B, Q, R, H)
                x, P = state.predict(x, P, u)
                x, K, P = state.update(x, P, z)

                # print('x', x)
                # # print('P', P)
                # # print('Q', Q)
                # # print('R', R)
                # # print('H', H)
                # # print('z', z)
                # # print('u', u)
                # # print('K', K)

                est_rbt_x[:, est_k + 1] = x
                est_rbt_L[:, :, est_k + 1] = K
                est_rbt_P[:, :, est_k + 1] = P
예제 #11
0
def main():

    # Pretty videos:
    fourcc = cv2.VideoWriter_fourcc(*'mp4v')
    kyleOutputVideo = cv2.VideoWriter('../UntrackedFiles/BallOutputKyle.mp4',
                                      fourcc, 60.0, (1920, 1080))
    meganOutputVideo = cv2.VideoWriter('../UntrackedFiles/BallOutputMegan.mp4',
                                       fourcc, 60.0, (1920, 1080))
    meganFilename = '../UntrackedFiles/stereoClip5_Megan.mov'
    kyleFilename = '../UntrackedFiles/stereoClip5_Kyle.mov'

    vrMegan1 = VideoReader(meganFilename)
    vrMegan2 = VideoReader(meganFilename)
    vrKyle1 = VideoReader(kyleFilename)
    vrKyle2 = VideoReader(kyleFilename)
    numFrameForward = 5
    vrMegan2.setNextFrame(numFrameForward)
    vrKyle2.setNextFrame(numFrameForward)

    # find court corners:
    cfKyle = CourtFinder()
    cfMegan = CourtFinder()
    numFrames = int(vrMegan1.getNumFrames())
    vrMegan1.setNextFrame(int(numFrames / 2))
    ret, frame = vrMegan1.readFrame()
    cfMegan.FindCourtCorners(frame, 0)
    vrKyle1.setNextFrame(int(numFrames / 2))
    ret, frame = vrKyle1.readFrame()
    cfKyle.FindCourtCorners(frame, 0)
    if (not cfMegan.found_corners) or not (cfKyle.found_corners):
        print "Couldn't find the court. Exiting."
        return

    # reset frame index to beginning
    vrMegan1.setNextFrame(0)
    vrKyle1.setNextFrame(0)
    frameNum = 1

    meganCam = Camera("megan", cfMegan.corners_sort)
    kyleCam = Camera("kyle", cfKyle.corners_sort)

    # make a ball finder
    bf = BallFinder()
    kf = KalmanFilter(vrMegan1.framerate)

    csvData = []
    while (True):
        ret1, kyleFrame1 = vrKyle1.readFrame()
        ret2, kyleFrame2, = vrKyle2.readFrame()
        ret3, meganFrame1 = vrMegan1.readFrame()
        ret4, meganFrame2, = vrMegan2.readFrame()
        if not (ret1) or not (ret2) or not (ret3) or not (ret4):
            print 'Ending after', frameNum - 1, 'frames.'
            break

        kyleMask, kyleRays = getBallCandidateRays(bf, kyleCam, kyleFrame1,
                                                  kyleFrame2)
        meganMask, meganRays = getBallCandidateRays(bf, meganCam, meganFrame1,
                                                    meganFrame2)

        # check all candidate rays for candidate balls
        minDist = 1000000
        # TODO set inf
        ballPt = []
        # all ball points and distances
        threshDist = 0.2
        # rays must be within .1 m of each other for intersect
        ballCandidates = []
        candidateCertainty = []
        for kyleRay in kyleRays:
            for meganRay in meganRays:
                pt, dist, _D, _E = IntersectRays(kyleRay, meganRay)
                if dist < threshDist and pt[1] < 3.5:
                    # don't include candidates clearly not valid intersect points
                    # also don't include candidates that are clearly too high to be the ball
                    courtBuffer = 2
                    if pt[0] < Camera.HALF_COURT_X + courtBuffer and pt[
                            0] > -Camera.HALF_COURT_X - courtBuffer:
                        if pt[2] < Camera.HALF_COURT_Z + 0.6:  # and pt[2] > -Camera.HALF_COURT_Z - 0:
                            ballCandidates.append(pt)
                            candidateCertainty.append(dist)
                    if dist < minDist:
                        minDist = dist
                        ballPt = pt
        kf.processMeas(ballCandidates, candidateCertainty)

        # ========== CSV and VIDEO output ===========
        csvTuple = list()
        csvTuple.append(frameNum)
        if np.linalg.norm(kf.sigma_k, 'fro') < 100:  # valid result
            # Format the tuple for successful reading
            csvTuple.append(1)
            posVel = np.reshape(kf.mu_k, (1, 6))[0]
            posVel = np.round(posVel, 3)
            for val in posVel:
                csvTuple.append(val)
            for val in kyleCam.ConvertWorldToImagePosition(posVel[0:3]):
                csvTuple.append(val)
            for val in meganCam.ConvertWorldToImagePosition(posVel[0:3]):
                csvTuple.append(val)
            # Videos
            kyleOutFrame = kyleFrame1.copy()
            bf.ballPixelLoc = kyleCam.ConvertWorldToImagePosition(posVel[0:3])
            kyleOutFrame = cfKyle.drawCornersOnFrame(kyleOutFrame)
            kyleOutFrame = bf.drawBallOnFrame(kyleOutFrame)
            kyleOutFrame |= kyleMask
            kyleOutputVideo.write(kyleOutFrame)
            meganOutFrame = meganFrame1.copy()
            meganOutFrame |= meganMask
            bf.ballPixelLoc = meganCam.ConvertWorldToImagePosition(posVel[0:3])
            meganOutFrame = cfMegan.drawCornersOnFrame(meganOutFrame)
            meganOutFrame = bf.drawBallOnFrame(meganOutFrame)
            meganOutputVideo.write(meganOutFrame)
        else:
            # Format the tuple for unsuccessful reading
            csvTuple.append(0)
        csvData.append(list(csvTuple))
        print csvTuple
        frameNum += 1
        # <END WHILE LOOP>

    with open('../UntrackedFiles/ballEst.csv', 'wb') as csvfile:
        filewriter = csv.writer(csvfile, delimiter=',')
        filewriter.writerows(csvData)

    vrKyle1.close()
    vrKyle2.close()
    vrMegan1.close()
    vrMegan2.close()
    kyleOutputVideo.release()
    meganOutputVideo.release()
예제 #12
0
    obs = 0.01
    (x, y) = dataGen.generateSamplePointsGG(samplingTime, numSamples, dt, x0, procs, obs)
    print "samplePoints generated"
    finalTime = samplingTime * (numSamples - 1)

    pf = ParticleFilter(oscillator, x0.size)
    pf.runFilter(y, samplingTime, dtf, procs, obs)
    pltWorker.plotPath(x, finalTime, 0.5)
    pltWorker.plotMarkers(y, finalTime, mk="*", ms=10, a=0.4)
    pltWorker.plotPFMarkers(pf.x, pf.w, finalTime)
    pltWorker.save("dists.pdf")
    pltWorker.clear()
    z = pf.getAveragePath()
    pltWorker.plotPath(x, finalTime, 0.5)
    pltWorker.plotMarkers(y, finalTime)
    pltWorker.plotMarkers(z, finalTime, mk="x", ms=8)
    pltWorker.save("sir.pdf")
    pltWorker.clear()
    ac = dataGen.calculateAccuracy(x, z)
    print "Particle filter error:", ac

    kf = KalmanFilter(oscillator, x0.size)
    kf.runFilter(y, samplingTime, dtf, procs, obs)
    k = kf.getAveragePath()
    pltWorker.plotPath(x, finalTime, 0.5)
    pltWorker.plotMarkers(y, finalTime)
    pltWorker.plotMarkers(k, finalTime, mk="x", ms=8)
    pltWorker.save("kalman.pdf")
    ac = dataGen.calculateAccuracy(x, k)
    print "Kalman filter error:", ac
예제 #13
0
    obs = 0.01
    (x, y) = dataGen.generateSamplePointsGG(samplingTime, numSamples, dt, x0,
                                            procs, obs)
    print 'samplePoints generated'
    finalTime = samplingTime * (numSamples - 1)

    pf = ParticleFilter(oscillator, x0.size)
    pf.runFilter(y, samplingTime, dtf, procs, obs)
    pltWorker.plotPath(x, finalTime, 0.5)
    pltWorker.plotMarkers(y, finalTime, mk='*', ms=10, a=0.4)
    pltWorker.plotPFMarkers(pf.x, pf.w, finalTime)
    pltWorker.save("dists.pdf")
    pltWorker.clear()
    z = pf.getAveragePath()
    pltWorker.plotPath(x, finalTime, 0.5)
    pltWorker.plotMarkers(y, finalTime)
    pltWorker.plotMarkers(z, finalTime, mk='x', ms=8)
    pltWorker.save('sir.pdf')
    pltWorker.clear()
    ac = dataGen.calculateAccuracy(x, z)
    print "Particle filter error:", ac

    kf = KalmanFilter(oscillator, x0.size)
    kf.runFilter(y, samplingTime, dtf, procs, obs)
    k = kf.getAveragePath()
    pltWorker.plotPath(x, finalTime, 0.5)
    pltWorker.plotMarkers(y, finalTime)
    pltWorker.plotMarkers(k, finalTime, mk='x', ms=8)
    pltWorker.save('kalman.pdf')
    ac = dataGen.calculateAccuracy(x, k)
    print "Kalman filter error:", ac
예제 #14
0
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])

Q = np.diag([0, acceleration_variance, 0, acceleration_variance])
H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])

R = np.array([[meas_variance, 0], [0, meas_variance]])

# ---------------------------------------------------------------------------
# ---------------------------------------------------------------------------

# Initializing Kalman Filter
kf = KalmanFilter(initial_state=x,
                  initial_covariance=P,
                  process_model=F,
                  process_noise=Q,
                  measurement_function=H,
                  measurement_covariance=R,
                  no_of_state_variables=no_of_state_variables,
                  no_of_state_measurements=no_of_state_measurements)

step = 0  # step counter, used for simulation of abscent measurements only

# Loop that runs throughout the time array defined previously
for t in time:

    #Simulate real speed
    real_speed_x = initial_speed_x + np.random.randn() * np.sqrt(
        acceleration_variance)
    real_speed_y = initial_speed_y + np.random.randn() * np.sqrt(
        acceleration_variance)