def getDepth(self, image, image2):
grayScaleFullImage = cv.CreateImage((image.width, image.height), 8, 1)
cv.CvtColor(image, grayScaleFullImage, cv.CV_BGR2GRAY)
grayScaleFullImage2 = cv.CreateImage((image2.width, image2.height), 8, 1)
cv.CvtColor(image2, grayScaleFullImage2, cv.CV_BGR2GRAY)
[mat_w, mat_h] = self.size
r = cv.CreateMat(mat_h, mat_w, cv.CV_8UC1)
r2 = cv.CreateMat(mat_h, mat_w, cv.CV_8UC1)
print type(r)
print type(image)
print type(self.map1x)
print cv.GetSize(r)
print cv.GetSize(self.map1x)
cv.Remap(grayScaleFullImage, r, self.map1x, self.map1y)
cv.Remap(grayScaleFullImage2, r2, self.map2x, self.map2y)
cv.ShowImage("win3", r)
cv.ShowImage("win4", r2)
#stereo_match that comes in opencv
# disparity range is tuned for 'aloe' image pair
window_size = 3
min_disp = 16
num_disp = 112 - min_disp
stereo = cv2.StereoSGBM(minDisparity=min_disp,
numDisparities=num_disp,
SADWindowSize=window_size,
uniquenessRatio=10,
speckleWindowSize=100,
speckleRange=32,
disp12MaxDiff=1,
P1=8 * 3 * window_size ** 2,
P2=32 * 3 * window_size ** 2,
fullDP=False
)
print 'computing disparity...'
disp = stereo.compute(np.asarray(r), np.asarray(r2)).astype(np.float32) / 16.0
print 'generating 3d point cloud...'
points = cv2.reprojectImageTo3D(disp, np.asarray(self.Q))
colors = cv2.cvtColor(np.asarray(r), cv2.COLOR_GRAY2RGB)
mask = disp > disp.min()
out_points = points[mask]
out_colors = colors[mask]
# Resulting .ply file cam be easily viewed using MeshLab ( http://meshlab.sourceforge.net
out_fn = 'out.ply'
write_ply('out.ply', out_points, out_colors)
print '%s saved' % 'out.ply'
cv2.imshow('disparity', (disp - min_disp) / num_disp)
def genView(panopath, outpath, orientation, viewdir, detail):
# panopath: location of raster, depth and plane_pano images
# outpath: location to put generated view, depth, and plane images
# orientation: [yaw, pitch, roll] of panorama (in degrees)
# viewdir: clock-based view; in set [2,3,4,8,9,10] for database
# detail: 0 = 768x512 with 60d fov, 1 = 2500x1200 with 90d fov
# local constants
start = time.time()
pi = np.pi
width, height, fov = (2500, 1200, 90) if detail else (768, 512, 60)
# view details
Rpano = geom.RfromYPR(orientation[0],orientation[1],orientation[2])
Yview = np.mod( orientation[0] + 30*viewdir, 360 )
Rview = geom.RfromYPR(Yview, 0, 0)
Kview = geom.cameramat(width, height, fov*pi/180)
Kinv = alg.inv(Kview)
# Load image pano, depth pano, and plane pano images
cvIP = cv.LoadImageM( os.path.join(panopath,'raster.jpg'), cv.CV_LOAD_IMAGE_UNCHANGED )
cvDP = cv.fromarray( np.asarray( Image.open( os.path.join(panopath,'depth.png') ) ) )
pp = np.asarray( Image.open( os.path.join(panopath,'plane_pano.png') ) ).copy()
vals = set(list(pp.reshape(-1)))
vals.remove(255)
gnd = max(vals)
pp[pp==gnd] = np.uint8(0)
cvPP = cv.fromarray(pp)
# load pixel map
pix = np.append(np.array(np.meshgrid(range(width),range(height))).reshape(2,-1),np.ones([1,width*height]),0)
midpoint = time.time()
print 'Loading pano images took ' + str(midpoint-start) + ' seconds.'
# Create output openCV matrices
cvI = cv.CreateMat(height,width,cv.CV_8UC3)
cvD = cv.CreateMat(height,width,cv.CV_32SC1)
cvP = cv.CreateMat(height,width,cv.CV_8UC1)
# compute mappings
ray = np.dot( np.transpose(Rpano), np.dot( Rview, np.dot( Kinv, pix ) ) )
yaw, pitch = np.arctan2( ray[0,:] , ray[2,:] ) , np.arctan2( -ray[1,:] , np.sqrt((np.array([ray[0,:],ray[2,:]])**2).sum(0)) )
ix, iy = cv.fromarray(np.array(8192/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array(4096/pi*(pi/2-pitch),np.float32).reshape(height,width))
dx, dy = cv.fromarray(np.array(5000/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array(2500/pi*(pi/2-pitch),np.float32).reshape(height,width))
px, py = cv.fromarray(np.array(1000/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array( 500/pi*(pi/2-pitch),np.float32).reshape(height,width))
# call remap function
cv.Remap(cvIP,cvI,ix,iy,cv.CV_INTER_CUBIC)
cv.Remap(cvDP,cvD,dx,dy,cv.CV_INTER_NN)
cv.Remap(cvPP,cvP,px,py,cv.CV_INTER_NN)
# write images to file
Image.fromarray(np.array(cvI)[:,:,[2,1,0]]).save(os.path.join(outpath,'view.jpg'),'jpeg')
Image.fromarray(np.array(cvD)).save(os.path.join(outpath,'depth.png'),'png')
Image.fromarray(np.array(cvP)).save(os.path.join(outpath,'plane.png'),'png')
print 'Generating views from pano took ' + str(time.time()-midpoint) + ' seconds.'
def remap(self, src):
"""
:param src: source image
:type src: :class:`cvMat`
Apply the post-calibration undistortion to the source image
"""
r = cv.CloneMat(src)
cv.Remap(src, r, self.mapx, self.mapy)
return r
def capture_image(capture, mapx=None, mapy=None):
img = cv.CloneImage(cv.QueryFrame(capture))
#Flip
if FLIP_IMAGE:
img2 = cv.CloneImage(img)
cv.Flip(img, img2, 0)
cv.Flip(img2, img, 1)
#undistort if calibration matrices were given
if mapx != None and mapy != None:
udimg = cv.CloneImage(img)
cv.Remap(img, udimg, mapx, mapy)
img = udimg
return img
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()
