def __init__(self, useProgressivePNC=False, pnc=1e-4): # state vector: # 0-7: x, y for each corner # 8-16: derivatives of 0-7 respectively self.useProgressivePNC = useProgressivePNC self.kf = cv.CreateKalman(16, 8, 0) self.setOldCvMat(self.kf.transition_matrix, [ [1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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, 0, 0, 0, 0, 1, 0, 0], [0, 0, 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, 0, 0, 0, 0, 1], ]) self.measurement = cv.CreateMat(8, 1, cv.CV_32FC1) cv.SetIdentity(self.kf.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kf.process_noise_cov, cv.RealScalar(pnc)) cv.SetIdentity(self.kf.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kf.error_cov_post, cv.RealScalar(1))
def __setKalman(self): ts = self if len(ts) < 2: self.kalman_x = ts[-1].x self.kalman_y = ts[-1].y else: self.kalman_x = ts[-2].x self.kalman_y = ts[-2].y self.kalman.state_pre[0, 0] = self.kalman_x self.kalman.state_pre[1, 0] = self.kalman_y self.kalman.state_pre[2, 0] = self.predict_pt[0] self.kalman.state_pre[3, 0] = self.predict_pt[1] self.kalman.transition_matrix[0, 0] = 1 self.kalman.transition_matrix[0, 1] = 0 self.kalman.transition_matrix[0, 2] = 0 self.kalman.transition_matrix[0, 3] = 0 self.kalman.transition_matrix[1, 0] = 0 self.kalman.transition_matrix[1, 1] = 1 self.kalman.transition_matrix[1, 2] = 0 self.kalman.transition_matrix[1, 3] = 0 self.kalman.transition_matrix[2, 0] = 0 self.kalman.transition_matrix[2, 1] = 0 self.kalman.transition_matrix[2, 2] = 0 self.kalman.transition_matrix[2, 3] = 1 self.kalman.transition_matrix[3, 0] = 0 self.kalman.transition_matrix[3, 1] = 0 self.kalman.transition_matrix[3, 2] = 0 self.kalman.transition_matrix[3, 3] = 1 cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-5)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1))
def __init__(self, processNoiseCovariance=1e-4, measurementNoiseCovariance=1e-1, errorCovariancePost=0.1): self.kalman = cv.CreateKalman(4, 2, 0) self.kalman_state = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(2, 1, cv.CV_32FC1) for j in range(4): for k in range(4): self.kalman.transition_matrix[j, k] = 0 self.kalman.transition_matrix[j, j] = 1 cv.SetIdentity(self.kalman.measurement_matrix) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(processNoiseCovariance)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(measurementNoiseCovariance)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(errorCovariancePost)) self.predicted = None self.esitmated = None
def __init__(self): self.tfListen = tf.TransformListener() rospy.sleep(1.0) self.kalman = cv.CreateKalman(3, 3, 0) self.kalman_state = cv.CreateMat(3, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(3, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(3, 1, cv.CV_32FC1) self.kalman.state_pre[0, 0] = 0 #first x self.kalman.state_pre[1, 0] = 0 #first y self.kalman.state_pre[2, 0] = 0 #first theta # set kalman transition matrix self.kalman.transition_matrix[0, 0] = 1 self.kalman.transition_matrix[1, 1] = 1 self.kalman.transition_matrix[2, 2] = 1 # set Kalman Filter cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1)) self.get_hive = rospy.Service('get_hive', GetLocation, self.get_hive) rospy.Subscriber('large_markers', AlvarMarkers, self.cb_ar_marker)
def __init__(self, processNoiseCovariance=1e-1, measurementNoiseCovariance=1e-1, errorCovariancePost=20): self.stateNum = 36 self.measureNum = 18 self.kalman = cv.CreateKalman(self.stateNum, self.measureNum, 0) self.kalman_state = cv.CreateMat(self.stateNum, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(self.stateNum, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(self.measureNum, 1, cv.CV_32FC1) for i in range(self.stateNum): for j in range(self.stateNum): if i == j or (j - self.measureNum) == i: self.kalman.transition_matrix[i, j] = 1.0 else: self.kalman.transition_matrix[i, j] = 0.0 cv.SetIdentity(self.kalman.measurement_matrix) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(processNoiseCovariance)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(measurementNoiseCovariance)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(errorCovariancePost)) self.predicted = None self.esitmated = None
def __init__(self, useProgressivePNC=False, pnc=1e-3): # state vector: # 0: min_x # 1: min_y # 2: max_x # 3: max_y # 4-7: derivatives of 0-4 respectively # measurement vector: # 0-4: same as state (no derivatives) self.useProgressivePNC = useProgressivePNC self.kf = cv.CreateKalman(8, 4, 0) self.setOldCvMat(self.kf.transition_matrix, [ [1, 0, 0, 0, 1, 0, 0, 0], [0, 1, 0, 0, 0, 1, 0, 0], [0, 0, 1, 0, 0, 0, 1, 0], [0, 0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1], ]) self.measurement = cv.CreateMat(4, 1, cv.CV_32FC1) cv.SetIdentity(self.kf.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kf.process_noise_cov, cv.RealScalar(pnc)) cv.SetIdentity(self.kf.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kf.error_cov_post, cv.RealScalar(1))
def __init__(self, processNoiseCovariance=1e-4, measurementNoiseCovariance=1e-1, errorCovariancePost=0.1): ''' Constructs a new Kalman2D object. For explanation of the error covariances see http://en.wikipedia.org/wiki/Kalman_filter ''' # 状态空间:位置--2d,速度--2d self.kalman = cv.CreateKalman(4, 2, 0) self.kalman_state = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(2, 1, cv.CV_32FC1) for j in range(4): for k in range(4): self.kalman.transition_matrix[j,k] = 0 self.kalman.transition_matrix[j,j] = 1 #加入速度 x = x + vx, y = y + vy # 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1 #如果把下面两句注释掉,那么位置跟踪kalman滤波器的状态模型就是没有使用速度信息 # self.kalman.transition_matrix[0, 2]=1 # self.kalman.transition_matrix[1, 3]=1 cv.SetIdentity(self.kalman.measurement_matrix) #初始化带尺度的单位矩阵 cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(processNoiseCovariance)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(measurementNoiseCovariance)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(errorCovariancePost)) self.predicted = None self.esitmated = None
def __init__(self, pt): self.pt = pt self.history = [pt] self.mse = 0.0 self.ns = 1 self.lifespan = 1 self.kalman = cv.CreateKalman(4, 2, 0) self.kalman_state = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(2, 1, cv.CV_32FC1) # set previous state for prediction self.kalman.state_pre[0, 0] = pt[0] self.kalman.state_pre[1, 0] = pt[1] self.kalman.state_pre[2, 0] = 0 self.kalman.state_pre[3, 0] = 0 # set kalman transition matrix self.kalman.transition_matrix[0, 0] = 1 self.kalman.transition_matrix[0, 1] = 0 self.kalman.transition_matrix[0, 2] = 0 self.kalman.transition_matrix[0, 3] = 0 self.kalman.transition_matrix[1, 0] = 0 self.kalman.transition_matrix[1, 1] = 1 self.kalman.transition_matrix[1, 2] = 0 self.kalman.transition_matrix[1, 3] = 0 self.kalman.transition_matrix[2, 0] = 0 self.kalman.transition_matrix[2, 1] = 0 self.kalman.transition_matrix[2, 2] = 0 self.kalman.transition_matrix[2, 3] = 1 self.kalman.transition_matrix[3, 0] = 0 self.kalman.transition_matrix[3, 1] = 0 self.kalman.transition_matrix[3, 2] = 0 self.kalman.transition_matrix[3, 3] = 1 # set Kalman Filter cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-5)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1)) self.assignObs(pt, 3)
def on_mouse(event, x, y, flags, param): if (not color_img): return if event == cv.CV_EVENT_LBUTTONDOWN: my_mask = None seed = (x, y) if ffill_case == 0: lo = up = 0 flags = connectivity + (new_mask_val << 8) else: lo = lo_diff up = up_diff flags = connectivity + ( new_mask_val << 8) + cv.CV_FLOODFILL_FIXED_RANGE b = random.randint(0, 255) g = random.randint(0, 255) r = random.randint(0, 255) if (is_mask): my_mask = mask cv.Threshold(mask, mask, 1, 128, cv.CV_THRESH_BINARY) if (is_color): color = cv.CV_RGB(r, g, b) comp = cv.FloodFill(color_img, seed, color, cv.CV_RGB(lo, lo, lo), cv.CV_RGB(up, up, up), flags, my_mask) cv.ShowImage("image", color_img) else: brightness = cv.RealScalar((r * 2 + g * 7 + b + 5) / 10) comp = cv.FloodFill(gray_img, seed, brightness, cv.RealScalar(lo), cv.RealScalar(up), flags, my_mask) cv.ShowImage("image", gray_img) print "%g pixels were repainted" % comp[0] if (is_mask): cv.ShowImage("mask", mask)
def init_kalman(self): self.kalman = cv.CreateKalman(3,3,0) self.kalman_state = cv.CreateMat(3,1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(3,1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(3,1, cv.CV_32FC1) self.kalman.state_pre[0,0] = 0 #first x self.kalman.state_pre[1,0] = 0 #first y self.kalman.state_pre[2,0] = 0 #first theta # set kalman transition matrix self.kalman.transition_matrix[0,0] = 1 self.kalman.transition_matrix[1,1] = 1 self.kalman.transition_matrix[2,2] = 1 # set Kalman Filter cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1))
def init_kalman(kalman, x, y, z): kalman.transition_matrix[:, :] = 0 for i in range(6): kalman.transition_matrix[i, i] = 1 kalman.transition_matrix[0, 3] = dT kalman.transition_matrix[1, 4] = dT kalman.transition_matrix[2, 5] = dT kalman.state_post[:, :] = 0 kalman.control_matrix[:, :] = 0 kalman.control_matrix[5, 0] = 1 cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-5)) cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1)) kalman.state_post[0, 0] = x kalman.state_post[1, 0] = y kalman.state_post[2, 0] = z
def track(kalman, x, y, z): vx = kalman.state_post[3, 0] vy = kalman.state_post[4, 0] vz = kalman.state_post[5, 0] v = math.sqrt(vx**2 + vy**2 + vz**2) kalman.transition_matrix[3, 3] = 1 - kD * v kalman.transition_matrix[4, 4] = 1 - kD * v kalman.transition_matrix[5, 5] = 1 - kD * v control[0, 0] = -GRAVITY * dT cv.KalmanPredict(kalman, control) pred = [ kalman.state_pre[0, 0], kalman.state_pre[1, 0], kalman.state_pre[2, 0] ] cv.RandArr(rng, measurement, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.005)) measurement[0, 0] += x measurement[1, 0] += y measurement[2, 0] += z cv.KalmanCorrect(kalman, measurement) meas = [measurement[0, 0], measurement[1, 0], measurement[2, 0]] return pred, meas, np.array([x, y, z])
def init_kalman(self, pose): kalman = cv.CreateKalman(6, 3, 0) #kalman_state = cv.CreateMat(6,1, cv.CV_32FC1) #kalman_process_noise = cv.CreateMat(3,1, cv.CV_32FC1) quat = quat_msg_to_array(pose.pose.orientation) r, p, theta = tf.transformations.euler_from_quaternion(quat) kalman.state_post[0, 0] = pose.pose.position.x #first x kalman.state_post[1, 0] = pose.pose.position.y #first y kalman.state_post[2, 0] = theta #first theta kalman.state_post[3, 0] = 0 #delta x kalman.state_post[4, 0] = 0 #delta y kalman.state_post[5, 0] = 0 #delta theta # set kalman transition matrix kalman.transition_matrix[0, 0] = 1 #kalman.transition_matrix[0,3] = 1 kalman.transition_matrix[1, 1] = 1 #kalman.transition_matrix[1,4] = 1 kalman.transition_matrix[2, 2] = 1 #kalman.transition_matrix[2,5] = 1 kalman.transition_matrix[3, 3] = 1 kalman.transition_matrix[4, 4] = 1 kalman.transition_matrix[5, 5] = 1 # set Kalman Filter cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1)) return kalman
def updateProcessNoiseCov(self): if not self.useProgressivePNC: return self.timestep += 1 startTS = 0 endTS = 400 startCov = 1e-4 endCov = 1e-3 covSlope = (endCov - startCov) / (endTS - startTS) covIntercept = startCov cov = covSlope * (self.timestep - 30) + covIntercept print "frame %d: cov=%.4g" % (self.timestep, cov) cv.SetIdentity(self.kf.process_noise_cov, cv.RealScalar(cov))
cv.SetMouseCallback("cam", on_mouse) on_mouse(cv.CV_EVENT_LBUTTONDOWN, centerX, centerY, None, None) font = cv.InitFont(cv.CV_FONT_HERSHEY_PLAIN, 1.0, 1.0) M = 60 while True: img = cv.QueryFrame(capture) cv.CvtColor(img, im, cv.CV_RGB2GRAY) #cv.LogPolar(img, polar, (centerX, centerY), M+1, cv.CV_INTER_NN + cv.CV_WARP_FILL_OUTLIERS ) cv.And(im, lm, lt) leftBrightness = cv.Avg(im, mask=lm) cv.Rectangle(lt, (0, 0), (32, 32), leftBrightness, thickness=-1) cv.PutText(lt, "%3.0f" % leftBrightness[0], (3,20), font, cv.RealScalar(0)) cv.And(im, rm, rt) rightBrightness = cv.Avg(im, mask=rm) cv.Rectangle(rt, (0, 0), (32, 32), rightBrightness, thickness=-1) cv.PutText(rt, "%3.0f" % rightBrightness[0], (3,20), font, cv.RealScalar(0)) cv.ShowImage('cam', im) cv.ShowImage('left', lt) cv.ShowImage('right', rt) key = cv.WaitKey(10) #print "key ",key if key == 27: break elif key == 65362:
if __name__ == "__main__": A = [[1, 1], [0, 1]] img = cv.CreateImage((500, 500), 8, 3) kalman = cv.CreateKalman(2, 1, 0) state = cv.CreateMat(2, 1, cv.CV_32FC1) # (phi, delta_phi) process_noise = cv.CreateMat(2, 1, cv.CV_32FC1) measurement = cv.CreateMat(1, 1, cv.CV_32FC1) rng = cv.RNG(-1) code = -1L cv.Zero(measurement) cv.NamedWindow("Kalman", 1) while True: cv.RandArr(rng, state, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.1)) kalman.transition_matrix[0, 0] = 1 kalman.transition_matrix[0, 1] = 1 kalman.transition_matrix[1, 0] = 0 kalman.transition_matrix[1, 1] = 1 cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-5)) cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1)) cv.RandArr(rng, kalman.state_post, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.1)) while True:
if i == 0: # This happens only one time to initialize the kalman Filter with the first (x,y) point kalman.state_pre[0, 0] = x kalman.state_pre[1, 0] = y kalman.state_pre[2, 0] = 0 kalman.state_pre[3, 0] = 0 # set kalman transition matrix kalman.transition_matrix[0, 0] = 1 kalman.transition_matrix[1, 1] = 1 kalman.transition_matrix[2, 2] = 1 kalman.transition_matrix[3, 3] = 1 # set Kalman Filter cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-5)) ## 1e-5 cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(0.1)) else: # Kalman prediction with Kalman Correction with the points I have in trajectory_0000.txt kalman_prediction = cv.KalmanPredict(kalman) rightPoints = cv.CreateMat(2, 1, cv.CV_32FC1) rightPoints[0, 0] = x rightPoints[1, 0] = y kalman.state_pre[0, 0] = x kalman.state_pre[1, 0] = y kalman.state_pre[2, 0] = 0 kalman.state_pre[3, 0] = 0
#! /usr/bin/env python print "OpenCV Python version of contours" # import the necessary things for OpenCV import cv2.cv as cv # some default constants _SIZE = 500 _DEFAULT_LEVEL = 3 # definition of some colors _red = (0, 0, 255, 0) _green = (0, 255, 0, 0) _white = cv.RealScalar(255) _black = cv.RealScalar(0) # the callback on the trackbar, to set the level of contours we want # to display def on_trackbar(position): # create the image for putting in it the founded contours contours_image = cv.CreateImage((_SIZE, _SIZE), 8, 3) # compute the real level of display, given the current position levels = position - 3 # initialisation _contours = contours