def fix_K(self, newK=None, new_fdist=None):
""" Modify depth image to adjust to a new intrinsics/focal
distance.
Usage: d_img.fix_K(newK = K)
d_img.fix_K(new_fdist = f)
Input:
newK = 3-by-3 intrinsics camera matrix
new_fdist = new focal distance (you should give either K or
the focal distance. If both are given, K takes preference.
Output:
-NONE- The depth image and intrinsics matrix are modified in place.
"""
if newK is not None:
KK = np.array([newK[0,0], newK[1,1], newK[0,2], newK[1,2]], \
dtype=float)
elif new_fdist is not None:
KK = np.array([new_fdist, new_fdist, self.height/2., \
self.width/2.], dtype=float)
else:
print 'Must input K or focal distance'
return
pts3D = self.tocloud()
# Order is important for Z-buffering
# We want points further away to be processed first
idx = pts3D[2,:].argsort()
pts3D = pts3D[:, idx[::-1]]
pts2D = utils.C_ProjectPts(pts3D, np.eye(4,4, dtype=float), KK)
# Now transfer points back to image
bounds = np.array([ [0, self.width-1], \
[0, self.height-1] ])
invalid_pts = utils.out_of_bounds(pts2D, bounds)
pts2D[:, invalid_pts] = 0
# Remember: IdxImg is in format [row col]
pts2D_int = np.array(np.round(pts2D), dtype=int)
self[:] = 0
self[pts2D_int[1,:], pts2D_int[0,:]] = pts3D[2,:]
self.K = np.eye(3,3)
self.K[0,0] = KK[0]
self.K[1,1] = KK[1]
self.K[0,2] = KK[2]
self.K[1,2] = KK[3]
return
def new_from_K(self, width=None, height=None, K=None):
""" New depth image changing size (interpolating) and modifying
intrinsics K.
Usage: new_d_img = d_img.new_from_K(width, height, K)
Input:
width - Width of output image
height - Height of output image
newK - 4-by-1 intrinsics camera matrix
Output:
new_d_img - Output depth image with different intrinsics and size.
"""
if K is None:
K = self.K
if len(K) == 9:
KK = np.r_[K[0,0], K[1,1], K[0,2], K[1,2]]
elif len(K) == 4:
KK = K
#TODO: Finish func
pts3D = self.tocloud()
# Order is important for Z-buffering
# We want points further away to be processed first
idx = pts3D[2,:].argsort()
pts3D = pts3D[:, idx[::-1]]
pts2D = utils.C_ProjectPts(pts3D, np.eye(4,4, dtype=float), KK)
# Now transfer points back to image
bounds = np.array([ [0, self.width-1], \
[0, self.height-1] ])
invalid_pts = utils.out_of_bounds(pts2D, bounds)
pts2D[:, invalid_pts] = 0
# Remember: IdxImg is in format [row col]
pts2D_int = np.array(np.round(pts2D), dtype=int)
self[:] = 0
self[pts2D_int[1,:], pts2D_int[0,:]] = pts3D[2,:]
self.K = np.eye(3,3)
self.K[0,0] = KK[0]
self.K[1,1] = KK[1]
self.K[0,2] = KK[2]
self.K[1,2] = KK[3]
return
def toDepthImage(self, width, height, K, viewpoint = None, zbuffer=True):
""" Compute Depth Image from point cloud.
Usage: dImg = cloud.toDepthImage(width, height, K, viewpoint)
Input:
width - Width of output image
height - Height of output image
K - numpy array, 3-by-3 intrinsics camera matrix
viewpoint - numpy array 4x4 extrinsic camera matrix, image to world
zbuffer{True} - Fill out missing values with Z-buffering
Output:
dImg - Output depth image from given viewpoint
"""
# Parse input arguments
if K is None:
K = self.K
if K.size == 9:
KK = np.r_[K[0,0], K[1,1], K[0,2], K[1,2]]
elif K.size == 4:
KK = K
if viewpoint is None:
viewpoint = np.eye(4,4, dtype=float)
pts3D = self.copy()
# Order is important for Z-buffering
# We want points further away to be processed first
idx_dist = pts3D[2,:].argsort()
pts3D = pts3D[:, idx_dist[::-1]]
pts2D = utils.C_ProjectPts(pts3D, viewpoint, KK)
# Now transfer points back to image
bounds = np.array([ [0, width-1], [0, height-1] ])
invalid_pts = utils.out_of_bounds(pts2D, bounds)
pts2D[:, invalid_pts] = 0
# Remember: IdxImg is in format [row col]
pts2D_int = np.array(np.round(pts2D), dtype=int)
# Get new depth image
dImg = DepthImage(np.zeros((height, width)), K = K)
if not zbuffer:
dImg[pts2D_int[1,:], pts2D_int[0,:]] = pts3D[2,:]
else:
# Z-Buffering --------------------------------------------------- #
# Create some circle windows according to distance
# Points that are further away from the camera --> smaller windows
num_windows = 7
winX = list()
winY = list()
minX = list(); maxX = list()
minY = list(); maxY = list()
for i in range(num_windows):
x, y = utils.circle( radius = i*0.75 + 2)
winX.append(x)
winY.append(y)
minX.append(np.zeros(x.shape))
maxX.append(np.ones(x.shape) * width - 1)
minY.append(np.zeros(y.shape))
maxY.append(np.ones(y.shape) * height - 1)
# Paint pixels, first the ones further away
for pt_idx in idx_dist[::-1]:
winsize = np.int(np.fmin(np.ceil(2./pts3D[2, pt_idx]),
num_windows))-2
winX_idx = np.array(np.fmax(np.fmin(winX[winsize] + \
pts2D_int[0, pt_idx], maxX[winsize]), \
minX[winsize]), dtype=int)
winY_idx = np.array(np.fmax(np.fmin(winY[winsize] + \
pts2D_int[1, pt_idx], maxY[winsize]), \
minY[winsize]), dtype=int)
dImg[winY_idx, winX_idx] = pts3D[2, pt_idx]
return dImg
