def test_ball_path(): y = 15 x = 0 omega = 0. noise = [1,1] v0 = 100. ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise) dt = 1 f1 = KalmanFilter(dim_x=6, dim_z=2) dt = 1/30. # time step ay = -.5*dt**2 f1.F = np.mat ([[1, dt, 0, 0, 0, 0], # x=x0+dx*dt [0, 1, dt, 0, 0, 0], # dx = dx [0, 0, 0, 0, 0, 0], # ddx = 0 [0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2 [0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt [0, 0, 0, 0, 0, 1]]) # ddy = -g f1.H = np.mat([ [1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) f1.R = np.eye(2) * 5 f1.Q = np.eye(6) * 0. omega = radians(omega) vx = cos(omega) * v0 vy = sin(omega) * v0 f1.x = np.mat([x,vx,0,y,vy,-9.8]).T f1.P = np.eye(6) * 500. z = np.mat([[0,0]]).T count = 0 markers.MarkerStyle(fillstyle='none') np.set_printoptions(precision=4) while f1.x[3,0] > 0: count += 1 #f1.update (z) f1.predict() plt.scatter(f1.x[0,0],f1.x[3,0], color='green')
def example1(): """ Test Kalman Filter implementation :return: """ dt = 1.0 / 60 F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]]) H = np.array([1, 0, 0]).reshape(1, 3) Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]]) R = np.array([0.5]).reshape(1, 1) x = np.linspace(-10, 10, 100) measurements = -(x**2 + 2 * x - 2) + np.random.normal(0, 2, 100) kf = KalmanFilter(F=F, H=H, Q=Q, R=R) predictions = [] for z in measurements: predictions.append(np.dot(H, kf.predict())[0]) kf.update(z) plt.plot(range(len(measurements)), measurements, label='Measurements') plt.plot(range(len(predictions)), np.array(predictions), label='Kalman Filter Prediction') plt.legend() plt.show()
def main(): # Create opencv video capture object VideoCap = cv2.VideoCapture('video/randomball.avi') #Variable used to control the speed of reading the video ControlSpeedVar = 100 #Lowest: 1 - Highest:100 HiSpeed = 100 #Create KalmanFilter object KF #KalmanFilter(dt, u_x, u_y, std_acc, x_std_meas, y_std_meas) KF = KalmanFilter(0.1, 1, 1, 1, 0.1, 0.1) debugMode = 1 while (True): # Read frame ret, frame = VideoCap.read() # Detect object centers = detect(frame, debugMode) # If centroids are detected then track them if (len(centers) > 0): # Draw the detected circle cv2.circle(frame, (int(centers[0][0]), int(centers[0][1])), 10, (0, 191, 255), 2) # Predict (x, y) = KF.predict() # Draw a rectangle as the predicted object position cv2.rectangle(frame, (int(x) - 15, int(y) - 15), (int(x) + 15, int(y) + 15), (255, 0, 0), 2) # Update (x1, y1) = KF.update(centers[0]) # Draw a rectangle as the estimated object position cv2.rectangle(frame, (int(x1) - 15, int(y1) - 15), (int(x1) + 15, int(y1) + 15), (0, 0, 255), 2) cv2.putText(frame, "Estimated Position", (int(x1) + 15, int(y1) + 10), 0, 0.5, (0, 0, 255), 2) cv2.putText(frame, "Predicted Position", (int(x) + 15, int(y)), 0, 0.5, (255, 0, 0), 2) cv2.putText(frame, "Measured Position", (int(centers[0][0]) + 15, int(centers[0][1]) - 15), 0, 0.5, (0, 191, 255), 2) cv2.imshow('image', frame) if cv2.waitKey(2) & 0xFF == ord('q'): VideoCap.release() cv2.destroyAllWindows() break cv2.waitKey(HiSpeed - ControlSpeedVar + 1)
def test_ball_path(): y = 15 x = 0 omega = 0. noise = [1, 1] v0 = 100. ball = BallPath(x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise) dt = 1 f1 = KalmanFilter(dim_x=6, dim_z=2) dt = 1 / 30. # time step ay = -.5 * dt**2 f1.F = np.mat([ [1, dt, 0, 0, 0, 0], # x=x0+dx*dt [0, 1, dt, 0, 0, 0], # dx = dx [0, 0, 0, 0, 0, 0], # ddx = 0 [0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2 [0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt [0, 0, 0, 0, 0, 1] ]) # ddy = -g f1.H = np.mat([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) f1.R = np.eye(2) * 5 f1.Q = np.eye(6) * 0. omega = radians(omega) vx = cos(omega) * v0 vy = sin(omega) * v0 f1.x = np.mat([x, vx, 0, y, vy, -9.8]).T f1.P = np.eye(6) * 500. z = np.mat([[0, 0]]).T count = 0 markers.MarkerStyle(fillstyle='none') np.set_printoptions(precision=4) while f1.x[3, 0] > 0: count += 1 #f1.update (z) f1.predict() plt.scatter(f1.x[0, 0], f1.x[3, 0], color='green')
class VelocityFilter: def __init__(self, dt): self.filter = KalmanFilter() self.filter.F = np.kron(np.array([[1., dt], [0., 1.]]), np.eye(2)) self.filter.H = np.kron(np.array([1., 0.]), np.eye(2)) self.filter.P = np.diag([1., 1., 1e-9, 1e-9]) self.filter.Q = np.kron(np.diag([1e-9] * 2), np.eye(2)) self.filter.R = np.diag([5.0] * 2) self.filter.xhat = np.array([0., 0., 0., 0.]) def predict(self): return self.filter.predict() def update(self, meas): return self.filter.update(meas) def run(self, meas): self.update(meas) return self.predict()
pos = (i, i) ra, rb = d.range_of(pos) rx, ry = d.range_of((i + f1.x[0, 0], i + f1.x[2, 0])) print('range =', ra, rb) z = np.mat([[ra - rx], [rb - ry]]) print('z =', z) f1.H = H_of(pos, pos_a, pos_b) print('H =', f1.H) ##f1.update (z) print(f1.x) xs.append(f1.x[0, 0] + i) ys.append(f1.x[2, 0] + i) pxs.append(pos[0]) pys.append(pos[1]) f1.predict() print(f1.H * f1.x) print(z) print(f1.x) f1.update(z) print(f1.x) p1, = plt.plot(xs, ys, 'r--') p2, = plt.plot(pxs, pys) plt.legend([p1, p2], ['filter', 'ideal'], 2) plt.show()
def main(): # Create opencv video capture object # VideoCap = cv2.VideoCapture('./video/randomball.avi') VideoCap = cv2.VideoCapture('./video/multi.mp4') # VideoCap = cv2.VideoCapture('./video/cars2.mp4') frame_width = int(VideoCap.get(3)) frame_height = int(VideoCap.get(4)) fps = VideoCap.get(cv2.CAP_PROP_FPS) outVid = cv2.VideoWriter('trackedObjects.avi', cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), fps, (frame_width, frame_height)) largestDistance = math.sqrt(frame_width**2 + frame_height**2) # Variable used to control the speed of reading the video ControlSpeedVar = 100 # Lowest: 1 - Highest:100 HiSpeed = 100 # Create KalmanFilter object KF #KalmanFilter(dt, u_x, u_y, std_acc, x_std_meas, y_std_meas) # debugMode = 1 debugMode = 0 KFs = [] KFs_used = [] j = 0 while (True): j += 1 # Read frame ret, frame = VideoCap.read() if not ret and cv2.waitKey(2) & 0xFF == ord('q'): outVid.release() VideoCap.release() cv2.destroyAllWindows() break # Detect object try: centers = detect(frame, debugMode) except: outVid.release() VideoCap.release() cv2.destroyAllWindows() break # If centroids are detected then track them if (len(centers) > 0): # getting rid of unused Kalman Filters temp = KFs.copy() KFs = [] for i in range(len(temp)): if (KFs_used[i]): KFs.append(temp[i]) gc.collect() # predict all KFs predicted_x, predicted_y = predictKFs(KFs) KFs_used = [False] * len(KFs) for i in range(len(centers)): # Draw the detected circle cv2.circle(frame, (int(centers[i][0]), int(centers[i][1])), 10, (0, 191, 255), 2) curr_KF = getClosestKF(centers[i][0], centers[i][1], predicted_x, predicted_y, largestDistance / 24) if (curr_KF < 0): KF = KalmanFilter(1.0 / fps, 1, 1, 1, 0.1, 0.1) x, y = KF.predict() KFs.append(KF) KFs_used.append(True) else: KF = KFs[curr_KF] KFs_used[curr_KF] = True x, y = predicted_x[curr_KF], predicted_y[curr_KF] # Draw a rectangle as the predicted object position x, y = int(x), int(y) cv2.rectangle(frame, (x - 15, y - 15), (x + 15, y + 15), ( 255, 0, ), 2) # Update (x1, y1) = KF.update(centers[i]) x1, y1 = int(x1), int(y1) # Draw a rectangle as the estimated object position cv2.rectangle(frame, (x1 - 15, y1 - 15), (x1 + 15, y1 + 15), (0, 0, 255), 2) cv2.putText(frame, "Estimated Position" + str(i), (x1 + 15, y1 + 10), 0, 0.5, (0, 0, 255), 2) cv2.putText(frame, "Predicted Position" + str(i), (x + 15, y), 0, 0.5, (255, 0, 0), 2) cv2.putText(frame, "Measured Position" + str(i), (int(centers[i][0] + 15), int(centers[i][1] - 15)), 0, 0.5, (0, 191, 255), 2) cv2.imshow('image', frame) outVid.write(frame) cv2.waitKey(HiSpeed - ControlSpeedVar + 1)
pos = (i, i) ra, rb = d.range_of(pos) rx, ry = d.range_of((i + f1.x[0, 0], i + f1.x[2, 0])) print('range =', ra, rb) z = np.mat([[ra - rx], [rb - ry]]) print('z =', z) f1.H = H_of(pos, pos_a, pos_b) print('H =', f1.H) ##f1.update (z) print(f1.x) xs.append(f1.x[0, 0] + i) ys.append(f1.x[2, 0] + i) pxs.append(pos[0]) pys.append(pos[1]) f1.predict() print(f1.H * f1.x) print(z) print(f1.x) f1.update(z) print(f1.x) p1, = plt.plot(xs, ys, 'r--') p2, = plt.plot(pxs, pys) plt.legend([p1, p2], ['filter', 'ideal'], 2) plt.show()
def face_blur(self, frame): # face_boxes are null if res is not 0 res, frame_blurred, face_boxes = fd.detect_face_video_frame( frame, self.detection_model) # if frame_crop: # TODO if self.face_tracking == self.TRACKING_QUEUE: frame_has_face = False if res == 0: frame_has_face = True edge = self.__edge_detect(frame_has_face) if edge == self.EDGE_RISING: if self.use_frame_cache == True: # TODO multiple faces can be detected self.face_coordinates_rising = [ face_boxes[0][0], face_boxes[0][1], face_boxes[0][2], face_boxes[0][3] ] self.face_coordinates_delta = self.__delta_coordinates() # interpolate and return retcache = deque() for i in range(0, len(self.frame_cache)): res_img = fd.blur_face(self.frame_cache.pop(), \ self.face_coordinates_delta[0], \ self.face_coordinates_delta[1], \ self.face_coordinates_delta[2], \ self.face_coordinates_delta[3]) retcache.appendleft(res_img) self.use_frame_cache = False return self.RESULT_QUEUE, retcache else: return self.RESULT_FRAME, frame_blurred elif edge == self.EDGE_FALLING: self.frame_cache.clear() self.use_frame_cache = True if face_boxes is not None: # TODO multiple faces can be detected self.face_coordinates_falling = [ face_boxes[0][0], face_boxes[0][1], face_boxes[0][2], face_boxes[0][3] ] if self.use_frame_cache == True: print("cached") self.frame_cache.appendleft(frame_blurred) if len(self.frame_cache) >= self.max_frame_padding: self.use_frame_cache = False return self.RESULT_QUEUE, self.frame_cache else: return self.RESULT_NONE, None else: return self.RESULT_FRAME, frame_blurred elif self.face_tracking == self.TRACKING_KALMAN: self.k_filters_with_face.fill(False) if res == 0: # assign face boxes to filters, if a new box appears create a new filter for i, face_box in enumerate(face_boxes): res, filter_idx = self.__find_proxy_filter(face_box) if res == self.FILTER_MATCH: # assign filter to box, update filter and predict the next face location self.k_filters_with_face[filter_idx] = True k_filter = self.k_filters[filter_idx] k_filter.update_and_correct(face_box) k_filter.predict() else: # initialize a new kalman filter k_filter = KalmanFilter(init_rectangle=face_box) self.k_filters = np.append(self.k_filters, k_filter) self.k_filters_with_face = np.append( self.k_filters_with_face, True) delete_filters = [] # if any filter has no face box assigned use filter prediction for i, status in enumerate(self.k_filters_with_face): if status == False: k_filter = self.k_filters[i] # mark filter for deletion if padding limit has been achieved if k_filter.n_padded_frames >= self.max_frame_padding: delete_filters.append(i) continue k_prediction = k_filter.prediction k_prediction = np.squeeze(k_prediction.astype(int)) rul_x, rul_y = utls.rectangle_upper_left([k_prediction[0], \ k_prediction[1], \ k_filter.rec_width, \ k_filter.rec_height]) print("upper left: ", rul_x, rul_y, k_filter.rec_width, k_filter.rec_height) # TODO change blur_face to accept rectangle as a list frame_blurred = fd.blur_face(frame_blurred, \ rul_x, \ rul_y, \ k_filter.rec_width, \ k_filter.rec_height) frame_blurred = cv2.circle( frame_blurred, (k_prediction[0], k_prediction[1]), 5, (0, 0, 255), -1) frame_blurred = cv2.putText(frame_blurred, str(len(self.k_filters)), (10, 700), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2, cv2.LINE_AA) frame_blurred = cv2.putText(frame_blurred, str(self.k_filters_with_face), (500, 700), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2, cv2.LINE_AA) k_filter.predict() k_filter.n_padded_frames += 1 self.__delete_kalman_filters(delete_filters) return self.RESULT_FRAME, frame_blurred else: return self.RESULT_FRAME, frame_blurred
def test_kf_drag(): y = 1 x = 0 omega = 35. noise = [0, 0] v0 = 50. ball = BaseballPath(x0=x, y0=y, launch_angle_deg=omega, velocity_ms=v0, noise=noise) #ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise) dt = 1 f1 = KalmanFilter(dim_x=6, dim_z=2) dt = 1 / 30. # time step ay = -.5 * dt**2 ax = .5 * dt**2 f1.F = np.mat([ [1, dt, ax, 0, 0, 0], # x=x0+dx*dt [0, 1, dt, 0, 0, 0], # dx = dx [0, 0, 1, 0, 0, 0], # ddx = 0 [0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2 [0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt [0, 0, 0, 0, 0, 1] ]) # ddy = -g # We will inject air drag using Bu f1.B = np.mat([[0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 0., 1.]]).T f1.u = np.mat([[0., 0.]]).T f1.H = np.mat([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) f1.R = np.eye(2) * 5 f1.Q = np.eye(6) * 0. omega = radians(omega) vx = cos(omega) * v0 vy = sin(omega) * v0 f1.x = np.mat([x, vx, 0, y, vy, -9.8]).T f1.P = np.eye(6) * 500. z = np.mat([[0, 0]]).T markers.MarkerStyle(fillstyle='none') np.set_printoptions(precision=4) t = 0 while f1.x[3, 0] > 0: t += dt #f1.update (z) x, y = ball.update(dt) #x,y = ball.pos_at_t(t) update_drag(f1, dt) f1.predict() print f1.x.T plt.scatter(f1.x[0, 0], f1.x[3, 0], color='red', alpha=0.5) plt.scatter(x, y) return f1
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)
class speedEstimator(): def __init__(self): self.validSpeedUpdated = False self.lastPoint = 0. self.distanceThreshold = 0.05 self.speedUpdateTime = [] self.speedWindowSize = 10 self.keyMeasPairs = deque(maxlen=80) self.speedWindow = [] self.filtedRange = [] self.curSpeed = 0.0 self.speedRecord = [] self.rangeKF = KalmanFilter(dim_x=1, dim_z=1) self.rangeKF.x = np.array([0.]) self.rangeKF.F = np.array([[1.]]) self.rangeKF.H = np.array([[1.]]) self.rangeKF.P *= 100. self.rangeKF.R = 0.1 self.rangeKF.Q = 0.001 self.rangeKF.initialized = False def vel_from_dis(self, l_0, l_1, l_2, t0, t1, t2): t_1 = t1 - t0 t_2 = t2 - t1 if (l_2 - l_1) * (l_1 - l_0) > 0: d = abs(l_2 * l_2 - l_1 * l_1 - (l_1 * l_1 - l_0 * l_0) * t_2 / t_1) tl = t_1 * t_2 + t_2 * t_2 return math.sqrt(d / tl) else: return False def range_key_pairs_maintaince(self, range, time): if abs(range - self.lastPoint) > self.distanceThreshold: self.lastPoint = range self.keyMeasPairs.append([range, time]) return True else: return False def estimate_speed(self, range, time, interval): fdragne = self.filter_range(range) self.speedWindowSize = 5 + 0.1 * range if self.range_key_pairs_maintaince( fdragne, time) and len(self.keyMeasPairs) >= 2 * interval: tempresult = self.vel_from_dis(self.keyMeasPairs[-2 * interval][0], self.keyMeasPairs[-interval][0], self.keyMeasPairs[-1][0], self.keyMeasPairs[-2 * interval][1], self.keyMeasPairs[-interval][1], self.keyMeasPairs[-1][1]) if tempresult: self.speedWindow.append(tempresult) if len(self.speedWindow) > (self.speedWindowSize - 1): self.curSpeed = np.median( self.speedWindow ) # Estimation of this linear motion speed self.speedRecord.append(self.curSpeed) self.speedUpdateTime.append(time) self.validSpeedUpdated = True while (len(self.speedWindow) > (self.speedWindowSize - 1)): self.speedWindow.pop(0) else: self.validSpeedUpdated = False def filter_range(self, range): if self.rangeKF.initialized == False: self.rangeKF.x = np.array([range]) self.filtedRange.append(range) self.rangeKF.initialized = True else: self.rangeKF.predict() self.rangeKF.update(range) self.filtedRange.append(self.rangeKF.x) return self.filtedRange[-1] def get_vel(self): return self.curSpeed
def test_kf_drag(): y = 1 x = 0 omega = 35. noise = [0,0] v0 = 50. ball = BaseballPath (x0=x, y0=y, launch_angle_deg=omega, velocity_ms=v0, noise=noise) #ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise) dt = 1 f1 = KalmanFilter(dim_x=6, dim_z=2) dt = 1/30. # time step ay = -.5*dt**2 ax = .5*dt**2 f1.F = np.mat ([[1, dt, ax, 0, 0, 0], # x=x0+dx*dt [0, 1, dt, 0, 0, 0], # dx = dx [0, 0, 1, 0, 0, 0], # ddx = 0 [0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2 [0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt [0, 0, 0, 0, 0, 1]]) # ddy = -g # We will inject air drag using Bu f1.B = np.mat([[0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 0., 1.]]).T f1.u = np.mat([[0., 0.]]).T f1.H = np.mat([ [1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) f1.R = np.eye(2) * 5 f1.Q = np.eye(6) * 0. omega = radians(omega) vx = cos(omega) * v0 vy = sin(omega) * v0 f1.x = np.mat([x,vx,0,y,vy,-9.8]).T f1.P = np.eye(6) * 500. z = np.mat([[0,0]]).T markers.MarkerStyle(fillstyle='none') np.set_printoptions(precision=4) t=0 while f1.x[3,0] > 0: t+=dt #f1.update (z) x,y = ball.update(dt) #x,y = ball.pos_at_t(t) update_drag(f1, dt) f1.predict() print f1.x.T plt.scatter(f1.x[0,0],f1.x[3,0], color='red', alpha=0.5) plt.scatter (x,y) return f1
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
class SensorSimulator: def __init__(self, theta, emax, cachelen): self.cache = [] self.readings = [] self.totaltrans = 0 self.cacheLen = cachelen self.theta = theta self.emax = emax self.counter = 0 self.correcteddata = [] self.totalTransferredData = 0 r_std, q_std = 1., 0.003 self.kf = KalmanFilter(dim_x=2, dim_z=1) self.kf.x = np.array([[19], [0]]) # position, velocity self.kf.F = np.array([[1, 1], [0, 1]]) self.kf.R = np.array([[r_std**2]]) # input noise self.kf.H = np.array([[1., 0.]]) self.kf.P = np.diag([0.001**2, 0]) self.kf.Q = Q_discrete_white_noise(2, 1, q_std**2) self.est_list = [] self.counter = 0 self.batCap = 3.6 self.variance = 0 self.V = 3.6 self.Itx = ((17.4 + 18.8 + 0.5 + 0.426) / 32768) self.DetaT = 5 def getSensorReadings(self, sensor_id, dataSize): self.readings = getData(sensor_id, dataSize) return self.readings def RDR(self, sensorReading): distance = sum = 0.0 transmitted_counter = 0 corr = 0.0 list_1 = [] est_list = [] s = 0 if len(self.cache) <= self.cacheLen: self.cache.append(sensorReading) SensorSimulator.findEnergyConsumption(self) self.correcteddata.append(sensorReading) self.totaltrans += 1 #print(sensorReading) self.kf.predict() self.kf.update([sensorReading]) return sensorReading else: if sensorReading == self.cache[self.cacheLen - 1]: self.correcteddata.append(sensorReading) return else: for i in range(0, self.cacheLen): distance += pow(sensorReading - self.cache[i], 2) sum += self.cache[i] distance = math.sqrt(distance) corr = 1 - distance / sum est = self.kf.predict() self.est_list.append(est[0]) estval = est[0][0] self.kf.update(est[0][0]) #print(sensorReading - estval, ", ", corr) if corr < self.theta: sensorReading = est[0][0] self.cache.pop(0) self.cache.append(sensorReading) self.counter += 1 SensorSimulator.findEnergyConsumption(self) self.correcteddata.append( sensorReading) #################### self.totaltrans += 1 #print(sensorReading) return sensorReading else: #print(sensorReading - estval) if abs(sensorReading - estval) < self.emax: self.correcteddata.append(sensorReading) exit else: self.cache.pop(0) self.cache.append(sensorReading) self.counter += 1 SensorSimulator.findEnergyConsumption(self) self.correcteddata.append(sensorReading) self.totaltrans += 1 #print(sensorReading) return sensorReading def findVariance(self): self.variance = np.var(self.readings) return self.variance def findEnergyEfficiency(self): # print(self.totalTransferredData) #print(3.6-self.batCap, 0) # print("Energy Efficiency:", self.totalTransferredData/(3.6-self.batCap)) return 1 #self.totalTransferredData/(3.6-self.batCap) def findEnergyConsumption(self): #charge= self.Itx*(0.426+15)/32768 charge = 17.4 / 32768 Etx = self.V * charge self.batCap = self.batCap - Etx
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self, bbox): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=7, dim_z=4) self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]]) self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0]]) self.kf.R[2:, 2:] *= 10. self.kf.P[ 4:, 4:] *= 1000. #give high uncertainty to the unobservable initial velocities self.kf.P *= 10. self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 self.kf.x[:4] = convert_bbox_to_z(bbox) self.time_since_update = 0 self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0 def update(self, bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ if ((self.kf.x[6] + self.kf.x[2]) <= 0): self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if (self.time_since_update > 0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return convert_x_to_bbox(self.kf.x)
import cv2 from Detector import detect_inrange, detect_visage from KalmanFilter import KalmanFilter import numpy as np VideoCap=cv2.VideoCapture(0) KF=KalmanFilter(0.1, [0, 0]) while(True): ret, frame=VideoCap.read() points, mask=detect_inrange(frame, 800) #points, mask=detect_visage(frame) etat=KF.predict().astype(np.int32) cv2.circle(frame, (int(etat[0]), int(etat[1])), 2, (0, 255, 0), 5) cv2.arrowedLine(frame, (etat[0], etat[1]), (etat[0]+etat[2], etat[1]+etat[3]), color=(0, 255, 0), thickness=3, tipLength=0.2) if (len(points)>0): cv2.circle(frame, (points[0][0], points[0][1]), 10, (0, 0, 255), 2) KF.update(np.expand_dims(points[0], axis=-1)) cv2.imshow('image', frame) if mask is not None: cv2.imshow('mask', mask)
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)
[0, 0, 1, dt], [0, 0, 0, 1]]) f.H = np.mat ([[1, 0, 0, 0], [0, 0, 1, 0]]) f.Q *= 4. f.R = np.mat([[50,0], [0, 50]]) f.x = np.mat([0,0,0,0]).T f.P *= 100. xs = [] ys = [] count = 200 for i in range(count): z = np.mat([[i+random.randn()*1],[i+random.randn()*1]]) f.predict () f.update (z) xs.append (f.x[0,0]) ys.append (f.x[2,0]) plt.plot (xs, ys) plt.show()
from KalmanFilter import KalmanFilter import numpy as np import matplotlib.pyplot as plt import numpy.random as random f = KalmanFilter(dim=4) dt = 1 f.F = np.mat([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) f.H = np.mat([[1, 0, 0, 0], [0, 0, 1, 0]]) f.Q *= 4. f.R = np.mat([[50, 0], [0, 50]]) f.x = np.mat([0, 0, 0, 0]).T f.P *= 100. xs = [] ys = [] count = 200 for i in range(count): z = np.mat([[i + random.randn() * 1], [i + random.randn() * 1]]) f.predict() f.update(z) xs.append(f.x[0, 0]) ys.append(f.x[2, 0]) plt.plot(xs, ys) plt.show()
def main(): params = check_params(parse_args(sys.argv[1:])) kf = KalmanFilter() if params['file']: template = read_template(params) surf, flann = init_surf() kp, des = surf.detectAndCompute(template, None) #Start up Video processing thread vs = PiVideoStream(resolution=(params['xRes'], params['yRes'])).start() #Wait for camera to warm up time.sleep(3.0) #Used for calculating FPS startTime = time.time() if params['capTime'] == 0: endTime = startTime + 1000000000 else: endTime = startTime + params['capTime'] frames = 0 # Open up serial port with Pixhawk if params['sendToPX4']: port = serial.Serial('/dev/ttyAMA0', 57600) mav = mavlink.MAVLink(port) # Typically only need to search within a small Y-range yRes = params['yRes'] (cropYmin, cropYmax) = (yRes * .25, yRes * .70) #Take weighted average of last # of distances to filter out noise notFoundCount = 1000 while time.time() < endTime: frames += 1 frame = vs.read() frame = frame[cropYmin : cropYmax, 0:params['xRes']] found, [x,y], frame = FindingFuncs.findPatternSURF(frame, surf, kp, des, template, flann, params['preview']) # Count how many frames it has been since the RPi has not found anything if not found: notFoundCount += 1 else: notFoundCount = 0 kf.update(x) # How many frames until you assume to keep going straight. if notFoundCount > 100: kf.reset() x = params['xRes'] / 2 else: x = kf.predict() loc = boundFloat(2.0 * x / params['xRes'] - 1, -1., 1.) if params['sendToPX4']: message = mavlink.MAVLink_duck_leader_loc_message(loc, 5.0) mav.send(message) if params['debug'] : print str(time.time() - startTime) + ", " + str(loc) if params['preview']: # Draw a circle on frame where the Kalman filter predicts. cv2.circle(frame, (int(x), 10), 4, (0, 0, 255), 6) frame = imutils.resize(frame, width=1000) cv2.imshow("Preview", frame) #Check for keypress, ending if Q is entered key = cv2.waitKey(1) & 0xFF if (key == ord("q")): break; totalTime = time.time() - startTime cv2.destroyAllWindows() vs.stop() print "Average main FPS: " + str(float(frames) / totalTime) + "fps"
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
from KalmanFilter import KalmanFilter import time kf = KalmanFilter() kf.update(5) print kf.predict() time.sleep(.2) kf.update(5.5) print kf.predict() time.sleep(.2) kf.update(6) print kf.predict() time.sleep(.2) kf.update(6.5) print kf.predict() time.sleep(.2) kf.update(7) print kf.predict() time.sleep(.2) kf.update(15) print kf.predict() time.sleep(.2) kf.update(8.5) print kf.predict() time.sleep(.2) kf.update(7)