def main(): points_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0]], dtype=np.float32) points_2 = np.array([[0, 0, 0.2], [1, 0, 0], [0, 1, 0], [1.1, 1, 0.5]], dtype=np.float32) pc_1 = pcl.PointCloud() pc_1.from_array(points_1) pc_2 = pcl.PointCloud() pc_2.from_array(points_2) kd = pcl.KdTreeFLANN(pc_1) print('pc_1:') print(points_1) print('\npc_2:') print(points_2) print('\n') pc_1 = pcl.PointCloud(points_1) pc_2 = pcl.PointCloud(points_2) kd = pc_1.make_kdtree_flann() # find the single closest points to each point in point cloud 2 # (and the sqr distances) indices, sqr_distances = kd.nearest_k_search_for_cloud(pc_2, 1) for i in range(pc_1.size): print('index of the closest point in pc_1 to point %d in pc_2 is %d' % (i, indices[i, 0])) print('the squared distance between these two points is %f' % sqr_distances[i, 0])
def setUp(self): rng = np.random.RandomState(42) # Define two dense sets of points of sizes 30 and 170, resp. a = rng.randn(100, 3).astype(np.float32) a[:30] -= 42 self.pc = pcl.PointCloud(a) self.kd = pcl.KdTreeFLANN(self.pc)
def cloud_callback(self, data): """process the cloud""" # get points from the subscribed pointcloud # for some reason storing pc into Pcam is working with point cloud that is voxel filtered. Investigate!! pc = ros_numpy.numpify(data) Pcam = np.zeros((pc.shape[0], 3)) Pcam[:, 0] = pc['x'] Pcam[:, 1] = pc['y'] Pcam[:, 2] = pc['z'] # make a cloud of points on the max z face of the camera xx, yy = np.meshgrid(self.x, self.y) xvalues = xx.ravel() yvalues = yy.ravel() zvalues = np.ones(len(xvalues)) * self.SensorRange Pgrid = np.zeros((len(xvalues), 3)) Pgrid[:, 0] = xvalues Pgrid[:, 1] = yvalues Pgrid[:, 2] = zvalues # project existing points to the plane containing new points header = std_msgs.msg.Header() header.stamp = data.header.stamp #rospy.Time.now() header.frame_id = 'world' #header.frame_id = 'camera_color_optical_frame' #total_points = np.concatenate((Pcam, Pgrid), 0) #p = pc2.create_cloud_xyz32(header, total_points) if len(Pcam) != 0: Pprojected = self.camera_location + np.array( [(Pcam[i] - self.camera_location) * self.SensorRange / Pcam[i, 2] for i in range(len(Pcam))]) #projected_points = self.camera_location + map(lambda i, j : (i-self.camera_location)*self.SensorRange/j, points, points[:,2]) PCprojected = pcl_cloud(np.array(Pprojected, dtype=np.float32)) PCgrid = pcl_cloud(np.array(Pgrid, dtype=np.float32)) kdt = pcl.KdTreeFLANN(PCgrid) indices, sqr_distances = kdt.nearest_k_search_for_cloud( PCprojected, 10) Pgrid_modified = np.delete(Pgrid, indices, axis=0) total_points = np.concatenate((Pcam, Pgrid_modified), 0) p = pc2.create_cloud_xyz32(header, total_points) else: #total_points = np.concatenate((Pcam, Pgrid), 0) p = pc2.create_cloud_xyz32(header, Pgrid) self.pcl_pub.publish(p)
def puntos_mas_cercanos(nube, numpuntos): c = nube.to_array() o = np.zeros((c.shape), dtype=np.float32) origen = pcl.PointCloud() origen.from_array(o) #nube #el punto mas cercano de p1 al primero elemetno de p2 es kd = pcl.KdTreeFLANN(nube) indices, sqr_distances = kd.nearest_k_search_for_cloud(origen, numpuntos) #guarda los puntos mas cercannos cercanos = np.zeros((numpuntos, 3), dtype=np.float32) for i in range(numpuntos): cercanos[i] = nube[indices[0, i]] pcercanos = pcl.PointCloud() pcercanos.from_array(cercanos) #nube return pcercanos
# -*- coding: utf-8 -*- from __future__ import print_function import numpy as np import pcl points_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0]], dtype=np.float32) points_2 = np.array([[0, 0, 0.2], [1, 0, 0], [0, 1, 0], [1.1, 1, 0.5]], dtype=np.float32) pc_1 = pcl.PointCloud() pc_1.from_array(points_1) pc_2 = pcl.PointCloud() pc_2.from_array(points_2) kd = pcl.KdTreeFLANN(pc_1) print('pc_1:') print(points_1) print('\npc_2:') print(points_2) print('\n') pc_1 = pcl.PointCloud(points_1) pc_2 = pcl.PointCloud(points_2) kd = pc_1.make_kdtree_flann() # find the single closest points to each point in point cloud 2 # (and the sqr distances) indices, sqr_distances = kd.nearest_k_search_for_cloud(pc_2, 1) for i in range(pc_1.size):
def callback(self, data): header = std_msgs.msg.Header() header.stamp = data.header.stamp #rospy.Time.now() header.frame_id = 'camera_color_optical_frame' """ # make a cloud of points on the mfar end of the camera xx, yy = np.meshgrid(self.x_farend, self.y_farend) xf = xx.ravel(); yf = yy.ravel() zf = np.ones(len(xf))* self.SensorRangeMax Pgrid_farend=np.zeros((len(xf),3)) Pgrid_farend[:,0]=xf Pgrid_farend[:,1]=yf Pgrid_farend[:,2]=zf # make a cloud of points on the near end of the camera xxx, yyy = np.meshgrid(self.x_nearend, self.y_nearend) xn = xxx.ravel(); yn = yyy.ravel() zn = np.ones(len(xn))* self.SensorRangeMin Pgrid_nearend=np.zeros((len(xn),3)) Pgrid_nearend[:,0]=xn Pgrid_nearend[:,1]=yn Pgrid_nearend[:,2]=zn total_points = np.concatenate((Pgrid_farend, Pgrid_nearend), 0) #print 'time1:',time.time()-self.start_time for i in range(len(Pgrid_farend)): xyz = np.linspace(Pgrid_nearend[i], Pgrid_farend[i], int(np.linalg.norm(Pgrid_farend[i]-Pgrid_nearend[i])/self.resolution)+1) total_points = np.concatenate((total_points, xyz), 0) """ # get points from the subscribed pointcloud print 'time0:', time.time() - self.start_time pc = ros_numpy.numpify(data) Pcam = np.zeros((pc.shape[0], 3)) Pcam[:, 0] = pc['x'] Pcam[:, 1] = pc['y'] Pcam[:, 2] = pc['z'] PCcam = pcl_cloud(np.array(Pcam, dtype=np.float32)) final_grid = np.zeros((0, 3)) G = nx.Graph() for k in range(len(self.z)): xmax = self.z[k] * np.tan(self.xFOV * np.pi / 180) ymax = self.z[k] * np.tan(self.yFOV * np.pi / 180) NumberOfPointsX = int(2 * xmax / self.resolution) + 1 NumberOfPointsY = int(2 * ymax / self.resolution) + 1 x = np.linspace(-xmax, xmax, NumberOfPointsX) y = np.linspace(-ymax, ymax, NumberOfPointsY) xx, yy = np.meshgrid(x, y) xface = xx.ravel() yface = yy.ravel() zface = np.ones(len(xface)) * self.z[k] Pgrid = np.zeros((len(xface), 3)) Pgrid[:, 0] = xface Pgrid[:, 1] = yface Pgrid[:, 2] = zface final_grid = np.concatenate((final_grid, Pgrid), 0) #if k == 0: # G.add_nodes_from(Pgrid) #else: # G.add_nodes_from(Pgrid) # PC_one_faces = pcl_cloud(np.array(Pgrid, dtype=np.float32)) # P_two_faces = np.concatenate((self.points_on_previous_face, Pgrid), 0) # PC_two_faces = pcl_cloud(np.array(P_two_faces, dtype=np.float32)) # kdt_two_faces = pcl.KdTreeFLANN(PC_two_faces) # i, d = kdt_two_faces.nearest_k_search_for_cloud(PC_one_faces, 10) #self.points_on_previous_face = Pgrid PCgrid = pcl_cloud(np.array(final_grid, dtype=np.float32)) kdt = pcl.KdTreeFLANN(PCgrid) indices, sqr_distances = kdt.nearest_k_search_for_cloud(PCcam, 10) Pgrid_modified = np.delete(final_grid, indices, axis=0) p = pc2.create_cloud_xyz32(header, Pgrid_modified) final_pub.publish(p) print 'time4:', time.time() - self.start_time
def get_edges(self, grid1, grid2): combined_grid = np.concatenate((grid1, grid2), 0) c1 = pcl_cloud(np.array(grid1, dtype=np.float32)) c_combined = pcl_cloud(np.array(combined_grid, dtype=np.float32)) kdt = pcl.KdTreeFLANN(c_combined) indices, sqr_distances = kdt.nearest_k_search_for_cloud(c_combined, 8)
def calculate_features(self, input, K=3, MAX_SIZE=500, splitNeighbors=True, aug=False, feature_type=1): ##input tensor = NxNx3 inputData = self.prefix + '/' + input label = self.labels[input.split('/')[-3]] cloud = pcl.load(inputData) #stolen from ECC code, drops out random points #if aug: #Probability a point is dropped p = 0.1 cloudArray = cloud.to_array() keptIndices = random.choice(range(cloudArray.shape[0]), size=int( math.ceil( (1 - p) * cloudArray.shape[0])), replace=False) cloudArray = cloudArray[keptIndices, :] cloud.from_array(cloudArray) cloud.resize(MAX_SIZE) xyz = cloud.to_array()[:, :3] #Stolen from ECC code if aug: M = np.eye(3) s = random.uniform(1 / 1.1, 1.1) M = np.dot(transforms3d.zooms.zfdir2mat(s), M) #angle = random.uniform(0, 2*math.pi) #M = np.dot(transforms3d.axangles.axangle2mat([0,0,1], angle), M) # z=upright assumption if random.random() < 0.5 / 2: M = np.dot(transforms3d.zooms.zfdir2mat(-1, [1, 0, 0]), M) if random.random() < 0.5 / 2: M = np.dot(transforms3d.zooms.zfdir2mat(-1, [0, 1, 0]), M) xyz = np.dot(xyz, M.T) if feature_type == 1: kd = pcl.KdTreeFLANN(cloud) #if aug: # currentK = random.randint(np.maximum(1,K-2),K+2) # indices, sqr_distances = kd.nearest_k_search_for_cloud(cloud, currentK) #else: indices, sqr_distances = kd.nearest_k_search_for_cloud( cloud, K ) # K = 2 gives itself and other point from cloud which is closest vertexMean = np.mean(xyz, axis=0) vertexStd = np.std(xyz, axis=0) #Jiggle the model a little bit if it is perfectly aligned with the axes #print(input) if not vertexStd.all(): M = np.eye(3) angle = random.uniform(0.01, 0.1, size=3) sign = random.choice([-1, 1], size=3, replace=True) M = np.dot( transforms3d.axangles.axangle2mat([0, 0, 1], sign[0] * angle[0]), M) M = np.dot( transforms3d.axangles.axangle2mat([0, 1, 0], sign[1] * angle[1]), M) M = np.dot( transforms3d.axangles.axangle2mat([1, 0, 0], sign[2] * angle[2]), M) xyz = np.dot(xyz, M.T) vertexMean = np.mean(xyz, axis=0) vertexStd = np.std(xyz, axis=0) xyz = (xyz - vertexMean) / vertexStd num_nodes = xyz.shape[0] sqr_distances[:, 0] += 1 #includes self-loops valid = np.logical_or(indices > 0, sqr_distances > 1e-10) rowi, coli = np.nonzero(valid) idx = indices[(rowi, coli)] #print("XYZ Shape {0}".format(xyz.shape)) edges = np.vstack([idx, rowi]).transpose() #print("Edge Shape {0}".format(edges.shape)) A = np.zeros(shape=(MAX_SIZE, 8, MAX_SIZE)) zindices = np.dot([4, 2, 1], np.greater((xyz[edges[:, 0], :] - xyz[edges[:, 1], :]).transpose(), np.zeros((3, edges.shape[0])))) edgeLen = 1 # print('From {0} to {1}: Len {2}',i,j,edgeLen) A[edges[:, 0], zindices, edges[:, 1]] = edgeLen A[edges[:, 1], zindices, edges[:, 0]] = edgeLen elif feature_type == 0: RADIUS = 0.5 vertexMean = np.mean(xyz, axis=0) vertexStd = np.std(xyz, axis=0) #Jiggle the model a little bit if it is perfectly aligned with the axes #print(input) if not vertexStd.all(): M = np.eye(3) angle = np.random.uniform(0.01, 0.1, size=3) sign = np.random.choice([-1, 1], size=3, replace=True) M = np.dot( transforms3d.axangles.axangle2mat([0, 0, 1], sign[0] * angle[0]), M) M = np.dot( transforms3d.axangles.axangle2mat([0, 1, 0], sign[1] * angle[1]), M) M = np.dot( transforms3d.axangles.axangle2mat([1, 0, 0], sign[2] * angle[2]), M) V = np.dot(V, M.T) vertexMean = np.mean(V, axis=0) vertexStd = np.std(V, axis=0) V = (V - vertexMean) / vertexStd #V = np.pad(V,pad_width=((0,MAX_SIZE - V.shape[0]),(0,0)),mode='constant') kdtree = scipy.spatial.KDTree(V) knns = kdtree.query_ball_tree(kdtree, r=RADIUS) A = np.zeros(shape=(MAX_SIZE, 8, MAX_SIZE)) numNeighbors = [len(x) for x in knns] v1 = np.repeat(np.arange(V.shape[0]), numNeighbors) knnsStack = np.concatenate(knns) zindex = np.dot([4, 2, 1], np.greater((V[v1] - V[knnsStack]).transpose(), np.zeros((3, len(knnsStack))))) edgeLen = 1 A[v1, zindex, knnsStack] = edgeLen A[knnsStack, zindex, v1] = edgeLen return xyz.astype(np.float32), A.astype(np.float32), label
def calculate_features(input, K=3, MAX_SIZE=500, aug=False,feature_type=1):##input tensor = NxNx3 #inputData = prefix + '/' + input # label = self.labels[input.split('/')[-3]] cloud = pcl.load(input) basename,ext = os.path.splitext(input) featureData = np.load(basename + '.npy') #stolen from ECC code, drops out random points #if aug: #Probability a point is dropped p = 0.1 cloudArray = cloud.to_array() if cloudArray.shape[0] > MAX_SIZE: keptIndices = np.random.choice(range(cloudArray.shape[0]), size=int(MAX_SIZE), replace=False) cloudArray = cloudArray[keptIndices, :] featureData = featureData[keptIndices, :] #print(cloudArray.shape) keptIndices = random.choice(range(cloudArray.shape[0]), size=int(math.ceil((1-p)*cloudArray.shape[0])),replace=False) cloudArray = cloudArray[keptIndices,:] featureData = featureData[keptIndices,:] featureData = np.concatenate((featureData, np.zeros((MAX_SIZE - featureData.shape[0],10)))) cloud.from_array(cloudArray) cloud.resize(MAX_SIZE) xyz = cloud.to_array()[:,:3] kd = pcl.KdTreeFLANN(cloud) indices, sqr_distances = kd.nearest_k_search_for_cloud(cloud, K) # K = 2 gives itself and other point from cloud which is closest sqr_distances[:, 0] += 1 # includes self-loops valid = np.logical_or(indices > 0, sqr_distances > 1e-10) rowi, coli = np.nonzero(valid) idx = indices[(rowi, coli)] #Stolen from ECC code if aug: angle = np.random.uniform(0,2*np.pi,size=3) scale = np.random.uniform(1,1.2,size=3) #M = np.eye(3) #M = np.dot(transforms3d.axangles.axangle2mat([0,0,1], angle[0]), M) #M = np.dot(transforms3d.axangles.axangle2mat([0,1,0], angle[1]), M) #M = np.dot(transforms3d.axangles.axangle2mat([1,0,0], angle[2]), M) #M = np.dot(transforms3d.zooms.zfdir2mat(scale[0], [1,0,0]), M) #M = np.dot(transforms3d.zooms.zfdir2mat(scale[1], [0,1,0]), M) #M = np.dot(transforms3d.zooms.zfdir2mat(scale[2], [0,0,2]), M) #xyz = np.dot(xyz,M.T) M = np.eye(3) s = random.uniform(1/1.1, 1.1) M = np.dot(transforms3d.zooms.zfdir2mat(s), M) #angle = random.uniform(0, 2*math.pi) #M = np.dot(transforms3d.axangles.axangle2mat([0,0,1], angle), M) # z=upright assumption if random.random() < 0.5/2: M = np.dot(transforms3d.zooms.zfdir2mat(-1, [1,0,0]), M) if random.random() < 0.5/2: M = np.dot(transforms3d.zooms.zfdir2mat(-1, [0,1,0]), M) xyz = np.dot(xyz,M.T) if feature_type == 1: vertexMean = np.mean(xyz, axis=0) vertexStd = np.std(xyz, axis=0) #Jiggle the model a little bit if it is perfectly aligned with the axes #print(input) if not vertexStd.all(): M = np.eye(3) angle = random.uniform(0.01,0.1,size=3) sign = random.choice([-1,1],size=3,replace=True) M = np.dot(transforms3d.axangles.axangle2mat([0,0,1], sign[0] * angle[0]), M) M = np.dot(transforms3d.axangles.axangle2mat([0,1,0], sign[1] * angle[1]), M) M = np.dot(transforms3d.axangles.axangle2mat([1,0,0], sign[2] * angle[2]), M) xyz = np.dot(xyz,M.T) vertexMean = np.mean(xyz, axis=0) vertexStd = np.std(xyz, axis=0) xyz = (xyz - vertexMean)/vertexStd num_nodes = xyz.shape[0] Aindices, Avalues, Adenseshape = geometricSplit(xyz, rowi, idx, MAX_SIZE) elif feature_type == 0: RADIUS = 0.5 vertexMean = np.mean(xyz, axis=0) vertexStd = np.std(xyz, axis=0) #Jiggle the model a little bit if it is perfectly aligned with the axes #print(input) if not vertexStd.all(): M = np.eye(3) angle = np.random.uniform(0.01,0.1,size=3) sign = np.random.choice([-1,1],size=3,replace=True) M = np.dot(transforms3d.axangles.axangle2mat([0,0,1], sign[0] * angle[0]), M) M = np.dot(transforms3d.axangles.axangle2mat([0,1,0], sign[1] * angle[1]), M) M = np.dot(transforms3d.axangles.axangle2mat([1,0,0], sign[2] * angle[2]), M) xyz = np.dot(xyz,M.T) vertexMean = np.mean(xyz, axis=0) vertexStd = np.std(xyz, axis=0) xyz = (xyz - vertexMean)/vertexStd #V = np.pad(V,pad_width=((0,MAX_SIZE - V.shape[0]),(0,0)),mode='constant') #kdtree = scipy.spatial.KDTree(xyz) #knns = kdtree.query_ball_tree(kdtree,r=RADIUS) #A = np.zeros(shape=(MAX_SIZE,self.theta*self.phi, MAX_SIZE)) #numNeighbors = [len(x) for x in knns] #v1 = np.repeat(np.arange(xyz.shape[0]),numNeighbors) #knnsStack = np.concatenate(knns) Aindices, Avalues, Adenseshape = sphericalSplit(xyz,rowi, idx,MAX_SIZE,THETA,PHI,THETA_KEYS,PHI_KEYS) #Final step, add eigenvectors and curvature features. Smallest eigenvector feature is surface normal #features = np.zeros((xyz.shape[0], 10)) #for i in range(xyz.shape[0]): # X = xyz[indices[i, :], :] # pca = sklearn.decomposition.PCA() # pca.fit(X) # features[i, 0:9] = np.ravel(pca.components_) # curvature = np.amin(pca.explained_variance_) / np.sum(pca.explained_variance_) # if np.isnan(curvature): # curvature = 0 # features[i, 9] = curvature #features #print(featureData.shape) #print(xyz.shape) xyz = np.concatenate((xyz,featureData),axis=1) return xyz.astype(np.float32), Aindices.astype(np.int64), Avalues.astype(np.float32)