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(prediction) # 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.counted = False
self.trace = [] # trace path
self.ground_truth_box = self.prediction
self.cnt_obj = {
'person': 0,
'motorbike': 0,
'car': 0,
'truck': 0,
'bicycle': 0,
'bus': 0
}
self.label = ''
self.box = []
self.conf_score = []
self.has_truebox = False
def __init__(self, opt, frame_rate=30):
self.opt = opt
if opt.gpus[0] >= 0:
opt.device = torch.device('cuda')
else:
opt.device = torch.device('cpu')
print('Creating model...')
self.model = create_model(opt.arch, opt.heads, opt.head_conv)
self.model = load_model(self.model, opt.load_model)
self.model = self.model.to(opt.device)
self.model.eval()
self.tracked_stracks = [] # type: list[STrack]
self.lost_stracks = [] # type: list[STrack]
self.removed_stracks = [] # type: list[STrack]
self.frame_id = 0
self.det_thresh = opt.conf_thres
self.buffer_size = int(frame_rate / 30.0 * opt.track_buffer)
self.max_time_lost = self.buffer_size
self.max_per_image = 128
self.mean = np.array(opt.mean, dtype=np.float32).reshape(1, 1, 3)
self.std = np.array(opt.std, dtype=np.float32).reshape(1, 1, 3)
self.kalman_filter = KalmanFilter()
def __init__(self, frame_rate=30, lsh_mode=0):
'''
Parameters
----------
frame_rate: int
Framerate of video tracked
lsh_mode : int (can have 0, 1, 2 as values)
0: LSH disabled
1: LSH Low Impact
2: LSH Higher Impact
'''
# Create LSH Table
self.lsh_mode = lsh_mode
if lsh_mode > 0:
self.lsh = createlshash()
else:
self.lsh = None
self.tracked_tracks = [] # type: list[Track]
self.lost_tracks = [] # type: list[Track]
self.removed_tracks = [] # type: list[Track]
self.frame_id = 0 # Initial Frame ID
self.det_thresh = 0.0 # Minimum confidence of detection to start new track
track_buffer = 30 # Prev detections to store
self.min_box_area = 200 # Minimum Box Area to be considered a valid detection
# Number of frames to be remembered
self.buffer_size = int(frame_rate / 30.0 * track_buffer)
# Maximum frames to wait for a track to be considered lost
self.max_time_lost = self.buffer_size
self.kalman_filter = KalmanFilter() # Kalman Filter
def __init__(self, timeStep):
self.dt = timeStep
self.localObserver = KalmanFilter(3, 3)
self.localObserver.SetMeasurement(0, 0, 1.0)
self.localObserver.SetMeasurement(1, 1, 1.0)
self.localObserver.SetMeasurement(2, 2, 1.0)
self.localObserver.SetStatePredictionFactor(0, 1, self.dt)
self.localObserver.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2)
self.localObserver.SetStatePredictionFactor(1, 2, self.dt)
self.remoteObserver = KalmanFilter(3, 3)
self.remoteObserver.SetMeasurement(0, 0, 1.0)
self.remoteObserver.SetMeasurement(1, 1, 1.0)
self.remoteObserver.SetMeasurement(2, 2, 1.0)
self.remoteObserver.SetPredictionNoise(0, 2.0)
self.remoteObserver.SetPredictionNoise(1, 2.0)
self.remoteObserver.SetPredictionNoise(2, 2.0)
def sensor_callback(self, data):
"""Process received positions data and return Kalman estimation.
:type data: Odometry
"""
if self.initial_run:
# Initialize Kalman filter and estimation.
initial_pos = data.pose.pose
self.filter = KalmanFilter(1.0 / self.sub_frequency, initial_pos)
self.X_est = Odometry()
self.X_est.pose.pose = initial_pos
self.initial_run = False
else:
X_measured = data.pose.pose
time = data.header.stamp
# If measurement data is not available, use only prediction step.
# Else, use prediction and update step.
if X_measured is None:
self.X_est = self.filter.predict()
else:
self.X_est = self.filter.predict_update(X_measured)
self.X_est.header.stamp = time
if self.debug_enabled:
self.debug_pub.publish(self.X_est)
def __init__(self):
self.kalman_filter = KalmanFilter()
self.is_initialized = False
self.previous_timestamp = 0
self.noise_ax = 1
self.noise_ay = 1
self.noise_az = 1
self.noise_vector = np.diag(
[self.noise_ax, self.noise_ay, self.noise_az])
self.kalman_filter.P = np.array([[1, 0, 0, 0, 0,
0], [0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0,
0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
self.kalman_filter.x = np.zeros((6, 1))
# self.uwb_std = 23.5 / 1000
self.uwb_std = 23.5 / 10
self.R_UWB = np.array([[self.uwb_std**2]])
self.kalman_filter.R = self.R_UWB
def _initialise_gui_widgets(self):
"""
Create all the widgets for the window.
"""
# # Main widget
self.main_widget = QtGui.QWidget()
# # Address label
self.address_label = QtGui.QLabel("Server:")
self.address_label.setAlignment(QtCore.Qt.AlignRight
| QtCore.Qt.AlignVCenter)
# # Address bar
self.address_bar = QtGui.QLineEdit()
# Create a regex validator for the IP address
ip_values = "(?:[0-1]?[0-9]?[0-9]|2[0-4][0-9]|25[0-5])"
ip_regex = QtCore.QRegExp("^" + ip_values + "\\." + ip_values + "\\." +
ip_values + "\\." + ip_values + "$")
ip_validator = QtGui.QRegExpValidator(ip_regex, self)
self.address_bar.setValidator(ip_validator)
self.address_bar.setText("192.168.42.1")
# # Port bar
self.port_bar = QtGui.QLineEdit()
self.port_bar.setValidator(QtGui.QIntValidator())
self.port_bar.setText("12345")
# # Connect button
self.connect_button = QtGui.QPushButton("&Connect")
self.connect_button.clicked.connect(self._toggle_connect)
# # Plot widgets
self.plot_widgets = {
'accelerometer': plt.UpdatingPlotWidget(title='Accelerometer'),
'magnetometer': plt.UpdatingPlotWidget(title='Magnetometer'),
'gyroscope': plt.UpdatingPlotWidget(title='Gyroscope'),
'barometer': plt.UpdatingPlotWidget(title='Barometer'),
'gps-pos': plt.UpdatingPlotWidget(title='GPS Position')
}
for name, widget in self.plot_widgets.items():
if name == 'barometer':
widget.add_item('altitude', pen='k')
widget.add_item('filtered', pen='r')
widget.add_item('altitude_gps', pen='b')
self.filter1 = KalmanFilter(x_prior=0,
P_prior=2,
A=1,
B=0,
H=1,
Q=0.005,
R=1.02958)
elif name == 'gps-pos':
widget.add_item('position', symbol='o')
else:
widget.add_item('x', pen='r')
widget.add_item('y', pen='g')
widget.add_item('z', pen='b')
def __init__(self, prediction, trackIdCount):
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
def __init__(self, prediction, id):
self.id = id # identification of each track object
self.KF = KalmanFilter() # KF instance to track this object
self.prediction = np.asarray(prediction) # predicted centroids (x,y)
self.lostDetectionTime = 0 # number of frames skipped undetected
self.detectionTime = 0
self.trace = [] # trace path
def __init__(self, initial_state):
"""
Initialises a tracked object according to initial bounding box.
:param initial_state: (array) single detection in form of bounding box [X_min, Y_min, X_max, Y_max]
"""
self.kf = KalmanFilter(dim_x=7, dim_z=4)
# Transition matrix
self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0],
[0, 1, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0, 1],
[0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]])
# Transformation matrix (Observation to State)
self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0]])
self.kf.R[2:, 2:] *= 10. # observation error covariance
self.kf.P[4:, 4:] *= 1000. # initial velocity error covariance
self.kf.P *= 10. # initial location error covariance
self.kf.Q[-1, -1] *= 0.01 # process noise
self.kf.Q[4:, 4:] *= 0.01 # process noise
self.kf.x[:4] = xxyy_to_xysr(initial_state) # initialize KalmanFilter state
def define_kalman_filter(self):
self.kf = KalmanFilter(F=self.F,
H=self.H,
Q=self.Q,
R=self.R,
P=self.P,
x0=self.initial_x0)
def __init__(self):
# Number of states
self.n = 12
# System state
self.X = np.matrix(np.zeros((self.n, 1)))
# Initial State Transition Matrix
self.F = np.asmatrix(np.eye(self.n))
self.F[3:6, 3:6] = 0.975 * np.matrix(np.eye(3))
self.F[9:12, 9:12] = 0.975 * np.matrix(np.eye(3))
# Initial Process Matrix
self.P = np.asmatrix(1.0e3 * np.eye(self.n))
# Process Noise Level
self.N = 1.0e-3
# Initialize H and R matrices for optitrack pose
self.Hopti = np.matrix(np.zeros((6, self.n)))
self.Hopti[0:3, 0:3] = np.matrix(np.eye(3))
self.Hopti[3:6, 6:9] = np.matrix(np.eye(3))
self.Ropti = np.matrix(np.zeros((6, 6)))
self.Ropti[0:3, 0:3] = 1.0 * 1.0e-3 * np.matrix(np.eye(3))
self.Ropti[3:6, 3:6] = 1.0 * 1.0e-2 * np.matrix(np.eye(3))
# Initialize Kalman Filter
self.kalmanF = KalmanFilter()
self.isInit = False
self.lastTime = None
def test(x, y, z, qcov, rcov):
# Initializations
dt = 1 # Measurement interval (s)
I = lambda x: identity(x)
A = zeros((9, 9)) # Transition matrix
A[0:3, 0:3] = I(3)
A[0:3, 3:6] = I(3) * dt
A[0:3, 6:9] = I(3) * 0.5 * dt * dt
A[3:6, 3:6] = I(3)
A[3:6, 6:9] = I(3) * dt
A[6:9, 6:9] = I(3)
H = zeros((3, 9)) # Measurement matrix
H[0:3, 0:3] = I(3)
Q = identity(9) * qcov # Transition covariance
R = identity(3) * rcov # Noise covariance
B = identity(9) # Control matrix
kf = KalmanFilter(A, H, Q, R, B)
# Run through the dataset
n = len(x)
xkf, ykf, zkf = zeros(n), zeros(n), zeros(n)
for i in xrange(n):
kf.correct(array([x[i], y[i], z[i]]))
kf.predict(array([]))
Skf = kf.getCurrentState()
xkf[i], ykf[i], zkf[i] = Skf[0], Skf[1], Skf[2]
return xkf, ykf, zkf
def __init__( self, impedance, timeStep ): self.waveController = WaveController( impedance[ 0 ] + impedance[ 1 ] + impedance[ 2 ], timeStep ) self.dt = timeStep self.waveObserver = KalmanFilter( 2, 2 ) self.waveObserver.SetMeasurement( 0, 0, 4.0 ) self.waveObserver.SetMeasurement( 1, 1, 4.0 ) self.waveObserver.SetStatePredictionFactor( 0, 1, self.dt )
def __init__(self, N):
self.N = N
self.state_estimater = KalmanFilter(6)
self.sub = rospy.Subscriber("/kinect_head/rgb/ObjectDetection",
ObjectDetection,
self.cb_object_detection,
queue_size=10)
self.pub = rospy.Publisher('fridge_pose', PoseStamped, queue_size=10)
def __init__(self, ):
rp.init_node("filter")
# Get rate of state publisher
self.rate = rp.get_param("rate", 100)
# Get VRPN client topic
self.vrpn_topic = rp.get_param("vrpn_topic")
# Number of states
self.n = 12
# System state
self.X = np.matrix(np.zeros((self.n, 1)))
# Initial State Transition Matrix
self.F = np.asmatrix(np.eye(self.n))
# Initial Process Matrix
self.P = np.asmatrix(1.0e3 * np.eye(self.n))
# Process Noise Level
self.N = 1.0e-3
# Initialize H and R matrices for optitrack pose
self.Hopti = np.matrix(np.zeros((6, self.n)))
self.Hopti[0:3, 0:3] = np.matrix(np.eye(3))
self.Hopti[3:6, 6:9] = np.matrix(np.eye(3))
self.Ropti = np.matrix(np.zeros((6, 6)))
self.Ropti[0:3, 0:3] = 1.0e-3 * np.matrix(np.eye(3))
self.Ropti[3:6, 3:6] = 1.0e-5 * np.matrix(np.eye(3))
# Initialize Kalman Filter
self.kalmanF = KalmanFilter()
self.isInit = False
self.lastTime = None
# Set up Subscriber
self.vrpn_sub = rp.Subscriber(self.vrpn_topic,
PoseStamped,
self.state_callback,
queue_size=1)
# Set up Publisher
self.state_pub = rp.Publisher("opti_state/pose",
PoseStamped,
queue_size=1)
self.state_rate_pub = rp.Publisher("opti_state/rates",
TwistStamped,
queue_size=1)
# Create thread for publisher
t = threading.Thread(target=self.statePublisher)
t.start()
rp.spin()
def __init__(self, detection, trackId):
super(Tracks, self).__init__()
self.KF = KalmanFilter()
self.KF.predict_state_vector()
self.KF.correct_state_vector(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 setUp(self):
A = np.eye(2)
H = np.eye(2)
Q = np.eye(2)
R = np.eye(2)
x_0 = np.array([1, 1])
P_0 = np.eye(2)
self.kalman_filter = KalmanFilter(A, H, Q, R, x_0, P_0)
def __init__(self, timeStep):
self.dt = timeStep
self.remoteObserver = KalmanFilter(3, 3)
self.remoteObserver.SetMeasurement(0, 0, 1.0)
self.remoteObserver.SetMeasurement(1, 1, 1.0)
self.remoteObserver.SetMeasurement(2, 2, 1.0)
self.remoteObserver.SetPredictionNoise(0, 2.0)
self.remoteObserver.SetPredictionNoise(1, 2.0)
self.remoteObserver.SetPredictionNoise(2, 2.0)
def kalman_angle():
global kAngle_g
T = 0.01
lamC = 1.0 #variance for compass
lamGPS = 1.0 #variance for gps bearing
lamGyro = 1.0 #variance for gyroscope
sig = 1.0 #variance for model
sig2 = sig**2
lamC2 = lamC**2
lamGPS2 = lamGPS**2
lamGyro2 = lamGyro**2
(A, H, Q, R) = create_model_parameters(T, sig2, lamC2) #initialize evolution, measurement, model error, and measurement error
# initial state
x = np.array([0, 0.1]) #starting velocity and position
P = 0 * np.eye(2) #starting probability 100%
kalman_filter = KalmanFilter(A, H, Q, R, x, P) #create the weights filter
lastTime = time.time()
startTime = lastTime
lastOmega = 0
lastAngle = 0
thetaCoord_l = 0 #local variable to prevent race conditions
lastBearing = 0
gpsTrust = 1.0
while True:
while (omega_g == lastOmega or thetaCoord_g == lastAngle): #check if there were updates to the compass
time.sleep(0.01) #if no updates then wait a bit
thetaCoord_l = thetaCoord_g
lastAngle = thetaCoord_l
if (kalman_filter._x[0] - thetaCoord_l) > 180: #make sure the filter knows that we crossed the pole
kalman_filter._x[0] = kalman_filter._x[0] - 360
elif (thetaCoord_l - kalman_filter._x[0]) > 180:
kalman_filter._x[0] = kalman_filter._x[0] + 360
kalman_filter.predict() #evolve the state and the error
kalman_filter.update((thetaCoord_l,omega_g),(lamC2,lamGyro2)) #load in 2 measurement values
(x, P) = kalman_filter.get_state() #return state
dt = time.time() - lastTime #calculate dt
lastTime = time.time()
kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps
kAngle_g = x[0]%360 #because the model portion is estimating based on that.
# while (omega_g == lastOmega or gpsTheta_g == lastBearing): #check if there were updates to the gps bearing
# time.sleep(0.01) #if no updates then wait a bit
gpsTheta_l = gpsTheta_g
lastBearing = gpsTheta_l
if (kalman_filter._x[0] - gpsTheta_l) > 180: #make sure the filter knows that we crossed the pole
kalman_filter._x[0] = kalman_filter._x[0] - 360
elif (gpsTheta_l - kalman_filter._x[0]) > 180:
kalman_filter._x[0] = kalman_filter._x[0] + 360
lamGPS2 = gpsTrust/gpsDistance_g
kalman_filter.predict() #evolve the state and the error
kalman_filter.update((gpsTheta_l,omega_g),(lamGPS2,lamGyro2)) #load in 2 measurement values
(x, P) = kalman_filter.get_state() #return state
dt = time.time() - lastTime #calculate dt
lastTime = time.time()
kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps
kAngle_g = x[0]%360 #because the model portion is estimating based on that.
def __init__(self, prediction, trackIdCount):
self.track_id = trackIdCount
self.KF = KalmanFilter()
self.prediction = np.asarray(prediction)
self.skipped_frames = 0
self.trace = []
self.start_time = datetime.utcnow()
self.track = []
self.passed = False
self.value = False
def __init__(self, prediction, trackIdCount, parallel):
self.track_id = trackIdCount # номер кожного ідентифікованого об'єкта
if not parallel:
self.KF = KalmanFilter(
) # Фільтра Калмана для відстеження даного об'єкта
else:
self.KF = KalmanFilterParallel()
self.prediction = np.asarray(
prediction) # прогнозовані центроїди (x,y)
self.skipped_frames = 0 # кількість пропущених кадрів
def __init__(self, metric, max_iou_distance=0.7, max_age=70, n_init=3):
self.metric = metric
self.max_iou_distance = max_iou_distance
self.max_age = max_age
self.n_init = n_init
self.kf = KalmanFilter()
self.tracks = []
self._next_id = 1
self.last_confirm_id = 1
def filter_data(data, x0, P, Q, R):
filter1 = KalmanFilter(x0, P, 1, 0, 1, Q, R)
x_out = np.zeros(data.size)
P_out = np.zeros(data.size)
for k in np.arange(1, data.size):
x_out[k], P_out[k] = filter1.update(0, data[k])
return x_out, P_out
def calculateKalman(measurements):
kf = KalmanFilter(dt=1, zdims=2, r=1000, q=0.0000001, xvals=3)
results = []
print "Calculating..."
for measurement in measurements:
measArray = np.asarray([[measurement[0]], [measurement[1]]])
x = kf.run(measArray)
results.append((x[:2]).tolist())
return results
def __init__(self, inertia, damping, stiffness, timeStep):
self.dt = timeStep
self.observer = KalmanFilter(3, 3, 1)
self.observer.SetMeasurement(0, 0, 1.0)
self.observer.SetMeasurement(1, 1, 1.0)
self.observer.SetMeasurement(2, 2, 1.0)
self.observer.SetStatePredictionFactor(0, 1, self.dt)
self.observer.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2)
self.observer.SetStatePredictionFactor(1, 2, self.dt)
self.observer.SetStatePredictionFactor(2, 2, 0.0)
self.SetSystem(inertia, damping, stiffness)
def __init__(self, prediction, trackIdCount, first_detected_frame,
start_position):
self.track_id = trackIdCount
self.KF = KalmanFilter()
self.prediction = np.asarray(prediction)
self.skipped_frames = 0
self.trace = []
self.start_time = datetime.utcnow()
self.start_position = start_position
self.passed = False
self.first_detected_frame = first_detected_frame
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
"""
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
def __init__(self, bb, id_, max_miss_):
self.track = [bb]
self.alive = True
self.kf = KalmanFilter()
self.id_track = id_
self.mean, self.cov = self.kf.initiate(bb.asp_ratio())
self.num_miss = 0
self.num_max_miss = max_miss_
self.project_mean = 0
self.color = (int(random.random() * 256), int(random.random() * 256),
int(random.random() * 256))
def compute_track(video):
track = []
proposals = []
object_detected = False
kalman_filter = None
for t, im in enumerate(video):
scores, bboxes = im_detect(sess, net, im)
bboxes, scores = filter_proposals(scores,
bboxes,
nms_thresh=NMS_THRESH,
conf_thresh=CONF_THRESH)
affinities = np.zeros(scores.shape)
if not object_detected:
if bboxes.shape[0] == 0:
track.append(np.zeros(6))
proposals.append(np.zeros((1, 6)))
continue
else:
object_detected = True
i = np.argmax(scores)
xhat_0 = bboxes[i]
kalman_filter = KalmanFilter(xhat_0)
track.append(
np.concatenate((bboxes[i], scores[i], affinities[i])))
proposals.append(
np.concatenate([bboxes, scores, affinities], axis=1))
continue
xhat, P = kalman_filter.update_time()
score, affinity = np.zeros(1), np.zeros(1)
if bboxes.shape[0] > 0:
track_im = crop_and_resize(im, track[-1][:4])
bbox_ims = [crop_and_resize(im, bbox) for bbox in bboxes]
affinities = [
get_affinity(track_im, bbox_im) for bbox_im in bbox_ims
]
affinities = np.reshape(affinities, scores.shape)
i, aff = np.argmax(affinities), np.max(affinities)
if aff > AFF_THRESH:
xhat, P = kalman_filter.update_measurement(bboxes[i][:4])
score, affinity = scores[i], affinities[i]
track.append(np.concatenate((xhat, score, affinity)))
proposals.append(np.concatenate((bboxes, scores, affinities), axis=1))
return track, proposals
