def init_capture_device(device=-1):
print "init capture device"
capture = cv.CreateCameraCapture(device)
# check that capture device is OK
if not capture:
print "Error opening capture device"
sys.exit(1)
#call specific camera initialization
camera_init(capture)
#Get inital image to set parameters
image = cv.QueryFrame(capture) #inital image
CAMERA_WIDTH = cv.GetSize(image)[0]
#print CAMERA_WIDTH
CAMERA_HEIGHT = cv.GetSize(image)[1]
#print CAMERA_HEIGHT
#load calibration
intrinsics = cv.Load(INTRINSICS_FILE)
distortion = cv.Load(DISTORTION_FILE)
mapx = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, distortion, mapx, mapy)
return capture, mapx, mapy
def undistort(self, img):
# intrinsic parameters
fx = self.C[0, 0]
fy = self.C[1, 1]
cx = self.C[0, 2]
cy = self.C[1, 2]
# radial distortion coefficients
k1, k2 = self.dist[0:2]
# †angential distortion coefficients
p1, p2 = self.dist[2:4]
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.Zero(intrinsics)
intrinsics[0, 0] = float(fx)
intrinsics[1, 1] = float(fy)
intrinsics[2, 2] = 1.0
intrinsics[0, 2] = float(cx)
intrinsics[1, 2] = float(cy)
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = float(k1)
dist_coeffs[0, 1] = float(k2)
dist_coeffs[0, 2] = float(p1)
dist_coeffs[0, 3] = float(p2)
#src = cv.LoadImage(src)
src = cv.fromarray(img)
dst = img.copy()
dst = cv.fromarray(dst)
#dst = cv.CreateImage(cv.GetSize(src), src.type, src.channels)
mapx = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, dist_coeffs, mapx, mapy)
cv.Remap(src, dst, mapx,
mapy, cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS,
cv.ScalarAll(0))
# cv.Undistort2(src,dst, intrinsics, dist_coeffs)
return np.array(dst)
ipts = mk_image_points(goodcorners)
opts = mk_object_points(len(goodcorners), .1)
npts = mk_point_counts(len(goodcorners))
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
distortion = cv.CreateMat(4, 1, cv.CV_64FC1)
cv.SetZero(intrinsics)
cv.SetZero(distortion)
# focal lengths have 1/1 ratio
intrinsics[0, 0] = 1.0
intrinsics[1, 1] = 1.0
cv.CalibrateCamera2(opts,
ipts,
npts,
cv.GetSize(images[0]),
intrinsics,
distortion,
cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1),
flags=0) # cv.CV_CALIB_ZERO_TANGENT_DIST)
print "D =", list(cvmat_iterator(distortion))
print "K =", list(cvmat_iterator(intrinsics))
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, distortion, mapx, mapy)
for img in images:
r = cv.CloneMat(img)
cv.Remap(img, r, mapx, mapy)
cv.ShowImage("snap", r)
cv.WaitKey()