def mesh_binary_rasterization(triangles_2d,h,w): tn,_,_=triangles_2d.shape assert(triangles_2d.shape[1]==3) assert(triangles_2d.shape[2]==2) mask=np.ascontiguousarray(np.zeros([h,w],np.uint8)) triangles_2d=np.ascontiguousarray(triangles_2d,np.float32) mask_ptr=ffi.cast('unsigned char *',mask.ctypes.data) triangles_2d_ptr=ffi.cast('float *',triangles_2d.ctypes.data) lib.mesh_binary_rasterization(triangles_2d_ptr,mask_ptr,tn,h,w) return mask
def farthest_point_sampling(pts,sn,init_center=False): pn,_=pts.shape assert(pts.shape[1]==3) pts=np.ascontiguousarray(pts,np.float32) idxs=np.ascontiguousarray(np.zeros([sn],np.int32)) pts_ptr=ffi.cast('float*',pts.ctypes.data) idxs_ptr=ffi.cast('int*',idxs.ctypes.data) if init_center: lib.farthest_point_sampling_init_center(pts_ptr, idxs_ptr, pn, sn) else: lib.farthest_point_sampling(pts_ptr,idxs_ptr,pn,sn) return pts[idxs]
def render_mesh_depth(RT,K,vert,face,h,w,init): fn=face.shape[0] RT=np.ascontiguousarray(RT,np.float32) K=np.ascontiguousarray(K,np.float32) vert=np.ascontiguousarray(vert,np.float32) face=np.ascontiguousarray(face,np.int32) img=np.ascontiguousarray(np.zeros([h,w],np.float32)) RT_ptr=ffi.cast('float *',RT.ctypes.data) K_ptr=ffi.cast('float *',K.ctypes.data) vert_ptr=ffi.cast('float *',vert.ctypes.data) face_ptr=ffi.cast('int *',face.ctypes.data) img_ptr=ffi.cast('float *',img.ctypes.data) init_val=1 if init else 0 lib.render_depth_cffi(RT_ptr,K_ptr,vert_ptr,face_ptr,img_ptr,fn,h,w,init_val) return img
def find_nearest_point_idx(ref_pts,que_pts): ''' for every point in que_pts, find the nearest point in ref_pts :param ref_pts: pn1,3 or 2 :param que_pts: pn2,3 or 2 :return: idxs pn2 ''' assert(ref_pts.shape[1]==que_pts.shape[1] and 1<que_pts.shape[1]<=3) pn1=ref_pts.shape[0] pn2=que_pts.shape[0] dim=ref_pts.shape[1] ref_pts=np.ascontiguousarray(ref_pts[None,:,:],np.float32) que_pts=np.ascontiguousarray(que_pts[None,:,:],np.float32) idxs=np.zeros([1,pn2],np.int32) ref_pts_ptr=ffi.cast('float *',ref_pts.ctypes.data) que_pts_ptr=ffi.cast('float *',que_pts.ctypes.data) idxs_ptr=ffi.cast('int *',idxs.ctypes.data) lib.findNearestPointIdxLauncher(ref_pts_ptr,que_pts_ptr,idxs_ptr,1,pn1,pn2,dim,0) return idxs[0]
def uncertainty_pnp(points_2d, weights_2d, points_3d, camera_matrix): ''' :param points_2d: [pn,2] :param weights_2d: [pn,3] wxx,wxy,wyy :param points_3d: [pn,3] :param camera_matrix: [3,3] :return: ''' pn = points_2d.shape[0] assert (points_3d.shape[0] == pn and pn >= 4) try: dist_coeffs = uncertainty_pnp.dist_coeffs except: dist_coeffs = np.zeros(shape=[8, 1], dtype=np.float64) points_3d = points_3d.astype(np.float64) points_2d = points_2d.astype(np.float64) weights_2d = weights_2d.astype(np.float64) camera_matrix = camera_matrix.astype(np.float64) idxs = np.argsort(weights_2d[:, 0] + weights_2d[:, 1])[-4:] _, R_exp, t = cv2.solvePnP(np.expand_dims(points_3d[idxs, :], 0), np.expand_dims(points_2d[idxs, :], 0), camera_matrix, dist_coeffs, None, None, False, flags=cv2.SOLVEPNP_P3P) if pn == 4: # no other points R, _ = cv2.Rodrigues(R_exp) Rt = np.concatenate([R, t], axis=-1) return Rt points_2d = np.ascontiguousarray(points_2d, np.float64) points_3d = np.ascontiguousarray(points_3d, np.float64) weights_2d = np.ascontiguousarray(weights_2d, np.float64) camera_matrix = np.ascontiguousarray(camera_matrix, np.float64) init_rt = np.ascontiguousarray(np.concatenate([R_exp, t], 0), np.float64) points_2d_ptr = ffi.cast('double*', points_2d.ctypes.data) points_3d_ptr = ffi.cast('double*', points_3d.ctypes.data) weights_3d_ptr = ffi.cast('double*', weights_2d.ctypes.data) camera_matrix_ptr = ffi.cast('double*', camera_matrix.ctypes.data) init_rt_ptr = ffi.cast('double*', init_rt.ctypes.data) result_rt = np.empty([6], np.float64) result_rt_ptr = ffi.cast('double*', result_rt.ctypes.data) lib.uncertainty_pnp(points_2d_ptr, points_3d_ptr, weights_3d_ptr, camera_matrix_ptr, init_rt_ptr, result_rt_ptr, pn) R, _ = cv2.Rodrigues(result_rt[:3]) Rt = np.concatenate([R, result_rt[3:, None]], axis=-1) return Rt
def render_mesh_rgb(RT,K,vert,colors,face,h,w,init): fn=face.shape[0] RT=np.ascontiguousarray(RT,np.float32) K=np.ascontiguousarray(K,np.float32) vert=np.ascontiguousarray(vert,np.float32) colors=np.ascontiguousarray(colors,np.float32) face=np.ascontiguousarray(face,np.int32) img=np.ascontiguousarray(np.zeros([h,w,4],np.uint8)) RT_ptr=ffi.cast('float *',RT.ctypes.data) K_ptr=ffi.cast('float *',K.ctypes.data) vert_ptr=ffi.cast('float *',vert.ctypes.data) colors_ptr=ffi.cast('float *',colors.ctypes.data) face_ptr=ffi.cast('int *',face.ctypes.data) img_ptr=ffi.cast('char *',img.ctypes.data) init_val=1 if init else 0 lib.render_rgb_cffi(RT_ptr,K_ptr,vert_ptr,colors_ptr,face_ptr,img_ptr,fn,h,w,init_val) return img
def uncertainty_pnp_v2(points_2d, covars, points_3d, camera_matrix, type='single'): ''' :param points_2d: [pn,2] :param covars: [pn,2,2] :param points_3d: [pn,3] :param camera_matrix: [3,3] :return: ''' pn=points_2d.shape[0] assert(points_3d.shape[0]==pn and pn>=4 and covars.shape[0]==pn) points_3d=points_3d.astype(np.float64) points_2d=points_2d.astype(np.float64) camera_matrix=camera_matrix.astype(np.float64) weights_2d=[] for pi in range(pn): weight=0.0 if covars[pi,0,0]<1e-5: weights_2d.append(weight) else: weight=np.max(np.linalg.eigvals(covars[pi])) weights_2d.append(1.0/weight) weights_2d=np.asarray(weights_2d,np.float64) try: dist_coeffs = uncertainty_pnp.dist_coeffs except: dist_coeffs = np.zeros(shape=[8, 1], dtype=np.float64) idxs=np.argsort(weights_2d)[-4:] _, R_exp, t=cv2.solvePnP(np.expand_dims(points_3d[idxs,:],0), np.expand_dims(points_2d[idxs,:],0), camera_matrix,dist_coeffs,None,None,False,flags=cv2.SOLVEPNP_P3P) if pn==4: # no other points R, _ = cv2.Rodrigues(R_exp) Rt=np.concatenate([R, t], axis=-1) return Rt points_2d=np.ascontiguousarray(points_2d,np.float64) points_3d=np.ascontiguousarray(points_3d,np.float64) weights_2d=weights_2d[:,None] weights_2d=np.concatenate([weights_2d,np.zeros([pn,1]),weights_2d],1) weights_2d=np.ascontiguousarray(weights_2d,np.float64) camera_matrix=np.ascontiguousarray(camera_matrix,np.float64) init_rt=np.ascontiguousarray(np.concatenate([R_exp,t],0),np.float64) points_2d_ptr=ffi.cast('double*',points_2d.ctypes.data) points_3d_ptr=ffi.cast('double*',points_3d.ctypes.data) weights_3d_ptr=ffi.cast('double*',weights_2d.ctypes.data) camera_matrix_ptr=ffi.cast('double*',camera_matrix.ctypes.data) init_rt_ptr=ffi.cast('double*',init_rt.ctypes.data) result_rt=np.empty([6],np.float64) result_rt_ptr=ffi.cast('double*',result_rt.ctypes.data) lib.uncertainty_pnp(points_2d_ptr,points_3d_ptr,weights_3d_ptr,camera_matrix_ptr,init_rt_ptr,result_rt_ptr,pn) R, _ = cv2.Rodrigues(result_rt[:3]) Rt = np.concatenate([R, result_rt[3:,None]], axis=-1) return Rt