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)