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