def main(argv):
if len(argv) < 10:
print('Usage: %s input-file fx fy cx cy k1 k2 p1 p2 output-file' %
argv[0])
sys.exit(-1)
src = argv[1]
fx, fy, cx, cy, k1, k2, p1, p2, output = argv[2:]
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)
dst = cv.CreateImage(cv.GetSize(src), src.depth, src.nChannels)
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)
cv.SaveImage(output, dst)
def dewarp(imagedir):
# Loading from json file
C = CameraParams()
C.load(imagedir+"/params.json")
K = cv.fromarray(C.K)
D = cv.fromarray(C.D)
print "loaded camera parameters"
mapx = None
mapy = None
for f in os.listdir(imagedir):
if (f.find('pgm')<0):
continue
image = imagedir+'/'+f
print image
original = cv.LoadImage(image,cv.CV_LOAD_IMAGE_GRAYSCALE)
dewarped = cv.CloneImage(original);
# setup undistort map for first time
if (mapx == None or mapy == None):
im_dims = (original.width, original.height)
mapx = cv.CreateImage( im_dims, cv.IPL_DEPTH_32F, 1 );
mapy = cv.CreateImage( im_dims, cv.IPL_DEPTH_32F, 1 );
cv.InitUndistortMap(K,D,mapx,mapy)
cv.Remap( original, dewarped, mapx, mapy )
tmp1=cv.CreateImage((im_dims[0]/2,im_dims[1]/2),8,1)
cv.Resize(original,tmp1)
tmp2=cv.CreateImage((im_dims[0]/2,im_dims[1]/2),8,1)
cv.Resize(dewarped,tmp2)
cv.ShowImage("Original", tmp1 )
cv.ShowImage("Dewarped", tmp2)
cv.WaitKey(-1)
def rectifyImage(self, raw, rectified):
"""
:param raw: input image
:type raw: :class:`CvMat` or :class:`IplImage`
:param rectified: rectified output image
:type rectified: :class:`CvMat` or :class:`IplImage`
Applies the rectification specified by camera parameters :math:`K` and and :math:`D` to image `raw` and writes the resulting image `rectified`.
"""
self.mapx = cv.CreateImage((self.width, self.height), cv.IPL_DEPTH_32F,
1)
self.mapy = cv.CreateImage((self.width, self.height), cv.IPL_DEPTH_32F,
1)
cv.InitUndistortMap(self.K, self.D, self.mapx, self.mapy)
cv.Remap(raw, rectified, self.mapx, self.mapy)
def _make_undistort_matrices(self):
p = self.config
some_arr = np.array([[
p['focal_length_x_in_pixels'], 0, p['optical_center_x_in_pixels']
], [0, p['focal_length_y_in_pixels'], p['optical_center_y_in_pixels']],
[0, 0, 1.0]])
self.intrinsic_cvmat = ad.array2cvmat(some_arr)
self.distortion_cvmat = ad.array2cvmat(
np.array([[
p['lens_distortion_radial_1'], p['lens_distortion_radial_2'],
p['lens_distortion_tangential_1'],
p['lens_distortion_tangential_2']
]]))
self.size = (int(p['calibration_image_width']),
int(p['calibration_image_height']))
#Sanity check
size_image = cv.QueryFrame(self.capture)
camera_image_size = cv.GetSize(size_image)
if not ((camera_image_size[0] == self.size[0]) and
(camera_image_size[1] == self.size[1])):
raise RuntimeError(
'Size of image returned by camera and size declared in config. file do not match.'
+ ' Config:' + str(self.size) + ' Camera: ' +
str(camera_image_size))
#Set up buffers for undistortion
self.raw_image = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 1)
self.gray_image = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 1)
self.undistort_mapx = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.undistort_mapy = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.unbayer_image = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 3)
self.color = p['color']
if self.color == True:
self.cvbayer_pattern = p['opencv_bayer_pattern']
if self.color == True:
self.undistort_image = cv.CreateImage(self.size, cv.IPL_DEPTH_8U,
3)
else:
self.undistort_image = cv.CreateImage(self.size, cv.IPL_DEPTH_8U,
1)
cv.InitUndistortMap(self.intrinsic_cvmat, self.distortion_cvmat,
self.undistort_mapx, self.undistort_mapy)
self.corrected_orientation = None
translation_vectors = cv.CreateMat(successes, 3, cv.CV_32FC1)
cv.SetZero(translation_vectors)
cv.CalibrateCamera2(object_points2, image_points2, point_counts2,
cv.GetSize(image), intrinsic, distortion, rotation_vectors,
translation_vectors, 0)
cv.Save("Intrinsics.xml", intrinsic)
cv.Save("Distortion.xml", distortion)
intrinsic = cv.Load("Intrinsics.xml")
distortion = cv.Load("Distortion.xml")
mapx = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsic, distortion, mapx, mapy)
cv.NamedWindow("Undistort")
while (image):
t = cv.CloneImage(image)
cv.ShowImage("Raw Video", image)
cv.Remap(t, image, mapx, mapy)
cv.ShowImage("Undistort", image)
c = cv.WaitKey(15)
if (c == ord('p')):
c = 0
while (c != ord('p') and c != 27):
c = cv.WaitKey(250)
if (c == 27):
term_crit = (cv.CV_TERMCRIT_ITER+cv.CV_TERMCRIT_EPS, 50, 1e-6)
cv.StereoCalibrate(opts, ipts1, ipts2, npts,
intrinsics1, distortion1, intrinsics2, distortion2,
cv.GetSize(images1[0]), R, T, E, F,
term_crit,
cv.CV_CALIB_FIX_INTRINSIC)
#print "E =",list(cvmat_iterator(E))
#print "F =",list(cvmat_iterator(F))
print "D1 =", list(cvmat_iterator(distortion1))
print "K1 =", list(cvmat_iterator(intrinsics1))
print "D2 =", list(cvmat_iterator(distortion2))
print "K2 =", list(cvmat_iterator(intrinsics2))
print "R =", list(cvmat_iterator(R))
print "T =", list(cvmat_iterator(T))
'''
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics1, distortion1, mapx, mapy)
for img in images1:
r = cv.CloneMat(img)
cv.Remap(img, r, mapx, mapy)
cv.ShowImage("snap", r)
cv.WaitKey()
'''
def VirtualMirror():
cv.NamedWindow("RGB_remap", cv.CV_WINDOW_NORMAL)
cv.NamedWindow("Depth_remap", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow('dst', cv.CV_WINDOW_NORMAL)
cv.SetMouseCallback("Depth_remap", on_mouse, None)
print "Virtual Mirror"
print "Calibrated 4 Screen corner= ", sn4_ref
print "Corner 1-2 = ", np.linalg.norm(sn4_ref[0] - sn4_ref[1])
print "Corner 2-3 = ", np.linalg.norm(sn4_ref[1] - sn4_ref[2])
print "Corner 3-4 = ", np.linalg.norm(sn4_ref[2] - sn4_ref[3])
print "Corner 4-1 = ", np.linalg.norm(sn4_ref[3] - sn4_ref[0])
global head_pos
global head_virtual
global scene4_cross
head_pos = np.array([-0.2, -0.2, 1.0]) #Head_detect()
while 1:
(depth, _) = freenect.sync_get_depth()
(rgb, _) = freenect.sync_get_video()
#print type(depth)
img = array2cv(rgb[:, :, ::-1])
im = array2cv(depth.astype(np.uint8))
#modulize this part for update_on() and loopcv()
#q = depth
X, Y = np.meshgrid(range(640), range(480))
d = 2 #downsampling if need
projpts = calibkinect.depth2xyzuv(depth[::d, ::d], X[::d, ::d],
Y[::d, ::d])
xyz, uv = projpts
if tracking == 0:
#*********************************
if pt is not None:
print "=================="
(x_d, y_d) = pt
print "x=", x_d, " ,y=", y_d
#print depth.shape
#Watch out the indexing for depth col,row = 480,640
d_raw = np.array([depth[y_d, x_d]])
u_d = np.array([x_d])
v_d = np.array([y_d])
print "d_raw= ", d_raw
print "u_d= ", u_d
print "v_d= ", v_d
head3D, head2D = calibkinect.depth2xyzuv(d_raw, u_d, v_d)
print "XYZ=", head3D
print "XYZonRGBplane=", head2D
head_pos = head3D[0]
#print "head_pos.shape",head_pos.shape
print "head_pos= ", head_pos
cv.WaitKey(100)
cv.Circle(im, (x_d, y_d), 4, (0, 0, 255, 0), -1, 8, 0)
cv.Circle(im, (int(head2D[0, 0]), int(head2D[0, 1])), 2,
(255, 255, 255, 0), -1, 8, 0)
#*********************************
elif tracking == 1:
#find the nearest point (nose) as reference for right eye position
print "nose"
inds = np.nonzero(xyz[:, 2] > 0.5)
#print xyz.shape
new_xyz = xyz[inds]
#print new_xyz.shape
close_ind = np.argmin(new_xyz[:, 2])
head_pos = new_xyz[close_ind, :] + (0.03, 0.04, 0.01)
#print head_pos.shape
#print head_pos
elif tracking == 2:
#find the closest point as eye posiiton
print "camera"
inds = np.nonzero(xyz[:, 2] > 0.5)
#print xyz.shape
new_xyz = xyz[inds]
#print new_xyz.shape
close_ind = np.argmin(new_xyz[:, 2])
head_pos = new_xyz[close_ind, :]
#print head_pos.shape
#print head_pos
else:
print "please select a tracking mode"
head_virtual = MirrorReflection(sn4_ref[0:3, :], head_pos)
print "head_virtual= ", head_virtual
rgbK = np.array([[520.97092069697146, 0.0, 318.40565581396697],
[0.0, 517.85544366622719, 263.46756370601804],
[0.0, 0.0, 1.0]])
rgbD = np.array([[0.22464481251757576], [-0.47968370787671893], [0.0],
[0.0]])
irK = np.array([[588.51686020601733, 0.0, 320.22664144213843],
[0.0, 584.73028132692866, 241.98395817513071],
[0.0, 0.0, 1.0]])
irD = np.array([[-0.1273506872313161], [0.36672476189160591], [0.0],
[0.0]])
mapu = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapv = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(rgbK, rgbD, mapu, mapv)
cv.InitUndistortMap(irK, irD, mapx, mapy)
if 1:
rgb_remap = cv.CloneImage(img)
cv.Remap(img, rgb_remap, mapu, mapv)
depth_remap = cv.CloneImage(im)
cv.Remap(im, depth_remap, mapx, mapy)
scene4_cross = Cross4Pts.CrossPts(xyz, uv, head_pos, head_virtual,
sn4_ref)
#[warp] Add whole warpping code here
#[warp] points = Scene4Pts() as warpping 4 pts
#Flip the dst image!!!!!!!!!
#ShowImage("rgb_warp", dst)
#Within/out of the rgb range
#Mapping Destination (width, height)=(x,y)
#Warning: the order of pts in clockwise: pt1(L-T),pt2(R-T),pt3(R-B),pt4(L-B)
#points = [(test[0,0],test[0,1]), (630.,300.), (700.,500.), (400.,470.)]
points = [(scene4_cross[0, 0], scene4_cross[0, 1]),
(scene4_cross[1, 0], scene4_cross[1, 1]),
(scene4_cross[2, 0], scene4_cross[2, 1]),
(scene4_cross[3, 0], scene4_cross[3, 1])]
#Warping the image without flipping (camera image)
#npoints = [(0.,0.), (640.,0.), (640.,480.), (0.,480.)]
#Warping the image with flipping (mirror flip image)
npoints = [(640., 0.), (0., 0.), (0., 480.), (640., 480.)]
mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(points, npoints, mat)
#src = cv.CreateImage( cv.GetSize(img), cv.IPL_DEPTH_32F, 3 )
src = cv.CreateImage(cv.GetSize(rgb_remap), cv.IPL_DEPTH_32F, 3)
#cv.ConvertScale(img,src,(1/255.00))
cv.ConvertScale(rgb_remap, src, (1 / 255.00))
dst = cv.CloneImage(src)
cv.Zero(dst)
cv.WarpPerspective(src, dst, mat)
#************************************************************************
#Remap the rgb and depth image
#Warping will use remap rgb image as src
if 1:
cv.ShowImage("RGB_remap", rgb_remap) #rgb[200:440,300:600,::-1]
cv.ShowImage("Depth_remap", depth_remap)
cv.ShowImage("dst", dst) #warp rgb image
if cv.WaitKey(5) == 27:
cv.DestroyWindow("RGB_remap")
cv.DestroyWindow("Depth_remap")
cv.DestroyWindow("dst")
break
intrinsics[1, 2] = float(3288 / 2)
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = float(-1)
dist_coeffs[0, 1] = float(-1) #loat(0.0193617)
dist_coeffs[0, 2] = float(-.1) #float(-0.002004)
dist_coeffs[0, 3] = float(-.1) #float(-0.002056)
#End Camera Matrix
allFiles = []
for root, dirs, files in os.walk(startdir + "/"):
allFiles += [os.path.join(root, name) for name in files if ".jpg" in name]
for im in allFiles:
#src = "2015-03-07 11.07.16.jpg"
src = cv.LoadImage(im)
size = cv.GetSize(src)
s = (int(size[0] * 0.9), int(size[1] * 0.9))
res = cv.CreateImage(s, src.depth, src.nChannels)
im1, im2 = cv.CreateImage(s, cv.IPL_DEPTH_32F,
1), cv.CreateImage(s, cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, dist_coeffs, im1, im2)
cv.Remap(src, res, im1, im2, cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS,
cv.ScalarAll(0))
print im.strip(".jpg") + "_nodistort.jpg"
cv.SaveImage(im.strip(".jpg") + "_nodistort.jpg", res)