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__(self, processNoiseCovariance=1e-2, 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
'''
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, process_noise_cov=(1.0, 800.0)):
self.kal = cv.CreateKalman(2, 1, 0)
cv.SetIdentity(self.kal.transition_matrix, 1.0)
cv.SetIdentity(self.kal.measurement_matrix, 1.0)
cv.SetIdentity(self.kal.process_noise_cov, 1.0)
self.kal.process_noise_cov[0, 0] = process_noise_cov[0]
self.kal.process_noise_cov[1, 1] = process_noise_cov[1]
cv.SetIdentity(self.kal.measurement_noise_cov, 1.0)
cv.SetIdentity(self.kal.error_cov_post, 1.0)
self.measurement = cv.CreateMat(1, 1,
cv.GetElemType(self.kal.state_pre))
self.t_previous = 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,
initial_val=[0],
p_noise=[1e-2],
m_noise=[1e-3],
m_mat=[1],
ecv=[1]):
self.kalman = cv.CreateKalman(len(initial_val), len(initial_val), 0)
self.measurement = cv.CreateMat(len(initial_val), 1, cv.CV_32FC1)
self.prediction = None
cv.Zero(self.measurement)
cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(*m_mat))
cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(*p_noise))
cv.SetIdentity(self.kalman.measurement_noise_cov,
cv.RealScalar(*m_noise))
cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(*ecv))
for v in initial_val:
self.kalman.state_post[initial_val.index(v), 0] = v
self.kalman.state_pre[initial_val.index(v), 0] = v
def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
kalman = cv.CreateKalman(6, 4)
kalman.transition_matrix[0, 2] = 1
kalman.transition_matrix[0, 4] = 1. / 2
kalman.transition_matrix[1, 3] = 1
kalman.transition_matrix[1, 5] = 1. / 2
kalman.transition_matrix[2, 4] = 1
kalman.transition_matrix[3, 5] = 1
cv.SetIdentity(kalman.measurement_matrix, 1.)
cv.SetIdentity(kalman.process_noise_cov, processNoiseCov)
cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov)
cv.SetIdentity(kalman.error_cov_post, 1.)
p = positions[0]
v = velocities[0]
v2 = velocities[2]
a = (v2 - v).multiply(0.5)
kalman.state_post[0, 0] = p.x
kalman.state_post[1, 0] = p.y
kalman.state_post[2, 0] = v.x
kalman.state_post[3, 0] = v.y
kalman.state_post[4, 0] = a.x
kalman.state_post[5, 0] = a.y
filteredPositions = moving.Trajectory()
filteredVelocities = moving.Trajectory()
measurement = cv.CreateMat(4, 1, cv.CV_32FC1)
for i in xrange(positions.length()):
cv.KalmanPredict(kalman) # no control
p = positions[i]
v = velocities[i]
measurement[0, 0] = p.x
measurement[1, 0] = p.y
measurement[2, 0] = v.x
measurement[3, 0] = v.y
cv.KalmanCorrect(kalman, measurement)
filteredPositions.addPositionXY(kalman.state_post[0, 0],
kalman.state_post[1, 0])
filteredVelocities.addPositionXY(kalman.state_post[2, 0],
kalman.state_post[3, 0])
return (filteredPositions, filteredVelocities)
def do_calibration(self):
if not self.goodenough:
print "Can not calibrate yet!"
return
#append all things in db
good_corners = [
corners for (params, corners, object_points) in self.db
]
good_points = [
object_points for (params, corners, object_points) in self.db
]
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
opts = self.mk_object_points(good_points)
ipts = self.mk_image_points(good_corners)
npts = self.mk_point_counts(good_points)
cv.CalibrateCamera2(opts,
ipts,
npts,
self.size,
intrinsics,
distortion,
cv.CreateMat(len(good_corners), 3, cv.CV_32FC1),
cv.CreateMat(len(good_corners), 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)
ncm = cv.GetSubRect(self.P, (0, 0, 3, 3))
cv.GetOptimalNewCameraMatrix(self.intrinsics, self.distortion,
self.size, self.alpha, ncm)
print self.size
self.calibrated = True
return (self.distortion, self.intrinsics, self.R, self.P)
def __init__(self):
self.initialized = False
self.kal = cv.CreateKalman(4, 4, 0)
cv.SetIdentity(self.kal.transition_matrix)
cv.SetIdentity(self.kal.measurement_matrix)
cv.SetIdentity(self.kal.process_noise_cov, 1.0)
self.kal.process_noise_cov[2, 2] = 0.5
self.kal.process_noise_cov[3, 3] = 0.5
cv.SetIdentity(self.kal.measurement_noise_cov, 80.0)
self.kal.measurement_noise_cov[0, 0] = 5E-5 #15.341
self.kal.measurement_noise_cov[0, 1] = 8E-6 # 2.354
self.kal.measurement_noise_cov[1, 0] = 8E-6 #1E-2 # 2.354
self.kal.measurement_noise_cov[
1, 1] = 5E-5 # Likely to be similar to the x value.
self.kal.measurement_noise_cov[2, 2] = 40.0 #0.22291
self.kal.measurement_noise_cov[3, 3] = 40.0 #0.21742
self.measurement = cv.CreateMat(4, 1,
cv.GetElemType(self.kal.state_pre))
self.t = None
self.zf = None
def calibrate(self):
image_points_mat = concatenate_mats(self.image_points)
object_points_mat = concatenate_mats(self.object_points)
print "Image Points:"
for row in range(image_points_mat.height):
for col in range(image_points_mat.width):
print image_points_mat[row, col]
print
print "Object Points:"
for row in range(object_points_mat.height):
for col in range(object_points_mat.width):
print object_points_mat[row, col]
print
point_counts_mat = cv.CreateMat(1, self.number_of_scenes, cv.CV_32SC1)
for i in range(self.number_of_scenes):
point_counts_mat[0, i] = self.image_points[i].width
intrinsics = cv.CreateMat(3, 3, cv.CV_32FC1)
distortion = cv.CreateMat(4, 1, cv.CV_32FC1)
cv.SetZero(intrinsics)
cv.SetZero(distortion)
size = (self.projector_info.width, self.projector_info.height)
tvecs = cv.CreateMat(self.number_of_scenes, 3, cv.CV_32FC1)
rvecs = cv.CreateMat(self.number_of_scenes, 3, cv.CV_32FC1)
cv.CalibrateCamera2(object_points_mat,
image_points_mat,
point_counts_mat,
size,
intrinsics,
distortion,
rvecs,
tvecs,
flags=0)
R = cv.CreateMat(3, 3, cv.CV_32FC1)
P = cv.CreateMat(3, 4, cv.CV_32FC1)
cv.SetIdentity(R)
cv.SetZero(P)
cv.Copy(intrinsics, cv.GetSubRect(P, (0, 0, 3, 3)))
self.projector_info = create_msg(size, distortion, intrinsics, R, P)
rospy.loginfo(self.projector_info)
for col in range(3):
for row in range(self.number_of_scenes):
print tvecs[row, col],
print
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 InitPredistortMap(cameraMatrix, distCoeffs, map1, map2):
rows = map1.height
cols = map1.width
mat = cv.CreateMat(1, rows * cols, cv.CV_32FC2)
for row in range(rows):
for col in range(cols):
mat[0, row * cols + col] = (col, row)
R = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.SetIdentity(R)
P = cv.CreateMat(3, 4, cv.CV_64FC1)
cv.SetZero(P)
cv.Copy(cameraMatrix, cv.GetSubRect(P, (0, 0, 3, 3)))
cv.UndistortPoints(mat, mat, cameraMatrix, distCoeffs, R, P)
mat = cv.Reshape(mat, 2, rows)
cv.Split(mat, map1, map2, None, None)
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)
def really_do_posit(locsar,
tags,
viewpoint,
FOCAL_LENGTH,
refpoints=[],
qimgsize=(0, 0)):
# change 3d coordinate systems
c = map(lambda entry: ({
'x': entry[0][0],
'y': entry[0][1]
}, entry[1]), locsar)
locpairs = em.ddImageLocstoLPT(viewpoint, c)
#translate to POSIT Specs
pts2d = map(lambda x: x[0], locpairs)
translation2d = (qimgsize[0] / 2.0, qimgsize[1] / 2.0)
pts2d = map(
lambda x: (x[0][0] - translation2d[0], x[0][1] - translation2d[1]),
locpairs)
translation3d = locpairs[0][1]
pts3d = map(lambda x: tuple(x[1] - translation3d), locpairs)
#convert tag coordinate system
c = map(
lambda x: ({
'x': 0,
'y': 0
}, {
'lat': x.lat,
'lon': x.lon,
'alt': x.alt
}), tags)
c2 = map(
lambda x: ({
'x': 0,
'y': 0
}, {
'lat': x['lat'],
'lon': x['lon'],
'alt': x['alt']
}), refpoints)
taglocpairs = em.ddImageLocstoLPT(viewpoint, c)
reflocpairs = em.ddImageLocstoLPT(viewpoint, c2)
tagpts3d = map(lambda x: tuple(x[1] - translation3d), taglocpairs)
refpts3d = map(lambda x: tuple(x[1] - translation3d), reflocpairs)
#compute POSIT
positobj = cv.CreatePOSITObject(pts3d)
rotMat, transVec = cv.POSIT(positobj, pts2d, FOCAL_LENGTH,
(cv.CV_TERMCRIT_EPS, 0, 0.000001))
# print "rotation matrix:\t{0}".format(rotMat)
# print "translation matrix:\t{0}".format(transVec)
#change rotMat to cvMat
rotMatCV = cv.CreateMat(3, 3, cv.CV_64F)
for i in range(0, 3):
for j in range(0, 3):
cv.Set2D(rotMatCV, i, j, rotMat[i][j])
#convert rotMatrix to rotVector
rotVec = cv.CreateMat(3, 1, cv.CV_64F)
cv.Rodrigues2(rotMatCV, rotVec)
#change transVec to cvMat
transVecCV = cv.CreateMat(1, 3, cv.CV_64F)
for i in range(0, 3):
transVecCV[0, i] = transVec[i]
#camera matrix
cameratrans = cv.CreateMat(3, 3, cv.CV_64F)
cv.SetIdentity(cameratrans)
cameratrans[0, 0] = FOCAL_LENGTH
cameratrans[1, 1] = FOCAL_LENGTH
#distCoefs
distCoef = cv.CreateMat(4, 1, cv.CV_64F)
cv.SetZero(distCoef)
#change 3d coordinate data format
pts3d_mat = cv.CreateMat(len(pts3d), 1, cv.CV_64FC3)
for i, m in enumerate(pts3d):
cv.Set2D(pts3d_mat, i, 0, cv.Scalar(*m))
#project points
d2 = cv.CreateMat(pts3d_mat.rows, 1, cv.CV_64FC2)
cv.ProjectPoints2(pts3d_mat, rotVec, transVecCV, cameratrans, distCoef, d2)
#compute self errors
xerrors = []
yerrors = []
for i in range(0, d2.rows):
xerror = abs(pts2d[i][0] - (d2[i, 0][0]))
yerror = abs(pts2d[i][1] - (d2[i, 0][1]))
xerrors.append(xerror)
yerrors.append(yerror)
print "avg xerror:\t {0}".format(sum(xerrors) / len(xerrors))
print "avg yerror:\t {0}".format(sum(yerrors) / len(yerrors))
#change tag 3d coordinate data format
tagpts3d_mat = cv.CreateMat(len(tagpts3d), 1, cv.CV_64FC3)
for i, m in enumerate(tagpts3d):
cv.Set2D(tagpts3d_mat, i, 0, cv.Scalar(*m))
refpts3d_mat = cv.CreateMat(len(refpts3d), 1, cv.CV_64FC3)
for i, m in enumerate(refpts3d):
cv.Set2D(refpts3d_mat, i, 0, cv.Scalar(*m))
#project points
d2 = cv.CreateMat(tagpts3d_mat.rows, 1, cv.CV_64FC2)
cv.ProjectPoints2(tagpts3d_mat, rotVec, transVecCV, cameratrans, distCoef,
d2)
d22 = cv.CreateMat(refpts3d_mat.rows, 1, cv.CV_64FC2)
cv.ProjectPoints2(refpts3d_mat, rotVec, transVecCV, cameratrans, distCoef,
d22)
ntags = []
nrefs = []
for i in range(0, d2.rows):
ntags.append((tags[i], (0, (d2[i, 0][0] + translation2d[0],
d2[i, 0][1] + translation2d[1]))))
for i in range(0, d22.rows):
nrefs.append((refpoints[i], (0, (d22[i, 0][0] + translation2d[0],
d22[i, 0][1] + translation2d[1]))))
return ntags, nrefs
def do_calibration(self):
if not self.goodenough:
print "Can not calibrate yet!"
return
#append all things in db
good_l_corners = [
lcorners for (lparams, rparams, lcorners, rcorners, lobject_points,
robject_points) in self.db
]
good_l_points = [
lobject_points for (lparams, rparams, lcorners, rcorners,
lobject_points, robject_points) in self.db
]
good_r_corners = [
rcorners for (lparams, rparams, lcorners, rcorners, lobject_points,
robject_points) in self.db
]
good_r_points = [
robject_points for (lparams, rparams, lcorners, rcorners,
lobject_points, robject_points) in self.db
]
# Perform monocular calibrations
self.l.do_calibration()
self.r.do_calibration()
## print "left intrinsics"
## print numpy.asarray(self.l.intrinsics)
## print "left distortion"
## print numpy.asarray(self.l.distortion)
## print "left projection"
## print numpy.asarray(self.l.P)
##
## print "right intrinsics"
## print numpy.asarray(self.r.intrinsics)
## print "right distortion"
## print numpy.asarray(self.r.distortion)
## print "right projection"
## print numpy.asarray(self.r.P)
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)
opts = self.mk_object_points(good_l_points)
l_ipts = self.mk_image_points(good_l_corners)
r_ipts = self.mk_image_points(good_r_corners)
npts = self.mk_point_counts(good_l_points)
cv.StereoCalibrate(
opts,
l_ipts,
r_ipts,
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)
## print "after cal left intrinsics"
## print numpy.asarray(self.l.intrinsics)
## print "after cal left distortion"
## print numpy.asarray(self.l.distortion)
##
## print "after cal right intrinsics"
## print numpy.asarray(self.r.intrinsics)
## print "after cal right distortion"
## print numpy.asarray(self.r.distortion)
##
## print "after cal R"
## print numpy.asarray(self.R)
## print "after cal T"
## print numpy.asarray(self.T)
cv.StereoRectify(self.l.intrinsics,
self.r.intrinsics,
self.l.distortion,
self.r.distortion,
self.size,
self.R,
self.T,
self.l.R,
self.r.R,
self.l.P,
self.r.P,
alpha=self.alpha)
## print "after rectify left intrinsics"
## print numpy.asarray(self.l.intrinsics)
## print "after rectify left distortion"
## print numpy.asarray(self.l.distortion)
##
## print "after rectify right intrinsics"
## print numpy.asarray(self.r.intrinsics)
## print "after rectify right distortion"
## print numpy.asarray(self.r.distortion)
##
## print "after rectify left R"
## print numpy.asarray(self.l.R)
## print "after rectify left P"
## print numpy.asarray(self.l.P)
## print "after rectify right R"
## print numpy.asarray(self.r.R)
## print "after rectify right P"
## print numpy.asarray(self.r.P)
return (self.l.distortion, self.l.intrinsics, self.l.R, self.l.P,
self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)
def __init__(self, cov, dynam_params, measure_params, control_params=0):
self.kf = cv.CreateKalman(dynam_params, measure_params, control_params)
cv.SetIdentity(self.kf.measurement_matrix, cv.RealScalar(1))
self.set_cov(cv.fromarray(cov))
