def o3dp3dTovtkPCD(p3d): xyz = numpy.asarray(p3d.points) minH = xyz[:, 2].min() maxH = xyz[:, 2].max() #print("minh,maxh:",minH,maxH) count = len(xyz) #print(len(xyz.shape)) pcd = VtkPointCloud(minH, maxH, count) pcd.clearPoints() counter = numpy.size(xyz, 0) #print(counter) #test np to vtk array nCoords = xyz.shape[0] nElem = xyz.shape[1] #print("c,e",nCoords,nElem) #print("xyz",xyz) depth = xyz[:, 2] #print("depth",depth) #colors = numpy_support.numpy_to_vtk(rgb) vtkDepth = numpy_support.numpy_to_vtk(depth) cells_npy = numpy.vstack([ numpy.ones(nCoords, dtype=numpy.int64), numpy.arange(nCoords, dtype=numpy.int64) ]).T.flatten() #print("cells_npy",cells_npy) cells = vtk.vtkCellArray() cells.SetCells(nCoords, numpy_support.numpy_to_vtkIdTypeArray(cells_npy)) #print("cells",cells) vtkArray = numpy_support.numpy_to_vtk(xyz) verts = vtk.vtkPoints() verts.SetData(vtkArray) #print(vtkArray) pcd.setPoints(vtkArray, nCoords, numpy_support.numpy_to_vtkIdTypeArray(cells_npy), vtkDepth) return pcd
def getVtkPointCloud(path, downsampling=0): print(time.time()) isNotOk = True while isNotOk: try: f = open(path) isNotOk = False except: isNotOk = True pd_xyzrgb = pd.read_csv(f, header=None, delim_whitespace=True, dtype=float, usecols=[0, 1, 2]) print(time.time()) #rgb = pd.read_csv(path,header=None,delim_whitespace=True,dtype=float,usecols=[3,4,5]) pd_xyzrgb = pd_xyzrgb.values xyz = pd_xyzrgb[:, [0, 1, 2]] #rgb = pd_xyzrgb[:,[3,4,5]] #print(xyz) #Create open3d pcd format======================== p3d = o3d.geometry.PointCloud() p3d.points = o3d.utility.Vector3dVector(xyz) #================================================ ''' downsample by open3d ''' if downsampling == 1: print("donwsample by open3d") p3d = o3d.geometry.PointCloud() p3d.points = o3d.utility.Vector3dVector(xyz) print("np to open3d") downpcd = p3d.voxel_down_sample(voxel_size=0.5) print("voxel down sample") cl, ind = downpcd.remove_statistical_outlier(nb_neighbors=50, std_ratio=3.0) #print(cl,ind) #downpcd = downpcd.select_by_index(ind) xyz = numpy.asarray(cl.points) print("numpy array") #================================================ print(time.time()) ''' print(xyz) theta1=numpy.radians(35) theta2=numpy.radians(5) c1,c2,s1,s2=numpy.cos(theta1),numpy.cos(theta2),numpy.sin(theta1),numpy.sin(theta2) rx=numpy.array(((1,0,0),(0,c1,-s1),(0,s1,c1))) ry=numpy.array(((c2,0,s2),(0,1,0),(-s2,0,c2))) xyz=xyz.dot(rx) print(xyz) xyz=xyz.dot(ry) ''' #rgb = rgb.values #print(xyz,rgb) #xyz = loadtxt(self.fileName,dtype=float,usecols=[0,1,2]) minH = xyz[:, 2].min() maxH = xyz[:, 2].max() #print("minh,maxh:",minH,maxH) count = len(xyz) #print(len(xyz.shape)) pcd = VtkPointCloud(minH, maxH, count) pcd.clearPoints() counter = numpy.size(xyz, 0) #print(counter) #test np to vtk array nCoords = xyz.shape[0] nElem = xyz.shape[1] #print("c,e",nCoords,nElem) #print("xyz",xyz) depth = xyz[:, 2] #print("depth",depth) #colors = numpy_support.numpy_to_vtk(rgb) vtkDepth = numpy_support.numpy_to_vtk(depth) cells_npy = numpy.vstack([ numpy.ones(nCoords, dtype=numpy.int64), numpy.arange(nCoords, dtype=numpy.int64) ]).T.flatten() #print("cells_npy",cells_npy) cells = vtk.vtkCellArray() cells.SetCells(nCoords, numpy_support.numpy_to_vtkIdTypeArray(cells_npy)) #print("cells",cells) vtkArray = numpy_support.numpy_to_vtk(xyz) verts = vtk.vtkPoints() verts.SetData(vtkArray) #print(vtkArray) pcd.setPoints(vtkArray, nCoords, numpy_support.numpy_to_vtkIdTypeArray(cells_npy), vtkDepth) ''' for k in range(size(xyz,0)): self.signalNow.emit(k) point = xyz[k] pcd.addPoint(point) ''' print(time.time()) data = [pcd, p3d] return data