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))