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
