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)
