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