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 __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, 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 __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 from_message(self, msgs, alpha=0.0):
""" Initialize the camera calibration from a pair of CameraInfo messages. """
self.size = (msgs[0].width, msgs[0].height)
self.T = cv.CreateMat(3, 1, cv.CV_64FC1)
self.R = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.SetIdentity(self.T)
cv.SetIdentity(self.R)
self.l.from_message(msgs[0])
self.r.from_message(msgs[1])
# Need to compute self.T and self.R here, using the monocular parameters above
if False:
self.set_alpha(0.0)
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 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 cal_fromcorners(self, good):
# Perform monocular calibrations
lcorners = [(l, b) for (l, r, b) in good]
rcorners = [(r, b) for (l, r, b) in good]
self.l.cal_fromcorners(lcorners)
self.r.cal_fromcorners(rcorners)
lipts = self.mk_image_points(lcorners)
ripts = self.mk_image_points(rcorners)
boards = [b for (_, _, b) in good]
opts = self.mk_object_points(boards, True)
npts = self.mk_point_counts(boards)
flags = cv2.CALIB_FIX_INTRINSIC
self.T = cv.CreateMat(3, 1, cv.CV_64FC1)
self.R = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.SetIdentity(self.T)
cv.SetIdentity(self.R)
cv.StereoCalibrate(
opts,
lipts,
ripts,
npts,
self.l.intrinsics,
self.l.distortion,
self.r.intrinsics,
self.r.distortion,
self.size,
self.R, # R
self.T, # T
cv.CreateMat(3, 3, cv.CV_32FC1), # E
cv.CreateMat(3, 3, cv.CV_32FC1), # F
(cv.CV_TERMCRIT_ITER + cv.CV_TERMCRIT_EPS, 1, 1e-5),
flags)
self.set_alpha(0.0)
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))
def cal_fromcorners(self, good):
"""
:param good: Good corner positions and boards
:type good: [(corners, ChessboardInfo)]
"""
boards = [b for (_, b) in good]
ipts = self.mk_image_points(good)
opts = self.mk_object_points(boards)
npts = self.mk_point_counts(boards)
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
if self.calib_flags & cv2.CALIB_RATIONAL_MODEL:
distortion = cv.CreateMat(8, 1, cv.CV_64FC1) # rational polynomial
else:
distortion = cv.CreateMat(5, 1, cv.CV_64FC1) # plumb bob
cv.SetZero(intrinsics)
cv.SetZero(distortion)
# If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio
intrinsics[0, 0] = 1.0
intrinsics[1, 1] = 1.0
cv.CalibrateCamera2(opts,
ipts,
npts,
self.size,
intrinsics,
distortion,
cv.CreateMat(len(good), 3, cv.CV_32FC1),
cv.CreateMat(len(good), 3, cv.CV_32FC1),
flags=self.calib_flags)
self.intrinsics = intrinsics
self.distortion = distortion
# R is identity matrix for monocular calibration
self.R = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.SetIdentity(self.R)
self.P = cv.CreateMat(3, 4, cv.CV_64FC1)
cv.SetZero(self.P)
self.mapx = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.mapy = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.set_alpha(0.0)
def KalmanFilter():
kf = cv.CreateKalman(6, 2, 0)
'''
init the prediction/evolution/transition matrix
| 1 0 1 0 .5 0 | x | | x + vx + .5ax |
| 0 1 0 1 0 .5 | y | = | y + vy + .5ay |
| 0 0 1 0 1 0 | vx | | vx + ax |
| 0 0 0 1 0 1 | vy | | vy + ay |
| 0 0 k 0 0 0 | ax | | k*vx |
| 0 0 0 0 0 1 | ay | | ay |
'''
# diagonals
for j in range(6):
for k in range(6):
kf.transition_matrix[j, k] = 0
kf.transition_matrix[j, j] = 1
# x + vx + 0.5ax
kf.transition_matrix[0, 2] = 1
kf.transition_matrix[0, 4] = 0.5
# y + vy + 0.5ay
kf.transition_matrix[1, 3] = 1
kf.transition_matrix[1, 5] = 0.5
# vx + ax
kf.transition_matrix[2, 4] = 1
# vy + ay
kf.transition_matrix[3, 5] = 1
# predict ax = k * vx
kf.transition_matrix[4, 4] = 0
kf.transition_matrix[4, 2] = horizontal_acc_const
'''
measurement matrix H: mean = H * state
| 1 0 | x | | x |
| 0 1 | y | = | y |
'''
kf.measurement_matrix[0, 0] = 1
kf.measurement_matrix[1, 1] = 1
# process noise cov matrix Q: models the EXTERNAL uncertainty
cv.SetIdentity(kf.process_noise_cov, PN_Cov)
# measurement noise cov matrix R: covariance of SENSOR noise
cv.SetIdentity(kf.measurement_noise_cov, Sensor_Cov)
'''
error estimate covariance matrix P: relates the correlation of state vars
priori: before measurement
posteriori: after measurement
diagonals are all 1. x-vy and y-vy also correlated.
| xx xy xvx xvy xax xay | | 1 0 1 0 0 0 |
| yx yy yvx yvy yax yay | | 0 1 0 1 0 0 |
| vxx vxy vxvx vxvy vxax vxay | = | 1 0 1 0 0 0 |
| vyx vyy vyvx vyvy vyax vyay | | 0 1 0 1 0 0 |
| axx axy axvx axvy axax axay | | 0 0 0 0 1 0 |
| ayx ayy ayvx ayvy ayax ayay | | 0 0 0 0 0 1 |
'''
cv.SetIdentity(kf.error_cov_post, 1)
kf.error_cov_post[0, 2] = 1
kf.error_cov_post[1, 3] = 1
kf.error_cov_post[2, 0] = 1
kf.error_cov_post[3, 1] = 1
return kf
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:
def calc_point(angle):
return (cv.Round(img.width / 2 + img.width / 3 * cos(angle)),
cv.Round(img.height / 2 - img.width / 3 * sin(angle)))
state_angle = state[0, 0]
state_pt = calc_point(state_angle)
