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=500)
self.nframe = deque(maxlen=500)
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))
def genMainPoints(self, fps=25):
main_points = np.ones(9) * -1
main_points[0] = self.trackId
if len(self.trace) == 0:
return main_points
main_points[1] = self.nframe[0] / fps
if (40 > self.trace[0][0, 0]) and (20 < self.trace[0][0, 1]):
main_points[2] = 0
main_points[3] = 1
for i in range(len(self.nframe)):
if self.trace[i][0, 0] > 40:
main_points[7] = self.nframe[i] / fps
break
for i in range(len(self.nframe)):
if self.trace[i][0, 0] > 200:
main_points[8] = self.nframe[i] / fps
break
elif (340 < self.trace[0][0, 0]) and (20 < self.trace[0][0, 1]):
main_points[2] = 3
main_points[3] = 2
for i in range(len(self.nframe)):
if self.trace[i][0, 0] < 200:
main_points[7] = self.nframe[i] / fps
break
for i in range(len(self.nframe)):
if self.trace[i][0, 0] < 40:
main_points[8] = self.nframe[i] / fps
break
elif (340 > self.trace[0][0, 0] > 200) and (20 > self.trace[0][0, 1]):
main_points[2] = 5
main_points[3] = 4
for i in range(len(self.nframe)):
if self.trace[i][0, 0] < 200:
main_points[7] = self.nframe[i] / fps
break
for i in range(len(self.nframe)):
if self.trace[i][0, 0] < 40:
main_points[8] = self.nframe[i] / fps
break
main_points[4] = self.nframe[-1] / fps
if (40 > self.trace[-1][0, 0]) and (20 < self.trace[-1][0, 1]):
main_points[5] = 1
main_points[6] = 0
elif (340 < self.trace[-1][0, 0]) and (20 < self.trace[-1][0, 1]):
main_points[5] = 2
main_points[6] = 3
elif (340 > self.trace[-1][0, 0] > 200) and (20 > self.trace[-1][0,
1]):
main_points[5] = 4
main_points[6] = 5
return main_points
class Tracker:
LIG_THR_EVERY_FRAMES = 15
def __init__(self, initialPosition, initialWidth, initialHeight, frame,
parametersNew):
#########################################
#########################################
self.initFrame = frame
self.initPos = initialPosition
self.initW = initialWidth
self.initH = initialHeight
self.KM = KalmanFilter()
self.MF = MaskingFilter()
self.KM.setStatePost(
np.array([initialPosition[0], initialPosition[1], 0.,
0.]).reshape(4, 1))
self.selectionWidth = initialWidth
self.selectionHeight = initialHeight
self.prevFrameGray = None
self.frameCounter = 0 #Shape = {tuple}: (x, 1, 2)
#Por ejemplo: [[[x1 y1]]\n\n [[x2 y2]]\n\n [[x3 y3]]]
#es decir una matriz de x filas y 1 columna, donde cada elemento
#de la unica columna es una coordenada [x y].
if self.MF.mask is not self.MF.maskingType["FILTER_OFF"]:
self.MF.calculateNewMask(
frame, frame[int(initialPosition[1] -
initialHeight / 2):int(initialPosition[1] +
initialHeight / 2),
int(initialPosition[0] -
initialWidth / 2):int(initialPosition[0] +
initialWidth / 2)])
frame = self.MF.filterFrame(frame)
self.prevFrameGray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
self.SC = Searcher(self.initFrame, initialHeight, initialWidth,
initialPosition[0], initialPosition[1],
cv.cvtColor(frame, cv.COLOR_BGR2GRAY))
self.SC.features, self.SC.trackingError = self.SC.ST.recalculateFeatures(
self.prevFrameGray[int(initialPosition[1] -
initialHeight / 2):int(initialPosition[1] +
initialHeight / 2),
int(initialPosition[0] -
initialWidth / 2):int(initialPosition[0] +
initialWidth / 2)])
self.SC.features = self.SC.featureTranslate(
initialPosition[0] - initialWidth / 2,
initialPosition[1] - initialHeight / 2, self.SC.features)
self.SC.LK.prevFeatures = self.SC.features
#OPTIMIZACIÓN
self.bins_var = 1
self.kernel_blur_var = 1
self.mask_blur_var = 1
self.low_pth_var = 200
def getTrackingError(self):
return self.SC.trackingError
def setFilter(self, filterType):
if filterType in self.MF.maskingType.keys():
self.MF.mask = self.MF.maskingType[filterType]
else:
print("Wrong filter type")
def featureTranslate(self, x, y, features):
if features is None:
return None
for i in range(features.shape[0]):
features[i][0][0] += x
features[i][0][1] += y
return features
def update(self, frame):
self.frameCounter += 1
self.KM.predict()
realframe = frame
if self.MF.mask is self.MF.maskingType["FILTER_LAB"]:
if self.frameCounter != 0 and self.frameCounter % self.MF.CIELabRecalculationNumber == 0 and self.MF.labPeriodicRecalculations is True and self.SC.trackingError is False:
vx, vy = self.getEstimatedVelocity()
if np.abs(vx) < 5 and np.abs(vy) < 5:
medx, medy = np.median(
self.SC.features[:, 0,
0]), np.median(self.SC.features[:, 0,
1])
std = np.sqrt((np.std(self.SC.features[:, 0, 0]))**2 +
(np.std(self.SC.features[:, 0, 1]))**2)
# calculate mean and std of features
mask = (self.SC.features[:, 0, 0] <
medx + self.SC.stdMultiplier * std + 0.1) & (
self.SC.features[:, 0, 0] >
medx - self.SC.stdMultiplier * std - 0.1) & (
self.SC.features[:, 0, 1] <
medy + self.SC.stdMultiplier * std +
0.1) & (self.SC.features[:, 0, 1] > medy -
self.SC.stdMultiplier * std - 0.1)
self.SC.features = self.SC.features[mask]
# remove outliers.
medx, medy = np.median(
self.SC.features[:, 0,
0]), np.median(self.SC.features[:, 0,
1])
if (~np.isnan(medx)) and (~np.isnan(medy)):
self.MF.calculateNewMask(
frame,
frame[int(medy - self.selectionHeight /
2):int(medy + self.selectionHeight / 2),
int(medx - self.selectionWidth /
2):int(medx + self.selectionWidth / 2)])
frame = self.MF.filterFrame(frame)
elif self.MF.mask is self.MF.maskingType["FILTER_CSHIFT"]:
frame = self.MF.filterFrame(frame)
#TINCHO
#Tacking error?
if self.SC.trackingError is True:
if self.SC.missAlgorithm == self.SC.missAlgorithmD["ST"]:
x, y = self.SC.searchMissing(self.KM.statePost[0][0],
self.KM.statePost[1][0], frame,
frame)
elif self.SC.missAlgorithm == self.SC.missAlgorithmD["CORR"]:
x, y = self.SC.searchMissing(self.KM.statePost[0][0],
self.KM.statePost[1][0],
realframe, frame)
if self.SC.trackingError is False:
self.KM.correct(x, y)
else:
x, y = self.SC.search(self.frameCounter, realframe, frame)
if self.SC.trackingError is False:
self.KM.correct(x, y)
def changeSettings(self, parametersNew):
self.KM.dt = parametersNew[0] #kalman_ptm
self.KM.PROCESS_COV = parametersNew[1] #kalman_pc
self.KM.MEAS_NOISE_COV = parametersNew[2] #kalman_mc
self.SC.LK.lkMaxLevel = int(parametersNew[3]) #lk_mr
if parametersNew[4] is False: #Color Filter OnOff
self.MF.mask = self.MF.maskingType["FILTER_OFF"]
self.MF.LSemiAmp = parametersNew[5] #colorFilter_LihtThr
self.MF.aSemiAmp = parametersNew[6] #colorFilter_a
self.MF.bSemiAmp = parametersNew[7] #colorFilter_b
if parametersNew[20] == True and parametersNew[19] == False:
self.SC.missAlgorithm = self.SC.missAlgorithmD["ST"]
elif parametersNew[20] == False and parametersNew[19] == True:
self.SC.missAlgorithm = self.SC.missAlgorithmD["CORR"]
if parametersNew[22] == True and parametersNew[21] == False:
self.SC.recalcAlgorithm = self.SC.recalcAlgorithmD["ST"]
elif parametersNew[22] == False and parametersNew[21] == True:
self.SC.recalcAlgorithm = self.SC.recalcAlgorithmD["CORR"]
self.SC.MASKCONDITION = parametersNew[23]
#= parametersNew[8] #Light R OnOff
#= parametersNew[9] #ligtRec_x)
#= parametersNew[10] #ligtRec_maxT
#= parametersNew[11] #Cam shift On/Off
self.SC.ST.maxcorners = int(parametersNew[13]) #shit_MaxFeat
self.SC.ST.qLevel = parametersNew[14] #shit_FeatQual
self.SC.ST.minDist = parametersNew[15] #shit_MinFeat
#= parametersNew[16] #ShiTomasiOn/ Off
self.SC.ST.frameRecalculationNumber = parametersNew[16] #shit_SPix
#self.MF.mask = self.MF.maskingType[parametersNew[??]] #MENSAJE PARA TOMI: tiene que ser un string parametersNew[??] fijate en la clase
self.MF.hist_filter.set_bins(parametersNew[9])
self.MF.hist_filter.set_mask_blur(parametersNew[10])
self.MF.hist_filter.set_kernel_blur(parametersNew[11])
self.MF.hist_filter.set_low_pth(parametersNew[12])
self.MF.ksize = parametersNew[24]
if int(self.MF.ksize) % 2 == 0:
self.MF.ksize = int(self.MF.ksize) + 1
else:
self.MF.ksize = int(self.MF.ksize)
self.MF.updateMaskFromSettings()
self.KM.updateParams()
def updateKalman(self, kalman_ptm, kalman_pc, kalman_mc):
self.KM.dt = kalman_ptm
self.KM.PROCESS_COV = kalman_pc
self.KM.MEAS_NOISE_COV = kalman_mc
self.KM.updateParams()
def updateLK(self, lk_mr):
self.SC.LK.lkMaxLevel = lk_mr
def updateColorFilter(self, CFPropOnOff, LihtThr, a, b, maskBlur_lab):
if CFPropOnOff is False: # Color Filter OnOff
self.MF.mask = self.MF.maskingType["FILTER_OFF"]
self.MF.LSemiAmp = LihtThr
self.MF.aSemiAmp = a
self.MF.bSemiAmp = b
self.MF.ksize = maskBlur_lab
if int(self.MF.ksize) % 2 == 0:
self.MF.ksize = int(self.MF.ksize) + 1
else:
self.MF.ksize = int(self.MF.ksize)
self.MF.updateMaskFromSettings()
def updateCamShift(self, CFCamShiftOnOff, bins, mb, sb, lbpt):
self.MF.hist_filter.set_bins(bins)
self.MF.hist_filter.set_mask_blur(mb)
self.MF.hist_filter.set_kernel_blur(sb)
self.MF.hist_filter.set_low_pth(lbpt)
self.MF.updateMaskFromSettings()
def updateShiT(self, MaxFeat, FeatQual, MinFeat, Rec, ShiTPropOnOff, SPix):
self.SC.ST.maxcorners = int(MaxFeat)
self.SC.ST.qLevel = FeatQual
self.SC.ST.minDist = MinFeat
self.SC.ST.frameRecalculationNumber = Rec
def updateMissinSearch(self, missCorr, missST, recCor, recST):
if missST == True and missCorr == False:
self.SC.missAlgorithm = self.SC.missAlgorithmD["ST"]
self.SC.searchHeight = self.initH
self.SC.searchWidth = self.initW
elif missST == False and missCorr == True:
self.SC.missAlgorithm = self.SC.missAlgorithmD["CORR"]
self.SC.searchHeight = self.initH
self.SC.searchWidth = self.initW
if recST == True and recCor == False:
self.SC.recalcAlgorithm = self.SC.recalcAlgorithmD["ST"]
elif recST == False and recCor == True:
self.SC.recalcAlgorithm = self.SC.recalcAlgorithmD["CORR"]
def updateMaskCond(self, maskCondition):
self.SC.MASKCONDITION = maskCondition
def updateBGR(self, color):
self.MF.calculateNewMask(None, None, True, color)
self.MF.updateMaskFromSettings()
def getFilteredFrame(self):
return self.MF.filteredFrame
def getCorrFrame(self):
return self.SC.corr_out
def getEstimatedPosition(self):
return self.KM.statePost[0][0], self.KM.statePost[1][0]
def getEstimatedVelocity(self):
return self.KM.statePost[2][0], self.KM.statePost[3][0]
def getTrajectory(self):
return self.KM.trajectory
def costChangeParamsLAB(self, x):
self.MF.LSemiAmp = x[0]
self.MF.aSemiAmp = x[1]
self.MF.bSemiAmp = x[2]
self.MF.updateMaskFromSettings()
testFrame = self.MF.filterFrame(self.initFrame)
countTotal = np.count_nonzero(testFrame)
countInside = np.count_nonzero(testFrame[
int(self.initPos[1] -
self.selectionHeight / 2):int(self.initPos[1] +
self.selectionHeight / 2),
int(self.initPos[0] -
self.selectionWidth / 2):int(self.initPos[0] +
self.selectionWidth / 2)])
countOutside = countTotal - countInside
# print(countOutside-countInside)
# return countOutside*(1/5) - countInside*(4/5)
return countOutside - countInside
def calculate_optimal_params(self):
if self.MF.mask is self.MF.maskingType["FILTER_LAB"]:
self.optimize()
params = {
"l": self.MF.LSemiAmp,
"a": self.MF.aSemiAmp,
"b": self.MF.bSemiAmp,
"blur": self.MF.ksize
}
return params
elif self.MF.mask is self.MF.maskingType["FILTER_CSHIFT"]:
for i in range(3):
self.optimize()
params = {
"bins": self.MF.hist_filter.bins_opti,
"mask_blur": self.MF.hist_filter.mask_blur_size_opti,
"kernel_blur": self.MF.hist_filter.kernel_blur_size_opti,
"low_pth": self.MF.hist_filter.low_pth_opti
}
return params
def calculate_cost(self):
test_frame = self.MF.filterFrame(self.initFrame)
count_total = np.count_nonzero(test_frame)
count_inside = np.count_nonzero(test_frame[
int(self.initPos[1] -
self.selectionHeight / 2):int(self.initPos[1] +
self.selectionHeight / 2),
int(self.initPos[0] -
self.selectionWidth / 2):int(self.initPos[0] +
self.selectionWidth / 2)])
count_outside = count_total - count_inside
return count_outside - count_inside
# return count_outside**4 - count_inside**3 #PREFERIMOS QUE ESTE VACIO AFUERA
def optimize(self):
if self.MF.mask is self.MF.maskingType["FILTER_LAB"]:
for j in range(3):
best_L = [self.MF.LSemiAmp]
best_cost = self.calculate_cost()
for i in range(10, 150):
self.MF.LSemiAmp = i
self.MF.updateMaskFromSettings()
cost = self.calculate_cost()
if cost < best_cost:
best_L.append(i)
best_cost = cost
self.MF.LSemiAmp = best_L[-1]
self.MF.updateMaskFromSettings()
best_mask_blur = [self.MF.ksize]
# best_cost = 0
for i in [1, 3, 5, 7, 9, 11, 13, 15, 17, 19]:
self.MF.ksize = i
cost = self.calculate_cost()
if cost < best_cost:
best_mask_blur.append(i)
best_cost = cost
self.MF.ksize = best_mask_blur[-1]
best_a = [self.MF.aSemiAmp]
for i in range(5, 100):
self.MF.aSemiAmp = i
self.MF.updateMaskFromSettings()
cost = self.calculate_cost()
if cost < best_cost:
best_a.append(i)
best_cost = cost
self.MF.aSemiAmp = best_a[-1]
self.MF.updateMaskFromSettings()
best_b = [self.MF.bSemiAmp]
for i in range(5, 100):
self.MF.bSemiAmp = i
self.MF.updateMaskFromSettings()
cost = self.calculate_cost()
if cost < best_cost:
best_b.append(i)
best_cost = cost
self.MF.bSemiAmp = best_b[-1]
self.MF.updateMaskFromSettings()
self.MF.LSemiAmp = best_L[-1]
self.MF.aSemiAmp = best_a[-1]
self.MF.bSemiAmp = best_b[-1]
self.MF.ksize = best_mask_blur[-1]
self.MF.updateMaskFromSettings()
x_bounds = [(0, 150), (0, 150), (0, 150)]
x0 = np.array(
[self.MF.LSemiAmp, self.MF.aSemiAmp, self.MF.bSemiAmp])
# res = optimize.least_squares(self.costChangeParamsLAB,x0=x0,bounds=[(0,0,0),(150,150,150)],ftol=1000)
#res = optimize.minimize(self.costChangeParamsLAB, x0=x0, bounds=x_bounds,method="Powell")
res = optimize.minimize(self.costChangeParamsLAB,
x0=x0,
method="Powell")
self.MF.LSemiAmp = res.x[0]
self.MF.aSemiAmp = res.x[1]
self.MF.bSemiAmp = res.x[2]
self.MF.updateMaskFromSettings()
else:
best_bin = [self.MF.hist_filter.bins]
best_cost = self.calculate_cost()
for i in range(1, 200):
self.MF.hist_filter.set_bins(i)
self.MF.updateMaskFromSettings()
cost = self.calculate_cost()
if cost < best_cost:
best_bin.append(i)
best_cost = cost
self.MF.hist_filter.set_bins(best_bin[-1])
self.MF.updateMaskFromSettings()
best_mask_blur = [self.MF.hist_filter.mask_blur_size]
# best_cost = 0
for i in [1, 3, 5, 7, 9, 11, 13, 15, 17, 19]:
self.MF.hist_filter.set_mask_blur(i)
cost = self.calculate_cost()
if cost < best_cost:
best_mask_blur.append(i)
best_cost = cost
self.MF.hist_filter.set_mask_blur(best_mask_blur[-1])
best_kernel_blur = [self.MF.hist_filter.kernel_blur_size]
for i in [1, 3, 5, 7, 9, 11, 13, 15, 17, 19]:
self.MF.hist_filter.set_kernel_blur(i)
self.MF.updateMaskFromSettings()
cost = self.calculate_cost()
if cost < best_cost:
best_kernel_blur.append(i)
best_cost = cost
self.MF.hist_filter.set_kernel_blur(best_kernel_blur[-1])
self.MF.updateMaskFromSettings()
best_low_pth = [self.MF.hist_filter.low_pth]
self.MF.hist_filter.set_bins(best_bin[-1])
self.MF.hist_filter.set_mask_blur(best_mask_blur[-1])
self.MF.hist_filter.set_kernel_blur(best_kernel_blur[-1])
self.MF.hist_filter.set_low_pth(best_low_pth[-1])
self.MF.hist_filter.bins_opti = best_bin[-1]
self.MF.hist_filter.mask_blur_size_opti = best_mask_blur[-1]
self.MF.hist_filter.kernel_blur_size_opti = best_kernel_blur[-1]
self.MF.hist_filter.low_pth_opti = best_low_pth[-1]
# self.MF.hist_filter.set_low_pth(best_low_pth[-1])
self.MF.updateMaskFromSettings()
def colorKernelChange(self, bgr):
b = bgr[0]
g = bgr[1]
r = bgr[2]
def showSearchArea(self):
if self.SC.missAlgorithm == self.SC.missAlgorithmD["ST"]:
return True
else:
return False
def main():
face_landmark_path = './shape_predictor_68_face_landmarks.dat'
# flags/initializations to be set
socket_connect = 1 # set to 0 if we are testing the code locally on the computer with only the realsense tracking.
simplified_calib_flag = 0 # this is set to 1 if we want to do one-time calibration
kalman_flag = 1
skip_frame = 3
detected_pre = []
ArrToSendPrev = np.array([1, 2, 3, 4, 1, 2, 3, 4, 12, 3, 4, 4, 1, 1, 1, 1])
img_idx = 0 # img_idx keeps track of image index (frame #).
img_sent = 0
reinit_thresh = 10
skip_frame_reinit = 120 #after every 150 frames, reinitialize detection
skip_frame_send = 4
arucoposeflag = 1
N_samples_calib = 10 # number of samples for computing the calibration matrix using homography
# kalman filter initialization
stateMatrix = np.zeros((12, 1), np.float32) # [x, y, delta_x, delta_y]
estimateCovariance = np.eye(stateMatrix.shape[0])
transitionMatrix = np.array([[1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]],
np.float32)
processNoiseCov = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]],
np.float32) * 0.001
measurementStateMatrix = np.zeros((6, 1), np.float32)
observationMatrix = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]],
np.float32)
measurementNoiseCov = 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]],
np.float32) * 1
kalman = KalmanFilter(X=stateMatrix,
P=estimateCovariance,
F=transitionMatrix,
Q=processNoiseCov,
Z=measurementStateMatrix,
H=observationMatrix,
R=measurementNoiseCov)
# next create a socket object
if socket_connect == 1:
s = socket.socket()
print("Socket successfully created")
# reserve a port on your computer in our case it is 2020 but it can be anything
port = 2020
s.bind(('', port))
print("socket binded to %s" % (port))
# put the socket into listening mode
s.listen(5)
print("socket is listening")
c, addr = s.accept()
print('got connection from ', addr)
if socket_connect == 1 and simplified_calib_flag == 0:
arucoinstance = arucocalibclass()
ReturnFlag = 1
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
marker_len = 0.0645
MLRSTr = arucoinstance.startcamerastreaming(c, ReturnFlag, marker_len,
aruco_dict,
N_samples_calib)
print(MLRSTr)
elif socket_connect == 1 and simplified_calib_flag == 1:
T_M2_RS = np.array([
-1.0001641, 0.00756584, 0.00479072, 0.03984956, -0.00774137,
-0.99988126, -0.03246199, -0.01359556, 0.00453644, -0.03251681,
0.99971441, -0.00428408, 0., 0., 0., 1.
])
T_M2_RS = T_M2_RS.reshape(4, 4)
MLRSTr = simplified_calib(T_M2_RS, c)
else:
MLRSTr = np.array((1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1))
MLRSTr = MLRSTr.reshape(4, 4)
print(MLRSTr)
# end socket ######
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
profile = pipeline.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
#load cascade classifier training file for lbpcascade
lbp_face_cascade = cv2.CascadeClassifier(
"./haarcascade_frontalface_alt2.xml")
facemark = cv2.face.createFacemarkLBF()
facemark.loadModel('./lbfmodel.yaml')
print('Start detecting pose ...')
while True:
# get video frame
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.get_color_frame()
aligned_depth_frame = aligned_frames.get_depth_frame()
if not aligned_depth_frame or not color_frame:
continue
# Intrinsics & Extrinsics of Realsense
depth_intrin = profile.get_stream(
rs.stream.depth).as_video_stream_profile().get_intrinsics()
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
depth_to_color_extrin = aligned_depth_frame.profile.get_extrinsics_to(
color_frame.profile)
mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy],
[0, 0, 1]])
dist = np.array(intr.coeffs)
input_img = np.asanyarray(color_frame.get_data())
img_idx = img_idx + 1
img_h, img_w, _ = np.shape(input_img)
# process the first frame or every frame after skip_frame number of frames
if img_idx == 1 or img_idx % skip_frame == 0:
# convert image to grayscale
gray_img = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# detect faces using LBP detector
faces = lbp_face_cascade.detectMultiScale(gray_img,
scaleFactor=1.1,
minNeighbors=5)
#draw rectangle around detected face
for (x, y, w, h) in faces:
cv2.rectangle(input_img, (x, y), (x + w, y + h), (0, 255, 0),
2)
depth_point = [0, 0, 0]
if len(faces) > 0:
#detect landmarks
status, landmarks = facemark.fit(gray_img, faces)
#draw dots on the detected facial landmarks
for f in range(len(landmarks)):
cv2.face.drawFacemarks(input_img, landmarks[f])
#get head pose
reprojectdst, rotation_vec, rotation_mat, translation_vec, euler_angle, image_pts = get_head_pose(
landmarks[0][0], mtx, dist)
# draw circle on image points (nose tip, corner of eye, lip and chin)
for p in image_pts:
cv2.circle(input_img, (int(p[0]), int(p[1])), 3,
(0, 0, 255), -1)
# draw line sticking out of the nose tip and showing the head pose
(nose_end_point2D,
jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
rotation_vec, translation_vec,
mtx, dist)
p1 = (int(image_pts[0][0]), int(image_pts[0][1]))
p2 = (int(nose_end_point2D[0][0][0]),
int(nose_end_point2D[0][0][1]))
cv2.line(input_img, p1, p2, (255, 0, 0), 2)
##get 3D co-ord of nose
depth = aligned_depth_frame.get_distance(
landmarks[0][0][30][0], landmarks[0][0][30][1])
cv2.circle(input_img,
(landmarks[0][0][30][0], landmarks[0][0][30][1]), 3,
(0, 255, 0), -1)
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin,
[landmarks[0][0][30][0], landmarks[0][0][30][1]], depth)
depth_point = np.array(depth_point)
depth_point = np.reshape(depth_point, [1, 3])
#check if the depth estimation is not zero and filters out faces within 0.8 m from the camera
if (depth_point[0][2] != 0 and depth_point[0][2] < 0.8): #
if kalman_flag == 0: #
print("reinit", img_idx, skip_frame_reinit)
Posarr = np.array([
depth_point[0][0], depth_point[0][1],
depth_point[0][2]
])
Posarr = Posarr.reshape(1, 3)
print("Posarr", Posarr)
r = R.from_euler('zyx', euler_angle, degrees=True)
RotMat = r.as_matrix()
img_sent = img_idx
else:
# execute the following if kalman filter to be applied
print("euler angle", euler_angle, euler_angle[0],
euler_angle[1], euler_angle[2])
current_measurement = np.append(
depth_point, euler_angle)
current_prediction = kalman.predict()
current_prediction = np.array(current_prediction,
np.float32)
current_prediction = current_prediction.transpose()[0]
# predicted euler angles
euler_angle_P = current_prediction[3:6]
# predicted posarr
Posarr_P = np.array(current_prediction[:3]).reshape(
[1, 3])
# convert to rotation matrix using r function
r = R.from_euler('zyx', euler_angle_P, degrees=True)
rotation_mat = r.as_matrix()
# update the estimate of the kalman filter
kalman.correct(
np.transpose(
np.reshape(current_measurement, [1, 6])))
Posarr_noKalman = np.array([
depth_point[0][0], depth_point[0][1],
depth_point[0][2]
])
depth_point = Posarr_P
print("Posarr_P", depth_point, Posarr_noKalman,
euler_angle, euler_angle_P)
#Combine rotation matrix and translation vector (given by the depth point) to get the head pose
RSTr = np.hstack([rotation_mat, depth_point.transpose()])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
print("head pose", RSTr)
# If arucoposeflag = 1, an Aruco marker will get detected and its transformed pose will be streamed to the AR headset and the pose
# of the tracking parent will be updated to reflect Aruco marker pose. This can be used to verify/test the accuracy of teh calibration
if arucoposeflag == 1:
print("aruco")
gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# set dictionary size depending on the aruco marker selected
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoindetectorts = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# font for displaying text (below)
font = cv2.FONT_HERSHEY_SIMPLEX
# check if the ids list is not empty
if np.all(ids != None):
# estimate pose of each marker
intr = profile.get_stream(
rs.stream.color
).as_video_stream_profile().get_intrinsics(
) #profile.as_video_stream_profile().get_intrinsics()
mtx = np.array([[intr.fx, 0, intr.ppx],
[0, intr.fy, intr.ppy], [0, 0, 1]])
dist = np.array(intr.coeffs)
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners, 0.045, mtx, dist
) # 0.0628 (0.061 if using Dell laptop - 95% zoom)
for i in range(0, ids.size):
# draw axis for the aruco markers
aruco.drawAxis(input_img, mtx, dist, rvec[i],
tvec[i], 0.1)
# draw a square around the markers
aruco.drawDetectedMarkers(input_img, corners)
#Combine rotation matrix and translation vector to get Aruco pose
R_rvec = R.from_rotvec(rvec[0])
R_rotmat = R_rvec.as_matrix()
AruRSTr = np.hstack(
[R_rotmat[0], tvec[0].transpose()])
AruRSTr = np.vstack([AruRSTr, [0, 0, 0, 1]])
RSTr = AruRSTr
print("Aruco pose", AruRSTr)
# Since pose detected in OpenCV will be right handed coordinate system, it needs to be converted to left-handed coordinate system of Unity
RSTr_LH = np.array([
RSTr[0][0], RSTr[0][2], RSTr[0][1], RSTr[0][3],
RSTr[2][0], RSTr[2][2], RSTr[2][1], RSTr[2][3],
RSTr[1][0], RSTr[1][2], RSTr[1][1], RSTr[1][3],
RSTr[3][0], RSTr[3][1], RSTr[3][2], RSTr[3][3]
]) # converting to left handed coordinate system
RSTr_LH = RSTr_LH.reshape(4, 4)
#Compute the transformed pose to be streamed to MagicLeap
HeadPoseTr = np.matmul(MLRSTr, RSTr_LH)
ArrToSend = np.array([
HeadPoseTr[0][0], HeadPoseTr[0][1], HeadPoseTr[0][2],
HeadPoseTr[0][3], HeadPoseTr[1][0], HeadPoseTr[1][1],
HeadPoseTr[1][2], HeadPoseTr[1][3], HeadPoseTr[2][0],
HeadPoseTr[2][1], HeadPoseTr[2][2], HeadPoseTr[2][3],
HeadPoseTr[3][0], HeadPoseTr[3][1], HeadPoseTr[3][2],
HeadPoseTr[3][3]
])
ArrToSendPrev = ArrToSend
if socket_connect == 1:
# Send transformed pose to AR headset
dataTosend = struct.pack('f' * len(ArrToSend),
*ArrToSend)
c.send(dataTosend)
# if depth of nose not estimated correctly, send previously detected nose location.
elif img_idx > 1:
dataTosendPrev = struct.pack('f' * len(ArrToSendPrev),
*ArrToSendPrev)
if socket_connect == 1 and img_idx % skip_frame_send == 0: #
img_sent = img_idx
c.send(dataTosendPrev)
else:
print("No Face Detected")
# display detected face with landmarks, pose.
cv2.imshow('Landmark_Window', input_img)
key = cv2.waitKey(1)
class KalmanEstimates(RaceTrack):
"""The KalmanEstimates class implements the KalmanFilter class predict()
and correct() methods, draws the estimated location of the car with a blue
line, and draws the estimate uncertainty as a transparent green circle with
a radius equal to 3 times the standard deviation of the estimate uncertainty.
"""
def __init__(self, **filter_kwargs):
"""Initialize the class with the Kalman filter kwargs. Inherits the
RaceTrack() class variables so that Kalman filter estimates and
uncertainty can be plotted on the race track.
"""
super().__init__() # Inherit RaceTrack class variables
self.kalman_rotation = self.path_angles.copy()
self.init_rotation() # Calculate pixel-by-pixel car orientation
self.kf = KalmanFilter(**filter_kwargs) # Instantiate Kalman filter
# Get initial estimate and estimate uncertainty values
self.estimate = (self.kf.x[0, 0], self.kf.x[1, 0])
self.estimate_uncertainty = (self.kf.P[0, 0], self.kf.P[1, 1],
self.kf.P[2, 2], self.kf.P[3, 3])
self.estimates = [self.estimate]
self.uncertainties = [self.estimate_uncertainty]
# Transparent surface used to blit estimate uncertainty circle
self.surface = pygame.Surface((self.game_width, self.game_height),
pygame.SRCALPHA)
def init_rotation(self):
"""Calculate the orientation of the car at every pixel along the race
track. There are only 8 possible orientations corresponding to one of
8 neighboring pixels. Because of this, the path coordinates must
never skip a pixel and no pixel should have more than two neighbors
(adjacent pixels).
"""
for idx, bend_bool in enumerate(self.path_bend):
try:
if bend_bool == 1 or self.path_bend[idx + 1] == 1:
coord_diff = tuple(
np.subtract(self.path_coords[idx + 1],
self.path_coords[idx]))
if coord_diff[0] != 0 and coord_diff[1] != 0:
if coord_diff[0] == coord_diff[1] == 1:
self.kalman_rotation[idx] = -45
if coord_diff[0] == coord_diff[1] == -1:
self.kalman_rotation[idx] = 135
if coord_diff[0] == 1 and coord_diff[1] == -1:
self.kalman_rotation[idx] = 45
if coord_diff[0] == -1 and coord_diff[1] == 1:
self.kalman_rotation[idx] = -135
else:
if coord_diff[0] == 1: self.kalman_rotation[idx] = 0
if coord_diff[0] == -1: self.kalman_rotation[idx] = 180
if coord_diff[1] == 1: self.kalman_rotation[idx] = -90
if coord_diff[1] == -1: self.kalman_rotation[idx] = 90
except:
pass # Reached finish line (end of coordinate list)
def predict(self):
"""Implement the Kalman filter predict() method, and erase the previous
iteration's uncertainty circle.
"""
self.kf.predict()
self.erase_uncertainty_circle()
def estimate_coord(self, z, Q=None, R=None, u=None):
"""Implement the Kalman filter correct() method, store the results in a
list, and draw the estimated path and uncertainty circle.
"""
x, P = self.kf.correct(z, Q, R, u)
x_coord = int(np.round(x[0, 0]))
y_coord = int(np.round(x[1, 0]))
x_uncertainty = P[0, 0]
y_uncertainty = P[1, 1]
xvel_uncertainty = P[2, 2]
yvel_uncertainty = P[3, 3]
self.estimate = (x_coord, y_coord)
self.estimates.append(self.estimate)
self.estimate_uncertainty = (x_uncertainty, y_uncertainty,
xvel_uncertainty, yvel_uncertainty)
self.uncertainties.append(self.estimate_uncertainty)
self.draw_estimate_path()
self.draw_uncertainty_circle()
def draw_estimate_path(self):
"""Draw estimates calculated by Kalman filter as a blue line."""
for idx, coord in enumerate(self.estimates):
if idx < len(self.estimates) - 1:
pygame.draw.line(self.gameDisplay, (0, 0, 255), coord,
self.estimates[idx + 1], self.line_width)
def erase_uncertainty_circle(self):
"""Delete old uncertainty circle at the previous car coordiante. Make
sure to clear transparent surface as well as blitting the background
and foreground to the gameDisplay.
"""
idx = len(self.estimates) - 1
coord = self.estimates[idx]
radius = 3 * int(np.round(np.max(self.uncertainties[idx])**0.5))
blit_coord = np.subtract(coord, (radius, radius))
clear_rect = pygame.Rect(blit_coord, (2 * radius, 2 * radius))
self.gameDisplay.blit(self.background, blit_coord, clear_rect)
self.gameDisplay.blit(self.foreground, blit_coord, clear_rect)
self.surface.fill((0, 0, 0, 0), clear_rect)
def draw_uncertainty_circle(self):
"""Draw new uncertainty circle at the current coordinate. In order to
get a transparent circle must first blit the circle to a transparent
surface, then blit the transparent surface to the gameDisplay.
Radius of circle is 3 times the standard deviation of the x-position
estimate uncertainty.
"""
idx = len(self.estimates) - 1
coord = self.estimates[idx]
radius = 3 * int(np.round(self.uncertainties[idx][0]**0.5))
pygame.draw.circle(self.surface, (0, 255, 0, 128), coord, radius)
blit_coord = np.subtract(coord, (radius, radius))
self.gameDisplay.blit(
self.surface, blit_coord,
pygame.Rect(blit_coord, (2 * radius, 2 * radius)))
def draw_GPS_measurements(self, measurements, num_measurements):
"""Draw GPS measurements as magenta crosses. The number of crosses
drawn each iteration can be limited to improve simulation frame rate.
"""
cross_dim = 7 # Cross height and width in pixels
cross_thk = 3 # Line width of cross
offset = (cross_dim - 1) / 2
color = (204, 0, 204)
for x, y in measurements[-num_measurements:]:
pygame.draw.line(self.gameDisplay, color, (x - offset, y),
(x + offset, y), cross_thk)
pygame.draw.line(self.gameDisplay, color, (x, y - offset),
(x, y + offset), cross_thk)
def plot_uncertainty(self):
"""Plot Kalman estimate uncertainty versus iteration in a Matplotlib
figure.
"""
fig, ax1 = plt.subplots()
ax2 = ax1.twinx()
axes = [ax1, ax1, ax2, ax2]
colors = ['tab:red', 'tab:red', 'tab:blue', 'tab:blue']
legend_labels = [
'X-coord Estimate Uncertainty', 'Y-coord Estimate Uncertainty',
'X-velocity Estimate Uncertainty',
'Y-velocity Estimate Uncertainty'
]
linestyles = ['-', '--', '-', '--']
plots = []
miny, maxy = 1e6, 0
for idx, uncertainty in enumerate(zip(*self.uncertainties)):
lineplot = axes[idx].plot(uncertainty,
color=colors[idx],
linestyle=linestyles[idx],
label=legend_labels[idx])
plots += lineplot
if idx > 1:
maxy = np.max([maxy, np.max(uncertainty)])
miny = np.min([miny, np.min(uncertainty)])
ax1.set_xlabel('Iteration')
ax1.set_ylabel('Position Uncertainty (Pixels^2)', color=colors[0])
ax1.tick_params(axis='y', labelcolor=colors[0])
ax2.set_ylabel('Velocity Uncertainty (Pixels^2)', color=colors[-1])
ax2.tick_params(axis='y', labelcolor=colors[-1])
# Auto scale because MatPlotLib doesn't handle small numbers correctly
if maxy < 1e-10: maxy = 1e-10
dy = (maxy - miny) * 0.1
ax2.set_ylim(miny - dy, maxy + dy)
plt.title('Kalman Filter Estimate Uncertainty')
ax1.legend(plots, legend_labels, loc=0)
fig.tight_layout()
plt.show()
def close_plots(self):
"""Close all Matplotlib plots."""
plt.close('all')
def main():
face_landmark_path = './shape_predictor_68_face_landmarks.dat'
#flags/initializations to be set
kalman_flag = 1
skip_frame = 1
skip_frame_to_send = 4
socket_connect = 1 # set to 0 if we are testing the code locally on the computer with only the realsense tracking.
simplified_calib_flag = 0 # this is set to 1 if we want to do one-time calibration
arucoposeflag = 1
img_idx = 0 # img_idx keeps track of image index (frame #).
RSTrSum = np.zeros(
(4, 4)
) #initialization of empty buffer for sending the mean of the transformation matrix across every skip_frames_to_send frames
skip_frame_reinit = 120 #after every 150 frames, reinitialize detection
N_samples_calib = 10 # number of samples for computing the calibration matrix using homography
# kalman filter initialization
stateMatrix = np.zeros((12, 1), np.float32) # [x, y, delta_x, delta_y]
estimateCovariance = np.eye(stateMatrix.shape[0])
transitionMatrix = np.array([[1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]],
np.float32)
processNoiseCov = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]],
np.float32) * 0.001
measurementStateMatrix = np.zeros((6, 1), np.float32)
observationMatrix = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]],
np.float32)
measurementNoiseCov = 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]],
np.float32) * 1
kalman = KalmanFilter(X=stateMatrix,
P=estimateCovariance,
F=transitionMatrix,
Q=processNoiseCov,
Z=measurementStateMatrix,
H=observationMatrix,
R=measurementNoiseCov)
if socket_connect == 1:
# create a socket object
s = socket.socket()
print("Socket successfully created")
# reserve a port on your computer in our case it is 2020 but it can be anything
port = 2020
s.bind(('', port))
print("socket binded to %s" % (port))
# put the socket into listening mode
s.listen(5)
print("socket is listening")
c, addr = s.accept()
print('got connection from ', addr)
# dlib face detector
dlibdetector = dlib.get_frontal_face_detector()
# dlib landmark detector
predictor = dlib.shape_predictor('./shape_predictor_68_face_landmarks.dat')
if socket_connect == 1 and simplified_calib_flag == 0:
arucoinstance = arucocalibclass()
ReturnFlag = 1
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
marker_len = 0.0645
MLRSTr = arucoinstance.startcamerastreaming(c, ReturnFlag, marker_len,
aruco_dict,
N_samples_calib)
print(MLRSTr)
elif socket_connect == 1 and simplified_calib_flag == 1:
T_M2_RS = np.array([
-1.0001641, 0.00756584, 0.00479072, 0.03984956, -0.00774137,
-0.99988126, -0.03246199, -0.01359556, 0.00453644, -0.03251681,
0.99971441, -0.00428408, 0., 0., 0., 1.
])
T_M2_RS = T_M2_RS.reshape(4, 4)
MLRSTr = simplified_calib(T_M2_RS, c)
else:
MLRSTr = np.array((1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1))
MLRSTr = MLRSTr.reshape(4, 4)
print(MLRSTr)
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
profile = pipeline.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
print('Start detecting pose ...')
while True:
# get video frame
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
color_frame = aligned_frames.get_color_frame()
aligned_depth_frame = aligned_frames.get_depth_frame()
if not aligned_depth_frame or not color_frame:
continue
# Intrinsics & Extrinsics of Realsense
depth_intrin = profile.get_stream(
rs.stream.depth).as_video_stream_profile().get_intrinsics()
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
depth_to_color_extrin = aligned_depth_frame.profile.get_extrinsics_to(
color_frame.profile)
mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy],
[0, 0, 1]])
dist = np.array(intr.coeffs)
input_img = np.asanyarray(color_frame.get_data())
img_idx = img_idx + 1
img_h, img_w, _ = np.shape(input_img)
# Process the first frame and every frame after "skip_frame" frames
if img_idx == 1 or img_idx % skip_frame == 0:
gray_img = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# detect faces using dlib
rects = dlibdetector(gray_img, 1)
input_imgcopy = input_img
for rect in rects:
# detect facial landmarks
shape = predictor(gray_img, rect)
#head pose estimation
reprojectdst, rotation_vec, rotation_mat, translation_vec, euler_angle, image_pts = get_head_pose(
shape, mtx, dist)
# Project a 3D point (0, 0, 1000.0) onto the image plane.
#We use this to draw a line sticking out of the nose
(nose_end_point2D,
jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
rotation_vec, translation_vec,
mtx, dist)
for p in image_pts:
cv2.circle(input_img, (int(p[0]), int(p[1])), 3,
(0, 0, 255), -1)
# draw line sticking out of the nose tip and showing the head pose
p1 = (int(image_pts[0][0]), int(image_pts[0][1]))
p2 = (int(nose_end_point2D[0][0][0]),
int(nose_end_point2D[0][0][1]))
cv2.line(input_img, p1, p2, (255, 0, 0), 2)
# convert landmarks detected to numpy type
shape = face_utils.shape_to_np(shape)
landmarks = np.float32(shape)
for (x, y) in landmarks:
cv2.circle(input_img, (x, y), 1, (0, 0, 255), -1)
#get 3D co-ord of nose
depth = aligned_depth_frame.get_distance(
image_pts[0][0], image_pts[0][1])
cv2.circle(input_img, (image_pts[0][0], image_pts[0][1]), 3,
(0, 255, 0), -1)
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, [image_pts[0][0], image_pts[0][1]], depth)
depth_point = np.array(depth_point)
depth_point = np.reshape(depth_point, [1, 3])
print("depth_point", depth_point)
if (depth_point[0][2] != 0 and depth_point[0][2] < 0.8):
if kalman_flag == 0: # or img_idx - img_sent > reinit_thresh or img_idx % skip_frame_reinit == 0:
##print("reset img_sent", img_idx, img_sent)
print("reinit", img_idx, skip_frame_reinit)
Posarr = np.array([
depth_point[0][0], depth_point[0][1],
depth_point[0][2]
])
Posarr = Posarr.reshape(1, 3)
print("Posarr", Posarr)
r = R.from_euler('zyx', euler_angle, degrees=True)
RotMat = r.as_matrix()
img_sent = img_idx
else:
# execute the following if kalman filter to be applied
##print("kalman loop", img_idx, img_sent)
print("euler angle", euler_angle, euler_angle[0],
euler_angle[1], euler_angle[2])
current_measurement = np.append(
depth_point, euler_angle)
current_prediction = kalman.predict()
current_prediction = np.array(current_prediction,
np.float32)
current_prediction = current_prediction.transpose()[0]
# predicted euler angles
euler_angle_P = current_prediction[3:6]
# predicted posarr
Posarr_P = np.array(current_prediction[:3]).reshape(
[1, 3])
# convert to rotation matrix using r function
r = R.from_euler('zyx', euler_angle_P, degrees=True)
rotation_mat = r.as_matrix()
# update the estimate of the kalman filter
print("current measurement", current_measurement)
#kalman.correct(np.transpose(current_measurement))
kalman.correct(
np.transpose(
np.reshape(current_measurement, [1, 6])))
Posarr_noKalman = np.array([
depth_point[0][0], depth_point[0][1],
depth_point[0][2]
])
depth_point = Posarr_P
print("Posarr_P", depth_point, Posarr_noKalman,
euler_angle, euler_angle_P)
#RSTr is the head pose - combine depth point (which gives the translation, and the rotation to get RSTr)
RSTr = np.hstack([rotation_mat, depth_point.transpose()])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
print("head pose", RSTr)
# compute mean of the head pose every few frames
RSTrSum += RSTr
if img_idx == skip_frame_to_send:
RSTrTosend = RSTrSum / skip_frame_to_send
RSTr = RSTrTosend
RSTrSum = np.zeros((4, 4))
if arucoposeflag == 1:
print("aruco")
gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# set dictionary size depending on the aruco marker selected
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoindetectorts = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# check if the ids list is not empty
if np.all(ids != None):
# estimate pose of each marker
intr = profile.get_stream(
rs.stream.color
).as_video_stream_profile().get_intrinsics(
) #profile.as_video_stream_profile().get_intrinsics()
mtx = np.array([[intr.fx, 0, intr.ppx],
[0, intr.fy, intr.ppy], [0, 0, 1]])
dist = np.array(intr.coeffs)
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners, 0.045, mtx, dist) #
for i in range(0, ids.size):
# draw axis for the aruco markers
aruco.drawAxis(input_img, mtx, dist, rvec[i],
tvec[i], 0.1)
# draw a square around the markers
aruco.drawDetectedMarkers(input_img, corners)
#Combine rotation matrix and translation vector to get Aruco pose
R_rvec = R.from_rotvec(rvec[0])
R_rotmat = R_rvec.as_matrix()
AruRSTr = np.hstack(
[R_rotmat[0], tvec[0].transpose()])
AruRSTr = np.vstack([AruRSTr, [0, 0, 0, 1]])
RSTr = AruRSTr
print("Aruco pose", AruRSTr)
if img_idx % skip_frame_to_send == 0:
# Since pose detected in OpenCV will be right handed coordinate system, it needs to be converted to left-handed coordinate system of Unity
RSTr_LH = np.array([
RSTr[0][0], RSTr[0][2], RSTr[0][1], RSTr[0][3],
RSTr[2][0], RSTr[2][2], RSTr[2][1], RSTr[2][3],
RSTr[1][0], RSTr[1][2], RSTr[1][1], RSTr[1][3],
RSTr[3][0], RSTr[3][1], RSTr[3][2], RSTr[3][3]
]) # converting to left handed coordinate system
RSTr_LH = RSTr_LH.reshape(4, 4)
#Compute the transformed pose to be streamed to MagicLeap
HeadPoseTr = np.matmul(MLRSTr, RSTr_LH)
ArrToSend = np.array([
HeadPoseTr[0][0], HeadPoseTr[0][1],
HeadPoseTr[0][2], HeadPoseTr[0][3],
HeadPoseTr[1][0], HeadPoseTr[1][1],
HeadPoseTr[1][2], HeadPoseTr[1][3],
HeadPoseTr[2][0], HeadPoseTr[2][1],
HeadPoseTr[2][2], HeadPoseTr[2][3],
HeadPoseTr[3][0], HeadPoseTr[3][1],
HeadPoseTr[3][2], HeadPoseTr[3][3]
])
ArrToSendPrev = ArrToSend
if socket_connect == 1:
dataTosend = struct.pack('f' * len(ArrToSend),
*ArrToSend)
c.send(dataTosend)
else:
print("No Face Detected")
cv2.imshow('Landmark_Window', input_img)
key = cv2.waitKey(1)
