def markers_callback(msg):
global markers, savedDetections, estimated_poses, dict_ekf
markers = msg.markers
# Dictionary for Kalman filter (we will have 3 aruco markers for the basic case)
estimated_dict = {}
for i in markers:
if str(i.id) not in savedDetections.keys():
savedDetections[str(i.id)] = [(msg.header.stamp.secs, i.pose.pose)]
estimated_poses[str(i.id)] = PoseStamped()
#Making sure that a Kalman filter is initialized for each marker if we don't have the Aruco Marker registered in the dictionary.
dict_ekf[str(i.id)] = cv2.KalmanFilter(
7, 7) # Initialization of kalman filter.
trans_mat = np.identity(7, np.float32)
dict_ekf[str(
i.id
)].transitionMatrix = trans_mat #x'=Ax+BU transition matrix is A
dict_ekf[str(i.id)].measurementMatrix = trans_mat
dict_ekf[str(i.id)].processNoiseCov = cv2.setIdentity(
dict_ekf[str(i.id)].processNoiseCov, 1e3) #4
dict_ekf[str(i.id)].measurementNoiseCov = cv2.setIdentity(
dict_ekf[str(i.id)].measurementNoiseCov, 1) #2
dict_ekf[str(i.id)].errorCovPost = cv2.setIdentity(
dict_ekf[str(i.id)].errorCovPost) #, 1)
else:
savedDetections[str(i.id)] = [(msg.header.stamp.secs, i.pose.pose)]
# Once the filter is initialized, for all new iterations:
translation = np.array([[
i.pose.pose.position.x, i.pose.pose.position.y,
i.pose.pose.position.z
]], np.float32).reshape(-1, 1)
rot_ang = np.array([
i.pose.pose.orientation.x, i.pose.pose.orientation.y,
i.pose.pose.orientation.z, i.pose.pose.orientation.w
], np.float32).reshape(-1, 1)
# x,y,z roll,pitch,yaw
measurements = np.concatenate((translation, rot_ang))
prediction = dict_ekf[str(i.id)].predict()
estimated = dict_ekf[str(i.id)].correct(measurements.reshape(
-1, 1))
estimated_poses[str(i.id)].header.stamp = msg.header.stamp
estimated_poses[str(i.id)].header.frame_id = i.id
estimated_poses[str(
i.id)].pose.position.x = estimated[0] #i.pose.pose.position.x
estimated_poses[str(
i.id)].pose.position.y = estimated[1] #i.pose.pose.position.y
estimated_poses[str(
i.id)].pose.position.z = estimated[2] #i.pose.pose.position.z
estimated_poses[str(i.id)].pose.orientation.x = estimated[
3] #i.pose.pose.orientation.x
estimated_poses[str(i.id)].pose.orientation.y = estimated[
4] #i.pose.pose.orientation.y
estimated_poses[str(i.id)].pose.orientation.z = estimated[
5] #i.pose.pose.orientation.z
if isinstance(i.pose.pose.orientation.w, list):
estimated_poses[str(
i.id)].pose.orientation.w = i.pose.pose.orientation.w[0]
else:
estimated_poses[str(
i.id)].pose.orientation.w = i.pose.pose.orientation.w
def __init__(self):
state_size = 5
meas_size = 3
contr_zize = 0
self.filter = cv2.KalmanFilter(state_size, meas_size, contr_zize)
self.state = np.empty([state_size, 1], np.float32)
self.meas = np.empty([meas_size, 1], np.float32)
self.found = False
self.not_found = 0
self.filter.transitionMatrix = np.asarray(self.filter.transitionMatrix)
self.filter.measurementMatrix = np.asarray(self.filter.measurementNoiseCov)
self.filter.processNoiseCov = np.asarray(self.filter.processNoiseCov)
self.filter.measurementNoiseCov = np.asarray(self.filter.measurementNoiseCov)
self.filter.errorCovPre = np.asarray(self.filter.errorCovPre)
cv2.setIdentity(self.filter.transitionMatrix)
self.filter.measurementMatrix = np.zeros((meas_size, state_size), np.float32)
self.filter.measurementMatrix.itemset(0, 1.0)
self.filter.measurementMatrix.itemset(6, 1.0)
self.filter.measurementMatrix.itemset(14, 1.0)
self.filter.processNoiseCov.itemset(0, 0.01)
self.filter.processNoiseCov.itemset(6, 0.01)
self.filter.processNoiseCov.itemset(12, 5.0)
self.filter.processNoiseCov.itemset(18, 5.0)
self.filter.processNoiseCov.itemset(24, 0.01)
cv2.setIdentity(self.filter.measurementNoiseCov, 0.1)
def initKalmanFilter(n_states, n_measurements, n_inputs, dt):
KF = cv2.KalmanFilter(n_states, n_measurements, n_inputs, cv2.CV_64F)
KF.processNoiseCov = cv2.setIdentity(KF.processNoiseCov, 1e-5)
KF.measurementNoiseCov = cv2.setIdentity(KF.measurementNoiseCov, 1e-2)
KF.errorCovPost = cv2.setIdentity(KF.errorCovPost, 1)
dt_2 = 0.5 * (dt**2)
"""
Problem: Assigning directly to elements in KF.transitionMatrix
and KF.measurementMatrix through numpy indexing does not actually
change the underlying data when calling them.
Quick solution: Reassign the variable directly to a new array object.
"""
tempTM = KF.transitionMatrix
tempMM = KF.measurementMatrix
# Position
tempTM[0, 3] = dt
tempTM[1, 4] = dt
tempTM[2, 5] = dt
tempTM[3, 6] = dt
tempTM[4, 7] = dt
tempTM[5, 8] = dt
tempTM[0, 6] = dt_2
tempTM[1, 7] = dt_2
tempTM[2, 8] = dt_2
# Orientation
tempTM[9, 12] = dt
tempTM[10, 13] = dt
tempTM[11, 14] = dt
tempTM[12, 15] = dt
tempTM[13, 16] = dt
tempTM[14, 17] = dt
tempTM[9, 15] = dt_2
tempTM[10, 16] = dt_2
tempTM[11, 17] = dt_2
tempMM[0, 0] = 1 # X
tempMM[1, 1] = 1 # Y
tempMM[2, 2] = 1 # Z
tempMM[3, 9] = 1 # Roll
tempMM[4, 10] = 1 # Pitch
tempMM[5, 11] = 1 # Yaw
KF.transitionMatrix = tempTM
KF.measurementMatrix = tempMM
return KF
def __init__(self, world, vehicle):
self.world = world
self.vehicle = vehicle
self.tMatrix = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32)
self.kf = cv2.KalmanFilter(4, 4)
self.kf.transitionMatrix = self.tMatrix
self.kf.measurementMatrix = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32)
self.kf.processNoiseCov = cv2.setIdentity(self.kf.processNoiseCov,
1e-2) # 1e-2
self.kf.measurementNoiseCov = cv2.setIdentity(
self.kf.measurementNoiseCov, 1e-3) # 1e-3
self.kf.errorCovPost = cv2.setIdentity(self.kf.errorCovPost,
1e-1) # 1e-1
print(self.kf.predict())
self.left_x = 0
self.right_x = 0
# Setup sensor
blueprint_library = world.get_blueprint_library()
# https://carla.readthedocs.io/en/latest/cameras_and_sensors
# get the blueprint for this sensor
self.blueprint = blueprint_library.find('sensor.camera.rgb')
print(self.blueprint)
# change the dimensions of the image
self.blueprint.set_attribute('image_size_x', f'{self.im_width}')
self.blueprint.set_attribute('image_size_y', f'{self.im_height}')
self.blueprint.set_attribute('fov', '110')
# Adjust sensor relative to vehicle
self.spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7))
# spawn the sensor and attach to vehicle.
self.sensor = world.spawn_actor(self.blueprint,
self.spawn_point,
attach_to=self.vehicle)
# print('sensor: ', self.sensor)
self.image_queue = queue.Queue()
self.sensor.listen(self.image_queue.put)
def _set_kalman(self):
self._kalman.transitionMatrix = np.float32(
[[1, 0, self._delta_time, 0], [0, 1, 0, self._delta_time],
[0, 0, 1, 0], [0, 0, 0, 1]])
self._kalman.statePre = np.zeros((4, 1), dtype=np.float32)
self._kalman.statePre[0][0] = self._point[0] # x
self._kalman.statePre[1][0] = self._point[1] # y
self._kalman.statePre[2][0] = 1 # init velocity x
self._kalman.statePre[3][0] = 1 # init velocity y
self._kalman.statePost = np.zeros((4, 1), dtype=np.float32)
self._kalman.statePost[0] = self._point[0]
self._kalman.statePost[1] = self._point[1]
self._kalman.measurementMatrix = cv2.setIdentity(
self._kalman.measurementMatrix)
self._kalman.processNoiseCov = np.float32(
[[
pow(self._delta_time, 4.0) / 4.0, 0,
pow(self._delta_time, 3.0) / 2.0, 0
],
[
0,
pow(self._delta_time, 4.0) / 4.0, 0,
pow(self._delta_time, 3.0) / 2.0
],
[
pow(self._delta_time, 3.0) / 2.0, 0,
pow(self._delta_time, 2.0), 0
],
[
0,
pow(self._delta_time, 3.0) / 2.0, 0,
pow(self._delta_time, 2.0)
]])
self._kalman.processNoiseCov *= self._accel_noise_mag
self._kalman.measurementNoiseCov = cv2.setIdentity(
self._kalman.measurementNoiseCov, 0.1)
self._kalman.errorCovPost = cv2.setIdentity(self._kalman.errorCovPost,
.1)
def __init__(self):
self.thresLower = (25, 50, 50)
self.thresUpper = (35, 255, 255)
self.image_sub = rospy.Subscriber("/vrep/image", Image, self.callback)
self.result_pub = rospy.Publisher("/ball_pos", Point32, queue_size=1)
self.bridge = CvBridge()
self.kf = cv2.KalmanFilter(5, 3)
cv2.setIdentity(self.kf.transitionMatrix, 1.)
self.kf.measurementMatrix = np.array(
[[1., 0., 0., 0., 0.], [0., 1., 0., 0., 0.], [0., 0., 0., 0., 1.]],
np.float32)
self.kf.processNoiseCov = np.array(
[[1e-3, 0, 0, 0, 0], [0, 1e-3, 0, 0, 0], [0, 0, 5., 0, 0],
[0, 0, 0, 5., 0], [0, 0, 0, 0, 1e-1]], np.float32)
cv2.setIdentity(self.kf.measurementNoiseCov, 1e-1)
self.ticks = cv2.getTickCount()
def __init__(self, start_measure, meaningful_of_noise=0.5):
self.tracker_id = KalmanTracker.next_id
KalmanTracker.next_id += 1
self.color = (random.randint(0, 255), random.randint(0, 255),
random.randint(0, 255)) # Set random RGB color
self.start_measure = start_measure
self.act_prediction = start_measure
self.frames_without_update = 0
self.life_time = 0
self.prediction_history = Queue(KalmanTracker.max_history_predictions)
self.kalman_filter = cv2.KalmanFilter(
dynamParams=KalmanTracker.matrix_size, # Dimension of the state
measureParams=KalmanTracker.
measure_params, # Dimension of measurement
controlParams=0) # Dimension of control vector
# Init state
pre_state = self.kalman_filter.statePre
pre_state = KalmanTracker.__get_updated_measure_array(
pre_state, self.start_measure)
self.kalman_filter.statePre = pre_state
self.kalman_filter.statePost = np.copy(pre_state) # Conversion
self.kalman_filter.measurementMatrix = cv2.setIdentity(
self.kalman_filter.measurementMatrix)
# Set transitionMatrix
self.__measures_addiction = np.diag(np.ones(
KalmanTracker.matrix_size - KalmanTracker.min_matrix_size,
dtype=np.float32),
k=KalmanTracker.min_matrix_size)
matrix_A = np.diag(
np.ones(KalmanTracker.matrix_size,
dtype=np.float32)) + self.__measures_addiction
self.kalman_filter.transitionMatrix = matrix_A
self.kalman_filter.processNoiseCov *= meaningful_of_noise
self.kalman_filter.measurementNoiseCov = cv2.setIdentity(
self.kalman_filter.measurementNoiseCov,
KalmanTracker.__cv2_scalar_all(0.1))
self.kalman_filter.errorCovPost = cv2.setIdentity(
self.kalman_filter.errorCovPost,
KalmanTracker.__cv2_scalar_all(0.1))
class KalmanFilter:
kf = cv.KalmanFilter(4, 2)
kf.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kf.transitionMatrix = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
cv2.setIdentity(kf.measurementMatrix)
cv2.setIdentity(kf.processNoiseCov, 1e-5)
cv2.setIdentity(kf.measurementNoiseCov, 1e-1)
cv2.setIdentity(kf.errorCovPost, 1)
cv2.randn(kf.statePost, 0, 0.1)
def Estimate(self, coordX, coordY):
''' This function estimates the position of the object'''
measured = np.array([[np.float32(coordX)], [np.float32(coordY)]])
self.kf.correct(measured)
predicted = self.kf.predict()
#print("PREDICTED ===== "+str(predicted))
return predicted
def initKalmanFilter(nStates, nMeasurements, nInputs, dt):
KF = cv2.KalmanFilter(nStates, nMeasurements, nInputs)
KF.processNoiseCov = cv2.setIdentity(KF.processNoiseCov, 1e-5)
KF.measurementNoiseCov = cv2.setIdentity(KF.measurementNoiseCov, 1e-2)
KF.errorCovPost = cv2.setIdentity(KF.errorCovPost, 1)
dt2 = 0.5*dt*dt
transitionMatrix = np.identity(nStates, dtype=np.float32)
transitionMatrix[0, 3] = dt
transitionMatrix[1, 4] = dt
transitionMatrix[2, 5] = dt
transitionMatrix[3, 6] = dt
transitionMatrix[4, 7] = dt
transitionMatrix[5, 8] = dt
transitionMatrix[0, 6] = dt2
transitionMatrix[1, 7] = dt2
transitionMatrix[2, 8] = dt2
transitionMatrix[9, 12] = dt
transitionMatrix[10, 13] = dt
transitionMatrix[11, 14] = dt
transitionMatrix[12, 15] = dt
transitionMatrix[13, 16] = dt
transitionMatrix[14, 17] = dt
transitionMatrix[9, 15] = dt2
transitionMatrix[10, 16] = dt2
transitionMatrix[11, 17] = dt2
KF.transitionMatrix = transitionMatrix
measurementMatrix = np.zeros((nMeasurements, nStates), dtype=np.float32)
measurementMatrix[0, 0] = 1
measurementMatrix[1, 1] = 1
measurementMatrix[2, 2] = 1
measurementMatrix[3, 9] = 1
measurementMatrix[4, 10] = 1
measurementMatrix[5, 11] = 1
KF.measurementMatrix = measurementMatrix
return KF
def Kcorrect(self, meas, restart=False):
'''
State correction using measurement matrix with 3D points
'''
if (restart): # Restart the filter
# Initialization
cv.setIdentity(kf.errorCovPre, 1.0)
# Force the measurement to be used with 100% weight ignoring Hx
kf.statePost[SPX] = meas[SPX]
kf.statePost[SPY] = meas[SPY]
kf.statePost[SPZ] = meas[SPZ]
kf.statePost[SVX] = 3.
kf.statePost[SVY] = 3.
kf.statePost[SVZ] = 3.
kf.statePost[SAC] = grav
else:
kf.correct(meas)
# Kalman Correction
return np.float32(
[kf.statePost[SPX], kf.statePost[SPY],
kf.statePost[SPZ]]).squeeze()
def initialTrackerwithHog(self, fvs, hogPersonDetector):
'''
initialize kalman tracker with hog detector
by reading frames until the first person is detected
'''
# loop over the frames obtained from the video file stream
while fvs.more():
frame = fvs.read()
objectDetected = hogPersonDetector.detectLargest(frame)
if len(objectDetected) > 0:
#x1, y1, w1, h1 = objectDetected.ravel()
#cv2.rectangle(frame, (x1, y1), (x1+w1, y1+h1), (0,0,255), 2, 4)
# copy max area to kalman filter
self.measurement = objectDetected[:3].astype(np.float32)
# update state , its worthy to note that x,y, w are set
# to measured values where vx= vy= vw = 0 as we have no idea
# about the velocities yet
self.KF.statePost[0:3, 0] = self.measurement[:, 0]
self.KF.statePost[3:6] = 0.0
# set processNoisCon (Q) and measurementNoiseCov (R)
# these values are set by trial and error by trying various values
# set diagnal values for covariance matrices . processNoiseCon is Q
self.KF.processNoiseCov = cv2.setIdentity(
self.KF.processNoiseCov, (1e-2))
self.KF.measurementNoiseCov = cv2.setIdentity(
self.KF.measurementNoiseCov, (1e-2))
return cv2.getTickCount()
def correct(self,
bbx,
bby,
bbw,
bbh,
restart=False
): # state correction using measurement matrix with BB
if restart:
self.ticks = cv.getTickCount()
cv.setIdentity(kfilt.errorCovPre, 1.0)
# Updating statePost with bbx, bby, bbw, and bbh
kfilt.statePost[SBBX] = bbx
kfilt.statePost[SBBX] = bby
kfilt.statePost[SV_X] = 0
kfilt.statePost[SV_Y] = 0
kfilt.statePost[SBBW] = bbw
kfilt.statePost[SBBH] = bbh
kfilt.statePost[SV_W] = 0
kfilt.statePost[SV_H] = 0
else:
# Running correct with bbx, bby, bbw, and bbh
kfilt.correct(np.float32([bbx, bby, bbw, bbh]).squeeze())
# Keeping the values greater than or equal to 0
kfilt.statePost[SBBX] = max(0.0, kfilt.statePost[SBBX])
kfilt.statePost[SBBY] = max(0.0, kfilt.statePost[SBBY])
kfilt.statePost[SBBW] = max(0.0, kfilt.statePost[SBBW])
kfilt.statePost[SBBH] = max(0.0, kfilt.statePost[SBBH])
# Return a floating point array with the corrected values for bbx, bby, bbw, bbh
return np.float32([
kfilt.statePost[SBBX], kfilt.statePost[SBBY],
kfilt.statePost[SBBW], kfilt.statePost[SBBH]
]).squeeze()
def initKalmanFilter(self):
# see https://docs.opencv.org/3.3.0/dc/d2c/tutorial_real_time_pose.html
fps = self.video.get(cv2.CAP_PROP_FPS)
dt = 1.0 / fps
KF = cv2.KalmanFilter(18, 6, 0, cv2.CV_64F)
print("process noise cov", KF.processNoiseCov.shape)
print("measurement noise cov", KF.measurementNoiseCov.shape)
print("error cov post", KF.errorCovPost.shape)
cv2.setIdentity(KF.processNoiseCov, 2000)
cv2.setIdentity(KF.measurementNoiseCov, 2000)
cv2.setIdentity(KF.errorCovPost, 2000)
# position
KF.transitionMatrix = np.eye(18)
KF.transitionMatrix[0, 3] = dt
KF.transitionMatrix[1, 4] = dt
KF.transitionMatrix[2, 5] = dt
KF.transitionMatrix[3, 6] = dt
KF.transitionMatrix[4, 7] = dt
KF.transitionMatrix[5, 8] = dt
KF.transitionMatrix[0, 6] = 0.5 * dt * dt
KF.transitionMatrix[1, 7] = 0.5 * dt * dt
KF.transitionMatrix[2, 8] = 0.5 * dt * dt
#orientation
KF.transitionMatrix[9, 12] = dt
KF.transitionMatrix[10, 13] = dt
KF.transitionMatrix[11, 14] = dt
KF.transitionMatrix[12, 15] = dt
KF.transitionMatrix[13, 16] = dt
KF.transitionMatrix[14, 17] = dt
KF.transitionMatrix[9, 15] = 0.5 * dt * dt
KF.transitionMatrix[10, 16] = 0.5 * dt * dt
KF.transitionMatrix[11, 17] = 0.5 * dt * dt
# measurement model
KF.measurementMatrix = np.zeros(KF.measurementMatrix.shape)
KF.measurementMatrix[0, 0] = 1
KF.measurementMatrix[1, 1] = 1
KF.measurementMatrix[2, 2] = 1
KF.measurementMatrix[3, 9] = 1
KF.measurementMatrix[4, 10] = 1
KF.measurementMatrix[5, 11] = 1
return KF
def __initializeKalmanFilter(self, initial_bounding_box):
self.filter = cv2.KalmanFilter(4, 2, 0)
self.filter.transitionMatrix = np.array(
[[1.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0], [0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]], np.float32)
initial_position = self.getCenter(initial_bounding_box)
self.filter.statePre = np.array(
[initial_position[0], initial_position[1], 0, 0], np.float32)
self.filter.statePost = np.array(
[initial_position[0], initial_position[1], 0, 0], np.float32)
self.filter.measurementMatrix = np.array(
[[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]], np.float32)
# cv2.setIdentity(self.filter.measurementMatrix)
cv2.setIdentity(self.filter.processNoiseCov, 10**-4)
cv2.setIdentity(self.filter.measurementNoiseCov, 10**-1)
cv2.setIdentity(self.filter.errorCovPost, 10**-1)
def __init__(self):
self.kf = cv2.KalmanFilter(6, 4, 0)
# self.kf.transitionMatrix = np.eye(6, dtype=np.float32)
cv2.setIdentity(self.kf.transitionMatrix)
self.kf.measurementMatrix = np.zeros((4, 6), dtype=np.float32)
self.kf.measurementMatrix[0, 0] = 1
self.kf.measurementMatrix[1, 1] = 1
self.kf.measurementMatrix[2, 4] = 1
self.kf.measurementMatrix[3, 5] = 1
# self.kf.processNoiseCov = 1e-2 * np.eye(6, dtype=np.float32)
cv2.setIdentity(self.kf.processNoiseCov, 1e-2)
self.kf.processNoiseCov[2, 2] = 5
self.kf.processNoiseCov[3, 3] = 5
# self.kf.measurementNoiseCov = 1e-1 * np.eye(6, dtype=np.float32)
cv2.setIdentity(self.kf.measurementNoiseCov, 1e-1)
print("Camera could not be opened.")
DELTA_FRAMES = 0.001;
FRAME_COUNT = 0
tMatrix = np.array([[1, 0, DELTA_FRAMES, 0],
[0, 1, 0, DELTA_FRAMES],
[0, 0, 1, 0],
[0, 0, 0, 1]], np.float32)
kf = cv2.KalmanFilter(4, 4)
kf.transitionMatrix = tMatrix
kf.measurementMatrix = np.array([[1, 0, DELTA_FRAMES, 0],
[0, 1, 0, DELTA_FRAMES],
[0, 0, 1, 0],
[0, 0, 0, 1]], np.float32)
kf.processNoiseCov = cv2.setIdentity(kf.processNoiseCov, 1e-2)
kf.measurementNoiseCov = cv2.setIdentity(kf.measurementNoiseCov, 1e-7)
kf.errorCovPost = cv2.setIdentity(kf.errorCovPost, 1e-1)
print(kf.statePre)
print(kf.transitionMatrix)
print(kf.measurementMatrix)
print(kf.processNoiseCov)
print(kf.measurementNoiseCov)
print(kf.errorCovPost)
lastX = 0
lastY = 0
prediction = kf.predict()
while True:
thresLower = (25, 50, 50)
thresUpper = (35, 255, 255)
pts = deque(maxlen=64)
camera = cv2.VideoCapture(0)
# In[3]:
stateSize = 5 # [x, y, v_x, v_y, r]'
measSize = 3 # [x, y, r]
kf = cv2.KalmanFilter(stateSize, measSize)
cv2.setIdentity(kf.transitionMatrix, 1.)
kf.measurementMatrix = np.array([
[1., 0., 0., 0., 0.],
[0., 1., 0., 0., 0.],
[0., 0., 0., 0., 1.]
],np.float32)
kf.processNoiseCov = np.array([
[1e-3, 0, 0, 0, 0],
[0, 1e-3, 0, 0, 0],
[0, 0, 5., 0, 0],
[0, 0, 0, 5., 0],
[0, 0, 0, 0, 1e-1]
],np.float32)
def Map2localization(trans):
global estimated_tf, init, localize, offset_hist, map2odom
offset = TransformStamped()
# We want to transform the detected aruco pose to map frame to do a reasonable comparison
qinv = quaternion_from_euler(0, 0, 0)
qdetected = quaternion_from_euler(0, 0, 0)
if init:
#Making sure that a Kalman filter is initialized for each marker if we don't have the Aruco Marker registered in the dictionary.
estimated_tf = cv2.KalmanFilter(6,
6) # Initialization of kalman filter.
trans_mat = np.identity(6, np.float32)
estimated_tf.transitionMatrix = trans_mat #x'=Ax+BU transition matrix is A
estimated_tf.measurementMatrix = trans_mat
estimated_tf.processNoiseCov = cv2.setIdentity(
estimated_tf.processNoiseCov, 1e-3) #4
estimated_tf.measurementNoiseCov = cv2.setIdentity(
estimated_tf.measurementNoiseCov, 1e-2) #2
estimated_tf.errorCovPost = cv2.setIdentity(
estimated_tf.errorCovPost) #, 1)
init = False
if not buff.can_transform(trans.child_frame_id, 'cf1/odom',
trans.header.stamp, rospy.Duration(0.5)):
rospy.logwarn_throttle(
5.0, 'No tranform from %s to map' % trans.child_frame_id)
return
else:
t = TransformStamped()
try:
# We want to keep the relative position.... We can calculate the error betwen these frames.
t = buff.lookup_transform('cf1/odom', trans.child_frame_id,
rospy.Time(0), rospy.Duration(0.5))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn_throttle(
5.0, 'No tranform from %s to odom' % trans.child_frame_id)
return
#t.transform.translation.z=0.35
#==============================================================================
#Ludvig's solution
#==============================================================================
Fodom = tf_conversions.fromTf(
((t.transform.translation.x, t.transform.translation.y,
t.transform.translation.z),
(t.transform.rotation.x, t.transform.rotation.y,
t.transform.rotation.z, t.transform.rotation.w)))
Fmap = tf_conversions.fromMsg(json_markers[int(
trans.child_frame_id[-1])])
Fdiff = Fmap * Fodom.Inverse()
offset = TransformStamped()
offset.header.stamp = rospy.Time.now()
offset.header.frame_id = 'map'
offset.child_frame_id = 'cf1/odom'
((offset.transform.translation.x, offset.transform.translation.y,
offset.transform.translation.z),
(offset.transform.rotation.x, offset.transform.rotation.y,
offset.transform.rotation.z,
offset.transform.rotation.w)) = tf_conversions.toTf(Fdiff)
#======================================
#======================================
#=====================================
#offset.transform.translation.z = 0 # Not considering any changes in z-axis
#print("before filter " + str(offset.transform.translation.x) + " " + str(offset.transform.translation.y) + " " + str(offset.transform.translation.z))
"""
print("")
offset.transform.translation.x = butter_lowpass_filter(offset.transform.translation.x, cutoff, fs, order)
offset.transform.translation.y = butter_lowpass_filter(offset.transform.translation.y, cutoff, fs, order)
offset.transform.translation.z = butter_lowpass_filter(offset.transform.translation.z, cutoff, fs, order)
#offset.transform.rotation.x = butter_lowpass_filter(offset.transform.rotation.x, cutoff, fs, order)
#offset.transform.rotation.y = butter_lowpass_filter(offset.transform.rotation.y, cutoff, fs, order)
#offset.transform.rotation.z = butter_lowpass_filter(offset.transform.rotation.z, cutoff, fs, order)
#offset.transform.rotation.w = butter_lowpass_filter(offset.transform.rotation.w, cutoff, fs, order)
"""
#print("after filter " + str(offset.transform.translation.x) + " " + str(offset.transform.translation.y) + " " + str(offset.transform.translation.z))
#=====================================
#offset_hist.append((offset.transform.translation.x,offset.transform.translation.y,offset.transform.translation.z))
#offset_hist.append(offset.transform.translation.z)
# pp.pprint(offset_hist)
# print('list\n')
#print(offset_hist)
# if len(offset_hist)>1:
# offset_hist=offset_hist[-1:]
# #print(offset_hist)
# average = np.mean(offset_hist, axis=0)
# #print(average)
# offset.transform.translation.x =average[0]
# offset.transform.translation.y =average[1]
# offset.transform.translation.z =average[2]
# #offset.transform.translation.y = average[1]
# # print('hi!')
# # Fdiff=np.average(offset_hist)
# # print('list 2\n')
# # pp.pprint(Fdiff)
# # #sum(offset_hist)/float(len(offset_hist))
# del offset_hist[:]
# offset.transform.translation.z = 0
# # #=====================================
# tf_bc.sendTransform(offset)
# map2odom=offset
# localize.data=True
# pub_tf.publish(localize)
# #=====================================
#offset.transform.translation.z = 0 # Not considering any changes in z-axis
filtered_offset = copy.deepcopy(offset)
# ########Update the Kalman filter
rot_ang = np.array([
filtered_offset.transform.rotation.x,
filtered_offset.transform.rotation.y,
filtered_offset.transform.rotation.z,
filtered_offset.transform.rotation.w
], np.float32).reshape(-1, 1)
translation = np.array([
filtered_offset.transform.translation.x,
filtered_offset.transform.translation.y
], np.float32).reshape(-1, 1)
# # x,y,z roll,pitch,yaw
measurements = np.concatenate((translation, rot_ang))
prediction = estimated_tf.predict()
estimation = estimated_tf.correct(measurements.reshape(-1, 1))
offset.transform.translation.x = estimation[0]
offset.transform.translation.y = estimation[1]
offset.transform.translation.x = estimation[0]
offset.transform.translation.y = estimation[1]
offset.transform.translation.z = 0
offset.transform.rotation.x = estimation[2]
offset.transform.rotation.y = estimation[3]
offset.transform.rotation.z = estimation[4]
offset.transform.rotation.w = estimation[5]
n = np.linalg.norm([
offset.transform.rotation.x, offset.transform.rotation.y,
offset.transform.rotation.z, offset.transform.rotation.w
])
# #Normalize the quaternion
offset.transform.rotation.x /= n
offset.transform.rotation.y /= n
offset.transform.rotation.z /= n
offset.transform.rotation.w /= n
#'''
#tf_bc.sendTransform(offset)
map2odom = offset
localize.data = True
pub_tf.publish(localize)
def eyeGazeTacking():
global metaMain, netMain, altNames
configPath = "cfg/yolov3-tiny-1c.cfg"
weightPath = "backup/yolov3-tiny-1c_last.weights"
metaPath = "data/eye.data"
if netMain is None:
netMain = darknet.load_net_custom(configPath.encode("ascii"),
weightPath.encode("ascii"), 0,
1) # batch size = 1
if metaMain is None:
metaMain = darknet.load_meta(metaPath.encode("ascii"))
if altNames is None:
try:
with open(metaPath) as metaFH:
metaContents = metaFH.read()
import re
match = re.search("names *= *(.*)$", metaContents,
re.IGNORECASE | re.MULTILINE)
if match:
result = match.group(1)
else:
result = None
try:
if os.path.exists(result):
with open(result) as namesFH:
namesList = namesFH.read().strip().split("\n")
altNames = [x.strip() for x in namesList]
except TypeError:
pass
except Exception:
pass
kalman = cv2.KalmanFilter(4, 2, 0)
measurement = np.array((2, 1), np.float32)
kalman.statePre[0] = 500
kalman.statePre[1] = 280
kalman.statePre[2] = 0
kalman.statePre[3] = 0
kalman.transitionMatrix = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]],
np.float32)
cv2.setIdentity(kalman.processNoiseCov, 1e-5)
cv2.setIdentity(kalman.measurementNoiseCov, 1e-1)
cv2.setIdentity(kalman.errorCovPost, 1)
cv2.randn(kalman.statePost, 0, 0.1)
eyev = []
kalmanv = []
cap = cv2.VideoCapture(1)
cap.set(3, 1280)
cap.set(4, 720)
cap.set(cv2.CAP_PROP_FPS, 60)
detector = dlib.get_frontal_face_detector()
predictor = dlib.shape_predictor("shape_predictor_68_face_landmarks.dat")
font = cv2.FONT_HERSHEY_SIMPLEX
# Create an image we reuse for each detect
darknet_image = darknet.make_image(darknet.network_width(netMain),
darknet.network_height(netMain), 3)
while True:
stime = time.time()
ret, image = cap.read()
keyboard = np.zeros((560, 1000, 3), np.uint8)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
faces = detector(gray)
for face in faces:
prediction = kalman.predict()
predict_pt = (prediction[0], prediction[1])
# x, y = face.left(), face.top()
# x1, y1 = face.right(), face.bottom()
# cv2.rectangle(frame, (x,y), (x1,y1), (0,255,0), 2)
landmarks = predictor(gray, face)
# for points in range(36, 48):
# x = landmarks.part(points).x
# y = landmarks.part(points).y
# cv2.circle(frame, (x, y), 3, (0, 0, 255), 2)
left_point = (landmarks.part(36).x, landmarks.part(36).y)
right_point = (landmarks.part(39).x, landmarks.part(39).y)
# hor_line = cv2.line(image, left_point, right_point, (0, 0, 255), 3)
srodek_gora_lewe = srodek(landmarks.part(37), landmarks.part(38))
srodek_dol_lewe = srodek(landmarks.part(41), landmarks.part(40))
# ver_line = cv2.line(image, srodek_gora_lewe, srodek_dol_lewe, (0, 0, 255), 3)
left_point1 = (landmarks.part(42).x, landmarks.part(42).y)
right_point1 = (landmarks.part(45).x, landmarks.part(45).y)
# hor_line1 = cv2.line(image, left_point1, right_point1, (0, 0, 255), 3)
srodek_gora_prawe = srodek(landmarks.part(43), landmarks.part(44))
srodek_dol_prawe = srodek(landmarks.part(47), landmarks.part(46))
# ver_line1 = cv2.line(image, srodek_gora_prawe, srodek_dol_prawe, (0, 0, 255), 3)
hor_line_lenght = hypot((left_point[0] - right_point[0]),
(left_point[1] - right_point[1]))
ver_line_lenght = hypot((srodek_gora_lewe[0] - srodek_dol_lewe[0]),
(srodek_gora_lewe[1] - srodek_dol_lewe[1]))
wsp_otwartosci_oka = hor_line_lenght / ver_line_lenght
if wsp_otwartosci_oka < 6:
cv2.putText(image, "Otwarte", (50, 150), font, 3, (255, 0, 0))
if wsp_otwartosci_oka > 6:
cv2.putText(image, "Zakmniete", (50, 150), font, 3,
(255, 0, 0))
left_eye_region = np.array(
[(landmarks.part(36).x, landmarks.part(36).y),
(landmarks.part(37).x, landmarks.part(37).y),
(landmarks.part(38).x, landmarks.part(38).y),
(landmarks.part(39).x, landmarks.part(39).y),
(landmarks.part(40).x, landmarks.part(40).y),
(landmarks.part(41).x, landmarks.part(41).y)], np.int32)
# cv2.polylines(image, [left_eye_region], True, (0, 0, 255), 2)
right_eye_region = np.array(
[(landmarks.part(36).x, landmarks.part(36).y),
(landmarks.part(37).x, landmarks.part(37).y),
(landmarks.part(38).x, landmarks.part(38).y),
(landmarks.part(39).x, landmarks.part(39).y),
(landmarks.part(40).x, landmarks.part(40).y),
(landmarks.part(41).x, landmarks.part(41).y)], np.int32)
# cv2.polylines(frame, [right_eye_region], True, (0, 0, 255), 2)
# height, width, _ = image.shape
# mask = np.zeros((height, width), np.uint8)
#
# # gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
#
# cv2.polylines(mask, [left_eye_region], True, 255, 2)
#
# min_x = np.min(left_eye_region[:, 0])
# max_x = np.max(left_eye_region[:, 0])
#
# min_y = np.min(left_eye_region[:, 1])
# max_y = np.max(left_eye_region[:, 1])
#
# gray_eye = image[min_y: max_y, min_x: max_x]
# _, threshold_eye = cv2.threshold(gray_eye, 50, 240, cv2.THRESH_BINARY)
color_eye = image
color_eye = cv2.cvtColor(color_eye, cv2.COLOR_BGR2RGB)
color_eye = cv2.resize(color_eye,
(darknet.network_width(netMain),
darknet.network_height(netMain)),
interpolation=cv2.INTER_LINEAR)
darknet.copy_image_from_bytes(darknet_image, color_eye.tobytes())
detections = darknet.detect_image(netMain,
metaMain,
darknet_image,
thresh=0.25)
_, xmin, ymin, xmax, ymax = cvDrawBoxes(detections, image)
xsr = int(round(xmin + ((xmax - xmin) / 2)))
ysr = int(round(ymin + ((ymax - ymin) / 2)))
cv2.line(
image,
(int(round(left_point1[0])), int(round(left_point1[1]))),
(int(round(right_point1[0])), int(round(right_point1[1]))),
(0, 0, 255), 2)
cv2.line(image, (int(round(
srodek_gora_prawe[0])), int(round(srodek_gora_prawe[1]))),
(int(round(srodek_dol_prawe[0])),
int(round(srodek_dol_prawe[1]))), (0, 0, 255), 2)
cv2.circle(image, (xsr, ysr), 4, (255, 0, 0), 4)
# color_eye = cv2.cvtColor(color_eye, cv2.COLOR_BGR2RGB)
print(ver_line_lenght)
print(hor_line_lenght)
# proportial_ratio_x = 1000/1280*15
# proportial_ratio_y = 560/720*30
# cv2.line(keyboard, (int(round((srodek_gora_prawe[0]-left_point1[0])*proportial_ratio_x)), int(round((srodek_gora_prawe[1]-srodek_gora_prawe[1])*proportial_ratio_y))),
# (int(round((srodek_dol_prawe[0]-left_point1[0])*proportial_ratio_x)), int(round((srodek_dol_prawe[1]-srodek_gora_prawe[1])*proportial_ratio_y))), (0, 0, 255), 2)
# cv2.line(keyboard, (int((round(left_point1[0]-left_point1[0])*proportial_ratio_x)), int(round((left_point1[1]-srodek_gora_prawe[1])*proportial_ratio_y))),
# (int(round((right_point1[0]-left_point1[0])*proportial_ratio_x)), int(round((right_point1[1]-srodek_gora_prawe[1])*proportial_ratio_y))), (0, 0, 255), 2)
#
# cv2.circle(keyboard, (int(round((xsr-left_point1[0])*proportial_ratio_x)), int(round((ysr-srodek_gora_prawe[1])*proportial_ratio_y))), 4, (255, 0, 0), 4)
proportial_ratio_x = hor_line_lenght / 1000
proportial_ratio_y = ver_line_lenght / 560
cv2.line(keyboard,
(int(
round((srodek_gora_prawe[0] - left_point1[0]) /
proportial_ratio_x)),
int(
round((srodek_gora_prawe[1] - srodek_gora_prawe[1]) /
proportial_ratio_y))),
(int(
round((srodek_dol_prawe[0] - left_point1[0]) /
proportial_ratio_x)),
int(
round((srodek_dol_prawe[1] - srodek_gora_prawe[1]) /
proportial_ratio_y))), (0, 0, 255), 2)
cv2.line(keyboard,
(int((round(left_point1[0] - left_point1[0]) /
proportial_ratio_x)),
int(
round((left_point1[1] - srodek_gora_prawe[1]) /
proportial_ratio_y))),
(int(
round((right_point1[0] - left_point1[0]) /
proportial_ratio_x)),
int(
round((right_point1[1] - srodek_gora_prawe[1]) /
proportial_ratio_y))), (0, 0, 255), 2)
cv2.circle(
keyboard,
(int(round((xsr - left_point1[0]) / proportial_ratio_x)),
int(round(
(ysr - srodek_gora_prawe[1]) / proportial_ratio_y))), 4,
(255, 0, 0), 4)
measurement[0] = int(
round((xsr - left_point1[0]) / proportial_ratio_x))
measurement[1] = int(
round((ysr - srodek_gora_prawe[1]) / proportial_ratio_y))
estimated = kalman.correct(measurement)
state_pt = (estimated[0], estimated[1])
meas_pt = (measurement[0], measurement[1])
eyev.append(meas_pt)
kalmanv.append(state_pt)
cv2.circle(keyboard, (meas_pt[0], meas_pt[1]), 5, (0, 255, 0), 1)
cv2.circle(keyboard, (predict_pt[0], predict_pt[1]), 5,
(0, 0, 255), 1)
for i in range(len(eyev) - 1):
cv2.line(keyboard, eyev[i], eyev[i + 1], (255, 0, 0), 1)
for i in range(len(kalmanv) - 1):
cv2.line(keyboard, kalmanv[i], kalmanv[i + 1], (0, 155, 255),
1)
cv2.imshow("Eyess", image)
cv2.imshow("Keyboard", keyboard)
print('FPS {:.1f}'.format(1 / (time.time() - stime)))
key = cv2.waitKey(1)
if key == 27:
break
cap.release()
cv2.destroyAllWindows()
def __init__(self,
timestep,
process_noise_cov=None,
measurement_noise_cov=None):
if process_noise_cov is None:
process_noise_cov = np.array([1e-1, 1e-1, 1e-1, 1e-1], "float32")
if measurement_noise_cov is None:
measurement_noise_cov = np.array([1e-1, 1e-1], "float32")
self.kf = cv2.KalmanFilter(STATE_SIZE, MEASURE_SIZE, CONTROL_SIZE)
self.state = np.zeros(STATE_SIZE, "float32") # [x, y, vx, vy]
self.meas = np.zeros(MEASURE_SIZE, "float32") # [zx, zy]
# State transition matrix (A).
# Used for calculating (and updating) the estimated, or next, state of the
# parameters. It is updated with dt in the predict phase which results in the
# following transitions:
# position_i (x, y) = position_{i-1} + dt_i * velocity_{i-1}
# velocity_i (vx, vy) = velocity_{i-1}
# In matrix form:
# [ 1 0 dt 0 ]
# [ 0 1 0 dt ]
# [ 0 0 1 0 ]
# [ 0 0 0 1 ]
self.kf.transitionMatrix = np.zeros((STATE_SIZE, STATE_SIZE),
"float32")
cv2.setIdentity(self.kf.transitionMatrix)
self.kf.transitionMatrix[0, 2] = timestep
self.kf.transitionMatrix[1, 3] = timestep
# Face detections results in two sensor measurements, position (x, y). I.e.
# [ 1 0 0 0 ]
# [ 0 1 0 0 ]
# [ 0 0 0 0 ]
# [ 0 0 0 0 ]
self.kf.measurementMatrix = np.zeros((MEASURE_SIZE, STATE_SIZE),
"float32")
self.kf.measurementMatrix[0, 0] = 1.0
self.kf.measurementMatrix[1, 1] = 1.0
# Process Noise Covariance Matrix (Q):
# [ Ex 0 0 0 ]
# [ 0 Ey 0 0 ]
# [ 0 0 Evx 0 ]
# [ 0 0 0 Evy ]
self.kf.processNoiseCov = np.zeros((STATE_SIZE, STATE_SIZE), "float32")
self.kf.processNoiseCov[0, 0] = process_noise_cov[0]
self.kf.processNoiseCov[1, 1] = process_noise_cov[1]
self.kf.processNoiseCov[2, 2] = process_noise_cov[2]
self.kf.processNoiseCov[3, 3] = process_noise_cov[3]
# Measurement Noise Covariance Matrix (R).
# [ Ex 0 ]
# [ 0 Ey ]
# Since we are using pixels, magnitudes will be around one.
self.kf.measurementNoiseCov = np.zeros((MEASURE_SIZE, MEASURE_SIZE),
"float32")
self.kf.measurementNoiseCov[0, 0] = measurement_noise_cov[0]
self.kf.measurementNoiseCov[1, 1] = measurement_noise_cov[1]
# Error Covariance
self.kf.errorCovPre = np.zeros((STATE_SIZE, STATE_SIZE), "float32")
self.kf.errorCovPre[0, 0] = 1.0
self.kf.errorCovPre[1, 1] = 1.0
self.kf.errorCovPre[2, 2] = 1.0
self.kf.errorCovPre[3, 3] = 1.0
self.kf.statePost = np.zeros((STATE_SIZE, 1), "float32")
self.found = False
# 1, 0, 0, dt, 0, 0, # 0, 1, 0, 0, dt, 0, # 0, 0, 1, 0, 0, dt, # 0, 0, 0, 1, 0, 0, # 0, 0, 0, 0, 1, 0, # 0, 0, 0, dt, 0, 1 # ] # because # x = x + vx * dt # y = y + vy * dt # w = y + vw * dt # vx = vx # vy = vy # vw = vw KF.transitionMatrix = cv2.setIdentity(KF.transitionMatrix) # Measurement matrix is of the form # [ # 1, 0, 0, 0, 0, 0, # 0, 1, 0, 0, 0, 0, # 0, 0, 1, 0, 0, 0, # ] # because we are detecting only x, y and w. # These quantities are updated. KF.measurementMatrix = cv2.setIdentity(KF.measurementMatrix) # Variable to store detected x, y and w measurement = np.zeros((3, 1), dtype=np.float32) # Variables to store detected object and tracked object objectTracked = np.zeros((4, 1), dtype=np.float32)
def analysis_video(in_file, out_file):
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video", help="path to the (optional) video file")
args = vars(ap.parse_args())
greenLower = (29, 86, 6)
greenUpper = (64, 255, 255)
cap = cv2.VideoCapture(UPLOAD_FOLDER + '/' + in_file)
cap.set(3, 320) #cv2.CAP_PROP_FRAME_WIDTH
cap.set(4, 240) #cv2.CAP_PROP_FRAME_HEIGHT
stateSize = 6
measSize = 4
contrSize = 0
ltype = cv2.CV_32F
end_flag = False
kf = cv2.KalmanFilter(stateSize, measSize, contrSize, cv2.CV_32F)
state = np.zeros((stateSize, 1), np.float32)
meas = np.zeros((measSize, 1), np.float32)
cv2.setIdentity(kf.transitionMatrix, 1)
kf.measurementMatrix = np.zeros((measSize, stateSize), np.float32)
kf.measurementMatrix[0, 0] = 1.0
kf.measurementMatrix[3, 1] = 1.0
kf.measurementMatrix[0, 4] = 1.0
kf.measurementMatrix[3, 5] = 1.0
kf.processNoiseCov[0, 0] = 1e-2
kf.processNoiseCov[1, 1] = 1e-2
kf.processNoiseCov[2, 2] = 5.0
kf.processNoiseCov[3, 3] = 5.0
kf.processNoiseCov[4, 4] = 1e-2
kf.processNoiseCov[5, 5] = 1e-2
cv2.setIdentity(kf.measurementNoiseCov, 1e-1)
ch = 0
ticks = 0
found = False
notFoundCount = 0
#Main loop
pos = tmpno = 0
maskx = 450
masky = 200
frame_width = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
frame_height = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
fps = cap.get(cv2.CAP_PROP_FPS)
#vector<cv::Rect> prevBox;
prevBox = []
Avgvelocity = 0.0
TotalVelocity = 0.0
TotalFrame = 0
totaldispixel = np.sqrt(707 * 707 + 468 * 468)
totaldisMM = 18440.4
BallD = 75.0
FocusD = 9.5 * totaldisMM / BallD
tmpDis = totaldisMM
remainframe = 0
fcc = cv2.VideoWriter_fourcc(*'XVID')
video = cv2.VideoWriter(UPLOAD_FOLDER + '/' + out_file, fcc, fps,
(1920, 1080))
#Point prev;
frameno = drawflag = curframe = StartFrameflag = 0
frameCounter = 1
speeds = [int]
while True:
precTick = ticks
ticks = cv2.getTickCount()
dT = (ticks - precTick) / cv2.getTickFrequency()
#seconds
#frame1 = np.zeros((1920, 1080,3), np.uint8)
(grabbed, frame) = cap.read()
length = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
if grabbed == False:
break
frame = imutils.resize(frame, width=1920, height=1080)
res = frame.copy()
if found:
#>>>> Matrix A
kf.transitionMatrix[2, 0] = dT
kf.transitionMatrix[3, 1] = dT
#<<<< Matrix A
state = kf.predict()
width = state[4]
height = state[5]
x = int(state[0] - width / 2)
y = int(state[1] - height / 2)
predRect = (x, y, width, height)
center = (state[0], state[1])
cv2.circle(res, center, 2, (0, 255, 0), -1)
if frameno > 50 and x + width <= 1000:
drawflag = 1
cou = 27
balls = []
ballsBox = []
tmpcols = 0
tmprows = 0
tmpLoc = (0, 0)
maxTemp = 0.0
#---------image2------------------------
image2 = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ttcou = 0
for ii in range(pos, cou):
tm = ii + 1
path = 'patterns/pattern'
tmpch = str(tm)
temppattern = cv2.imread(path + tmpch + ".png")
temppattern = cv2.cvtColor(temppattern, cv2.COLOR_BGR2GRAY)
match_method = cv2.TM_CCORR_NORMED
res1 = cv2.matchTemplate(image2, temppattern, match_method)
minVal, maxVal, minLoc, maxLoc = cv2.minMaxLoc(res1)
if match_method == cv2.TM_SQDIFF or match_method == cv2.TM_SQDIFF_NORMED:
matchLoc = minLoc
else:
matchLoc = maxLoc
if maxVal >= 0.97 and maxVal >= maxTemp and matchLoc[
0] > maskx and matchLoc[1] > masky and matchLoc[0] < 1000:
if ii != 0 and tmpno == 0:
break
if ii == 0 and tmpno == 0:
tmpno = 1
maskx = matchLoc[0] - 100
masky = matchLoc[1] - 10
curframe = 1
if curframe > 0 and ii - curframe > 0 and ii < curframe - 1:
break
curframe = ii + 1
maxTemp = maxVal
tmpLoc = matchLoc
ttcou += 1
tmprows, tmpcols = temppattern.shape
if ttcou > 0:
bBox = [tmpLoc[0], tmpLoc[1], tmpcols, tmprows]
ttcou = 0
tmp = []
tmp.append([tmpLoc[0], tmpLoc[1]])
balls.append(tmp)
ballsBox.append(bBox)
#double deltadis;
deltaspeed = 0
if len(ballsBox) != 0:
deltadis = BallD * FocusD / ballsBox[0][2] #width
if tmpDis >= deltadis:
deltaspeed = tmpDis - deltadis
if TotalFrame == 0 and ballsBox[0][2] < 15: #width
StartFrameflag = 1
TotalFrame += 1
if TotalFrame == 0 and ballsBox[0][2] >= 15: #width
balls.clear()
ballsBox.clear()
if StartFrameflag == 1:
TotalVelocity = deltaspeed * 3.6 * fps / 1000 / TotalFrame
Avgvelocity += TotalVelocity
prevBox.append(ballsBox[0])
TotalFrame += 1
else:
balls.clear()
ballsBox.clear()
if TotalFrame == 0:
speed = 0
else:
speed = Avgvelocity / TotalFrame
sstr = '( Average Velocity= ' + str(int(speed)) + ')'
if TotalVelocity >= 60 and TotalVelocity <= 80:
speeds.append(int(TotalVelocity))
if frameCounter >= (length - 1):
tSpeed = 0
for s in range(1, len(speeds)):
tSpeed = tSpeed + speeds[s]
eSpeed = tSpeed / (len(speeds) - 1)
print('speed!!!:' + str(int(eSpeed)))
sstr1 = '( Velocity= ' + str(int(eSpeed)) + ')'
cv2.putText(res, sstr1, (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 2.0,
(255, 0, 0), 2)
# Detection result
for i in range(len(balls)):
cv2.rectangle(res, (ballsBox[i][0], ballsBox[i][1]),
(ballsBox[i][0] + ballsBox[i][2],
ballsBox[i][1] + ballsBox[i][3]), (0, 255, 0), 2)
#cv::Point center;
x = int(ballsBox[i][0] + ballsBox[i][2] / 2)
y = int(ballsBox[i][1] + ballsBox[i][3] / 2)
cv2.circle(res, (x, y), 2, (20, 150, 20), -1)
#stringstream sstr;
sstr = '(' + str(x) + ',' + str(y) + ')'
cv2.putText(res, sstr, (x + 3, y - 3), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (20, 150, 20), 2)
if len(balls) == 0:
notFoundCount += 1
if notFoundCount >= 100:
found = False
else:
notFoundCount = 0
meas[0] = ballsBox[0][0] + ballsBox[0][2] / 2
meas[1] = ballsBox[0][1] + ballsBox[0][3] / 2
meas[2] = ballsBox[0][2]
meas[3] = ballsBox[0][3]
if not (found): #First detection!
# Initialization
kf.errorCovPre[0, 0] = 1
kf.errorCovPre[1, 1] = 1
kf.errorCovPre[2, 2] = 1
kf.errorCovPre[3, 3] = 1
kf.errorCovPre[4, 4] = 1
kf.errorCovPre[5, 5] = 1
state[0] = meas[0]
state[1] = meas[1]
state[2] = 0
state[3] = 0
state[4] = meas[2]
state[5] = meas[3]
# Initialization
kf.statePost = state
found = True
else:
kf.correct(meas) #Kalman Correction
frameCounter = frameCounter + 1
video.write(res)
video.release()
cap.release()
cv2.destroyAllWindows()
def callback(self, data):
# Convert the image from OpenCV to ROS format
#self.fps = FPS().start()
# Filter requirements.
order = 6
fs = 30.0 # sample rate, Hz
cutoff = 3.3 # desired cutoff frequency of the filter, Hz
# Get the filter coefficients so we can check its frequency response.
b, a = butter_lowpass(cutoff, fs, order)
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
"""
Fancy code here.
"""
global mtx, dist
h, w = cv_image.shape[:2]
mtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
blob = cv2.dnn.blobFromImage(cv_image,
1.5,
size=(300, 300),
swapRB=True,
crop=True)
self.net.setInput(blob)
#Prediction of network
detections = self.net.forward()
cols = 300
rows = 300
for i in range(detections.shape[2]):
confidence = detections[0, 0, i, 2] #Confidence of prediction
if confidence > 0.8: # Filter prediction
class_id = int(detections[0, 0, i, 1]) # Class label
# Object location
xLeftBottom = int(detections[0, 0, i, 3] * cols)
yLeftBottom = int(detections[0, 0, i, 4] * rows)
xRightTop = int(detections[0, 0, i, 5] * cols)
yRightTop = int(detections[0, 0, i, 6] * rows)
# Factor for scale to original size of frame
heightFactor = cv_image.shape[0] / 300.0
widthFactor = cv_image.shape[1] / 300.0
# Scale object detection to frame
xLeftBottom = int(widthFactor * xLeftBottom)
yLeftBottom = int(heightFactor * yLeftBottom)
xRightTop = int(widthFactor * xRightTop)
yRightTop = int(heightFactor * yRightTop)
roi = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
roi = roi[yLeftBottom:yRightTop, xLeftBottom:xRightTop]
# Draw location of object
cv2.rectangle(cv_image, (xLeftBottom, yLeftBottom),
(xRightTop, yRightTop), (0, 255, 0))
if class_id in self.classNames:
label = self.classNames[class_id] + ": " + str(confidence)
labelSize, baseLine = cv2.getTextSize(
label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)
yLeftBottom = max(yLeftBottom, labelSize[1])
cv2.rectangle(
cv_image, (xLeftBottom, yLeftBottom - labelSize[1]),
(xLeftBottom + labelSize[0], yLeftBottom + baseLine),
(0, 255, 0), cv2.FILLED)
cv2.putText(cv_image, label, (xLeftBottom, yLeftBottom),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255))
# Use the box of the detected object for the translation estimation.
''''
image_points =np.array([(xRightTop, yRightTop),
(xRightTop-(xRightTop-xLeftBottom), yRightTop),
(xLeftBottom+(xRightTop-xLeftBottom), yLeftBottom),
(xLeftBottom, yLeftBottom)
],dtype=np.float32)
'''
#_,rmat,tmat= cv2.solvePnPRansac(self.model_points,image_points, mtx,dist,flags=6,iterationsCount = 500,reprojectionError = 2.0,confidence = 0.99)[:3]
#success,rmat,tmat= cv2.solvePnPRansac(self.model_points,image_points, mtx,dist,flags=6)[:3]#,iterationsCount = 500,reprojectionError = 2.0,confidence = 0.95)[:3]
#Then check ORB matching for the orientation.
kp_model, desc_model = self.unpickle_keypoints(
self.keypoints_database[class_id - 1])
# print(desc_model)
if str(class_id) not in self.dict_ekf.keys():
#Kalman Filter for the pose
self.dict_ekf[str(class_id)] = cv2.KalmanFilter(6, 6)
trans_mat = np.identity(6, np.float32)
self.dict_ekf[str(
class_id)].transitionMatrix = trans_mat
self.dict_ekf[str(
class_id)].measurementMatrix = trans_mat
self.dict_ekf[str(
class_id)].processNoiseCov = cv2.setIdentity(
self.dict_ekf[str(class_id)].processNoiseCov,
1e-1) #4
self.dict_ekf[str(
class_id)].measurementNoiseCov = cv2.setIdentity(
self.dict_ekf[str(
class_id)].measurementNoiseCov, 1e-1) #2
self.dict_ekf[str(
class_id)].errorCovPost = cv2.setIdentity(
self.dict_ekf[str(
class_id)].errorCovPost) #, 1)
kp_image, desc_image = self.orb.detectAndCompute(roi,
mask=None)
try:
matches = self.flann.knnMatch(desc_model,
desc_image,
k=2)
matchesMask = [[0, 0] for i in range(len(matches))]
#-- Filter matches using the Lowe's ratio test
good_matches = []
for i, (m, n) in enumerate(matches):
if m.distance < 0.7 * n.distance:
matchesMask[i] = [1, 0]
good_matches.append(m)
draw_params = dict(matchColor=(0, 0, 255),
singlePointColor=(255, 0, 0),
matchesMask=matchesMask,
flags=cv2.DrawMatchesFlags_DEFAULT)
matches = good_matches #sorted(good_matches, key = lambda x: x.distance)
list_kpimage = []
list_kpmodel = []
for mat in matches:
img1_idx = mat.queryIdx
img2_idx = mat.trainIdx
(x1, y1) = kp_model[img1_idx].pt
(x2, y2) = kp_image[img2_idx].pt
# Append to each list
list_kpmodel.append((x1, y1, 0.0))
list_kpimage.append((x2, y2))
except:
continue
image_points2 = np.array(list_kpimage, dtype=np.float32)
model_points2 = np.array(list_kpmodel, dtype=np.float32)
try:
if not (image_points2.size == 0
and model_points2.size == 0):
_, rmat, tmat = cv2.solvePnPRansac(
model_points2,
image_points2,
mtx,
dist,
flags=6,
iterationsCount=500,
reprojectionError=1.0,
confidence=0.99)[:3]
if np.linalg.norm(tmat) < 1000:
measurements = np.array(
tmat, np.float32) #.reshape(1,-1)
prot = np.array(rmat,
np.float32) #.reshape(1,-1)
measurements = np.concatenate(
(measurements, prot))
prediction = self.dict_ekf[str(
class_id)].predict()
estimated = self.dict_ekf[str(
class_id)].correct(
measurements.reshape(-1, 1))
rmat2 = np.array(estimated[3:6])
#Update the Kalman filter
RotMat = cv2.Rodrigues(rmat2)[0]
pose = np.hstack((RotMat, tmat))
pose = np.vstack((pose, [0, 0, 0, 1]))
_, invpose = cv2.invert(pose)
rot = invpose[0:3, 0:3]
#print('Pose\n',invpose[0:3,3:4])
tmat = invpose[0:3, 3:4]
rmat = cv2.Rodrigues(rot)[0]
if np.linalg.norm(tmat) < 10000:
transform = TransformStamped()
transform.header.frame_id = 'cf1/camera_link'
transform.child_frame_id = 'sign/' + self.classNames[
class_id]
transform.header.stamp = rospy.Time.now()
transform.transform.translation.x = round(
butter_lowpass_filter(
tmat[2], cutoff, fs, order),
3) + 0.2 #tmat[2]/950#+0.2
transform.transform.translation.y = round(
butter_lowpass_filter(
tmat[1], cutoff, fs, order),
3) - 0.1 #tmat[1]/950#-0.1
transform.transform.translation.z = round(
butter_lowpass_filter(
tmat[0], cutoff, fs, order), 3) * 1.5
#a=quaternion_from_euler(rmat[0]-math.pi,rmat[1],rmat[2]+math.pi)
a = quaternion_from_euler(
rmat[0] * math.pi / 180,
rmat[1] * math.pi / 180 + math.pi,
rmat[2] * math.pi / 180 - math.pi / 2)
transform.transform.rotation.x = a[0]
transform.transform.rotation.y = a[1]
transform.transform.rotation.z = a[2]
transform.transform.rotation.w = a[3]
#print(transform)
if transform.transform.translation.z > 0.15:
tf_bc.sendTransform(transform)
except:
pass
"""
Fancy code finishes here.
"""
# Publish the image
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(
cv_image, "bgr8")) #np.asarray(thresh_img),"mono8"))
except CvBridgeError as e:
print(e)
def __init__(self):
'''
Initialize Kalman filter for tracking one person
In openCV Kalman filter is initialized using
KalmanFilter KF(nmbStateVars, nmbMeasts, nmbControlInputs, type)
Here we will use the state of our kalman filter which consists of
6 elments (x,y,w,vx,vy,vw)
where
x,y are the coord of the top left corner of the bounding box
w is the width of the detected person
vx,vy are the x and y velocities of the top left corner
vw is the rate of change of the width w.r.to time
the height of the bounding box is not considered as the part
of the state because it is assumed to be equal to twice the width
'''
nmbStateVars = 6
# the measurement matrix has 3 elements (x,y, w)
nmbMeasts = 3
# there are no control inputs , as we will this class
# for tracking in video file and there is no way to change
# the affect of the state of the person walking
nmbControlInputs = 0
# the type is float32
self.KF = cv2.KalmanFilter(nmbStateVars, nmbMeasts, nmbControlInputs)
'''
bz our motion model is
x = x + vx * dt
y = y + vy * dt
w = y + vw * dt
here for simplicity we assume zero accelaration, therefore
vx = vx
vy = vy
vw = vw
Our transition matrix is of the following form
[
1, 0, 0, dt, 0, 0,
0, 1, 0, 0, dt, 0
0, 0, 1, 0, 0, dt
0, 0, 0, 1, 0, 0
0, 0, 0, 0, 1, 0
0, 0, 0, 0, 0, 1
]
which is 6x6 identity matrix and later we add dt in a loop
'''
self.KF.transitionMatrix = cv2.setIdentity(self.KF.transitionMatrix)
'''
our measurment matrix is the following form
bz we are only detecting x, y, and w, the measurment matrix
picks only these quantities and leaves vx, vy and vw
[
1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0
0, 0, 1, 0, 0, 0
]
'''
self.KF.measurementMatrix = cv2.setIdentity(self.KF.measurementMatrix)
# initializing variables to be used of tracking
# initialize var to store detected x, y, w
self.measurement = np.zeros((3, 1), dtype=np.float32)
# initialize vars to store tracked object
self.objTracked = np.zeros((4, 1), dtype=np.float32)
# vars used to store the results of the predict and update
self.updatedMeasts = np.zeros((3, 1), dtype=np.float32)
self.predictedMeasts = np.zeros((6, 1), dtype=np.float32)
# boolean for the indication of whether the measurments are updated
self.meastsWasUpdated = False
