def localize_test(self):
cloud1 = np.load(self.PC_files[0]) #use self.PC_files_reg
xyzs = np.array((cloud1['x'], cloud1['y'], cloud1['z'])).T
pc = pointcloud.PointCloud(xyzs)
pc.boxfilter(xmin=self.boxFilter[0], xmax=self.boxFilter[1],\
ymin=self.boxFilter[2], ymax=self.boxFilter[3],\
zmin=self.boxFilter[4], zmax=self.boxFilter[5])
pose = self.pose_list[0] #(0,0,0,0,0,0)#
# pc.pose = pose
PCs.append(pc)
self.vmap.add_points(PCs[0], PCs[0].pose)
''' Import object '''
data = np.load('data/merged_PC_aligned.npy')
xyzs = np.array((data[:, 0], data[:, 1], data[:, 2])).T
centroid = np.mean(xyzs, axis=0)
xyzs[:, 0] -= centroid[0]
xyzs[:, 1] -= centroid[1]
xyzs[:, 2] -= centroid[2]
# pose = (0,0,0,0,0,0)#self.pose_list[0]
centroid_self.vmap = np.mean(self.vmap.get_points().points, axis=0)
guessPose = (centroid_self.vmap[0], centroid_self.vmap[1],
centroid_self.vmap[2], 0, 0, 0
) #(0,0,0,0,0,0)#self.pose_list[0]#(0,0,0,0,0,0)
# xyzs = np.array((data['x'], data['y'], data['z'])).T
pc = pointcloud.PointCloud(xyzs)
bestpose, maxcost = self.vmap.align_points(pc, guessPose)
pdb.set_trace()
self.vmap.add_points(pc, bestpose)
print "GuessPose: ", guessPose
print "BestPose: ", bestpose
print "Max cost: ", maxcost
def setUp(self):
X = np.random.random((10, 3))
self.A = X[:7, :]
self.B = X[5:, :]
self.PCA = pointcloud.PointCloud(self.A)
self.PCB = pointcloud.PointCloud(self.B)
self.PCC = pointcloud.PointCloud(self.A[:-2])
def match_all(self, current_pc, current_centroid=[], name_list = []): vmap = mapper.VoxelMap(self.res, levels = self.mrolLevels) pose_zero = poseutil.Pose3D() pose_zero.set((0,0,0,0,0,0)) poseList = [] costList = [] centroid_input = 0 current_centroid_local = np.zeros(3) if current_centroid==[]: centroid_input = 1 if centroid_input==[]: current_centroid = np.mean(current_pc, axis=0) current_pc -= current_centroid current_pc_vox = pointcloud.PointCloud(current_pc) rad = .2 current_pc_vox.boxfilter(xmin=-rad, xmax=rad, ymin=-rad, ymax=rad, zmin=-rad, zmax=rad) if centroid_input: current_centroid_local = np.mean(current_pc_vox.points, axis=0) current_pc_vox -= current_centroid_local vmap.add_points(current_pc_vox, pose_zero) for i in range(self.modelCount): #Check to see if for j in range(len(name_list)): # print self.all_names[i] # print name_list[j] # print self.all_names[i]==name_list[j] if (name_list == [] and j == 0) or self.all_names[i] == name_list[j]: pc_ground_vox = self.models_vox[i] bestPose, maxCost = vmap.align_points(pc_ground_vox, pose_zero) # print "Pose: ", bestPose # print "Max cost: ", maxCost bestPose_inv = poseutil.inverse(bestPose.get()) bestPose_inv = poseutil.Pose3D(bestPose_inv) modelPose_T = current_centroid + current_centroid_local + bestPose_inv.get()[0:3] modelPose = np.reshape([modelPose_T, bestPose_inv.get()[3:]], 6) poseList.append(modelPose) costList.append(maxCost) # mat = poseutil.Pose3D() # mat.set(bestPose.get()) if 0: xyzs = poseutil.transformPoints(pc_ground_vox.points, bestPose.getMatrix()) pc = pointcloud.PointCloud(xyzs) pdb.set_trace() return costList, poseList
def image_to_points(di, max_range=5., fov=np.radians(75)):
''' Convert a depth image to point cloud'''
# horizontal field of view setting in blender in degrees
#fov = np.radians(75)
# at edge tan fov/2 = 320 / a where a is the adjacent pixel length
a = 0.5 * di.shape[1] / np.tan(fov / 2)
cols = np.arange(0, di.shape[1]) - di.shape[1] / 2 + 0.5
rows = np.arange(0, di.shape[0]) - di.shape[0] / 2 + 0.5
rows.shape = -1, 1
f = di / a
# scale factors between image plane and real space position of
# point, then by similar shapes, one is just a scale factor enlargement of the other
# Blender z buffer returns not the range to the point but the distance from the camera plane, namely the z distance.
# TODO could use a masked array for performance
# camera looking along the x-axis
Y = -f * cols
Z = -f * rows
X = f * a
#inds = np.logical_and(di < di.max(), di > 0)
inds = np.logical_and(di < max_range, di > 0)
xyzs = np.vstack((X[inds], Y[inds], Z[inds])).T
pc = pointcloud.PointCloud(xyzs)
return pc
def setUp(self):
self.pc = pointcloud.PointCloud((
(0, 0, 0),
(0.1, 0.1, 0.1),
(1, 1, 1),
(1, 1, 1.01),
))
self.res = 0.2
def store_objects(self, name_list = None):
# Use name_list to only import selected object models
dirs = os.walk(self.baseDir)
root = []
folders = []
files = []
for i in dirs:
root.append(i[0])
folders.append(i[1])
files.append(i[2])
for i in range(len(root)):
for j in range(len(files[i])):
PC_index = files[i][j].find(".txt")
# PC_index = files[i][j].find("_PC")
if PC_index>-1:
if name_list == None:
object_name = files[i][j][:PC_index]
filename = root[i]+'/'+files[i][j]
self.all_names.append(object_name)
pc = np.loadtxt(filename)
self.models.append(pc)
self.models_vox.append(pointcloud.PointCloud(pc))
# If there is a name list, only accept pointclouds from that list
else:
object_name = files[i][j][:end_name_ind]
filename = root[i]+'/'+files[i][j]
for i_name in range(len(name_list)):
if object_name == name_list[i_name]:
self.all_names.append(object_name)
pc = np.loadtxt(filename)
self.models.append(pc)
self.models_vox.append(pointcloud.PointCloud(pc))
self.modelCount = len(self.models)
def match_original(self):
cloud1 = np.load(self.PC_files[0]) #use self.PC_files_reg
xyzs = np.array((cloud1['x'], cloud1['y'], cloud1['z'])).T
pc = pointcloud.PointCloud(xyzs)
#pc.boxfilter(xmin=self.boxFilter[0], xmax=self.boxFilter[1],ymin=self.boxFilter[2], ymax=self.boxFilter[3],zmin=self.boxFilter[4], zmax=self.boxFilter[5])
pose = self.pose_list[0] #np.load(self.PC_files_reg_all[0])['pose']
pc.set(pose)
PCs.append(pc)
self.vmap.add_points(PCs[0], PCs[0].pose)
guessPose = PCs[0].pose
cloud2 = np.load(self.PC_files_reg[1])
xyzs = np.array((cloud2['x'], cloud2['y'], cloud2['z'])).T
pc = pointcloud.PointCloud(xyzs)
start = time.time()
bestpose, maxcost = self.vmap.align_points(pc, guessPose)
taken = time.time() - start
print 'Scanmatch time:', taken, 'seconds'
print "Pose: ", bestpose
print "Max cost: ", maxcost
def create_model(self):
PCs_added = 0
if self.fileCount > 1:
for j in range(0, self.fileCount, 1):
data = np.load(self.PC_files[j]) #should be self.PC_files_reg
xyzs = np.array((data['x'], data['y'], data['z'])).T
xyzs = xyzs[(np.nonzero(np.nansum(xyzs < 100, axis=1) > 0)[0])]
pose = poseutil.Pose3D(self.pose_list[j])
pose_zero = poseutil.Pose3D()
pose_zero.set((0, 0, 0, 0, 0, 0))
xyzs = poseutil.transformPoints(xyzs, pose.getMatrix())
pc = pointcloud.PointCloud(xyzs)
pc.pose = pose_zero #pose
# pc.boxfilter(xmin=-.1, xmax=.1, ymin=-.1, ymax=.1, zmin=-.1, zmax=.1)
# pc.boxfilter(xmin=-0.05, xmax=.05, ymin=-.05, ymax=.05, zmin=-.05, zmax=.05)
pc.boxfilter(xmin=self.boxFilter[0],
xmax=self.boxFilter[1],
ymin=self.boxFilter[2],
ymax=self.boxFilter[3],
zmin=self.boxFilter[4],
zmax=self.boxFilter[5])
# print pose
# pdb.set_trace()
if j == 0: #Don't try to align first pose
self.vmap.add_points(pc, pc.pose)
else:
guessPose = pose_zero #self.pose_list[i]#np.load(self.PC_files_reg_all[i])['pose']
bestPose = guessPose
# print guessPose
bestPose = pose_zero
# bestPose, maxcost = self.vmap.align_points(pc, guessPose)
# bestPose, maxcost = self.vmap.align_points(pc, guessPose, True) #only 2D alignment
disp = np.array(bestPose.getTuple())
if 1: #all(np.abs(disp[:3]) < self.transThresh) and all(np.abs(disp[3:])<self.rotThresh*3.14/180.):
self.vmap.add_points(pc, bestPose)
PCs_added += 1
# print '%i PCs added'%PCs_added
# print "Best Pose: ", bestPose
# guessPose = bestpose
# print "Pose from ground: ", guessPose
print '%i of %i point clouds were added' % (PCs_added, self.fileCount)
def test_Amapper_raytrace(
self): # "A" prepended to run this first cos it is quick
print "test_Amapper_raytrace"
res = 0.04
vm = mapper.VoxelMap(res, levels=3)
xyzs = np.zeros([4, 3])
xyzs[:, 1] = np.cumsum(np.ones(4)) * res
xyzs[3, :] = [0.5, 0.5, 0.5] # this one isn't ray traced out
pc = pointcloud.PointCloud(xyzs)
vm.add_points(pc, None)
pose = (0, 0, 0, 0, 0, np.pi / 2.)
ray_occs_pc = vm.ray_trace(pose, 50 * res)
self.assertEqual(
ray_occs_pc,
pointcloud.PointCloud(
occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
ray_occs_pc = vm.ray_trace_pts(xyzs[0, :], xyzs[2, :])
self.assertEqual(
ray_occs_pc,
pointcloud.PointCloud(
occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
ray_occs_pc = vm.ray_trace_pts(np.array([0, 0, 0]), xyzs[3, :])
self.assertEqual(
ray_occs_pc,
pointcloud.PointCloud(
occupiedlist.pointstovoxels(xyzs[3, :], res, vm.lattice)))
ray_occs_pc = vm.ray_trace_pts(xyzs[2, :], xyzs[0, :])
self.assertEqual(
ray_occs_pc,
pointcloud.PointCloud(
occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
# TODO ray check perpendicular to a plane
# test the free-space tracing
pc_555 = pointcloud.PointCloud(xyzs[3, :])
free_555_pc = vm.free_space_ray_trace(pc_555,
np.asarray([0, 0, 0]),
resolution=res)
xyzs_free_555 = np.array([[0., 0., 0.], [0.04, 0.04, 0.04],
[0.08, 0.08, 0.08], [0.12, 0.12, 0.12],
[0.16, 0.16, 0.16], [0.2, 0.2, 0.2],
[0.24, 0.24, 0.24], [0.28, 0.28, 0.28],
[0.32, 0.32, 0.32], [0.36, 0.36, 0.36],
[0.4, 0.4, 0.4], [0.44, 0.44, 0.44]])
expected_555_pc = pointcloud.PointCloud(xyzs_free_555)
self.assertEqual(expected_555_pc, free_555_pc)
def merge_PC_ground(self):
if self.fileCount > 1:
for j in range(self.fileCount):
data = np.load(self.PC_files[j]) #should be self.PC_files_reg
xyzs = np.array((data['x'], data['y'], data['z'])).T
# pose = self.pose_list[j]#np.load(self.PC_files_reg_all[j])['pose']
pose_zero = poseutil.Pose3D()
pose_zero.set((0, 0, 0, 0, 0, 0))
xyzs = poseutil.transformPoints(xyzs, pose.getMatrix())
pc = pointcloud.PointCloud(xyzs)
pc.pose = pose_zero #pose
pc.boxfilter(xmin=self.boxFilter[0],
xmax=self.boxFilter[1],
ymin=self.boxFilter[2],
ymax=self.boxFilter[3],
zmin=self.boxFilter[4],
zmax=self.boxFilter[5])
print pose
self.vmap.add_points(pc, pc.pose)
#import pydb; pydb.set_trace()
#print ol
#absent_map.display()
segment_map = absent_map
bestpose = poseutil.Pose3D(X=(0.5,-0.5,0,0,0,np.pi/4))
for fname in fnames:
pc = iros_mapper.get_pointcloud(fname)
print fname,
bestpose = segment_map.align_points(pc, bestpose)[0]
print bestpose
hits, new_points = segment_map.mrol.getfinest().segment_ovl(bestpose,pc.points)
inliers, outliers = freespace_ol.segment_ovl(None, new_points)
segment_map.add_points(pointcloud.PointCloud(outliers), None)
if freespace2:
if len(inliers) > 0:
tmp_freespace_ol = segment_map.generate_freespace2(inliers,pose=bestpose)
else:
# nothing to segment?
import pdb;pdb.set_trace()
tmp_freespace_ol = occupiedlist.OccupiedList(1)
else:
tmp_freespace_ol = segment_map.generate_freespace(res,pose=bestpose)
freespace_ol.addpoints(tmp_freespace_ol.getpoints(), None)
if visualise:
print "Visualising"
vis.clear()
vis.addmappts(segment_map.get_points().points)
fnames = [
datadir + '/' + fname for fname in fnames if fname.endswith('.png')
]
except:
print 'Need to specify a valid directory for input'
sys.exit()
visualise = True
long_term = False
bestpose = poseutil.Pose3D(X=(0, 0, 0, 0, 0, 0))
pc_xform = poseutil.Pose3D(X=(0, 0, 0, -np.pi / 2., 0, -np.pi / 2.))
res = 0.01
segment_map = mapper.VoxelMap(res, levels=3)
pc = pointcloud.load(fnames.pop(0))
#pc = pointcloud.PointCloud(pc)
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
segment_map.add_points(pc, bestpose)
object_map = mapper.VoxelMap(res / 4., levels=1)
#freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3)
#freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res
free_space_res = res
pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
pc_vox = quantizer.uniquerows(pc_vox, 0)
pc_pts = occupiedlist.voxelstopoints(pc_vox, free_space_res)
pc_regular = pointcloud.PointCloud(pc_pts)
freespace = segment_map.free_space_ray_trace(pc_regular, (0, 0, 0),
free_space_res,
voxel_offset=2.5)
def get_pointcloud(fname):
xyzs = depth_image.image_to_points(fname)
pc = pointcloud.PointCloud(xyzs)
# remove ceiling and floor
pc.boxfilter(zmin=-15, zmax=0.5)
return pc
fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
except:
print 'Need to specify a valid directory for input and file name for output'
sys.exit()
# variable initialisation
iros_map = mapper.VoxelMap(res,levels=5)
iros_free_map = mapper.VoxelMap(res,levels=1)
bestpose = poseutil.Pose3D()
pc = get_pointcloud(fnames.pop(0))
iros_map.add_points(pc, bestpose)
if visualise:
iros_map.display(changes=True)
if make_free:
freepts = get_freespace(pc.points, bestpose, iros_map)
pcfree = pointcloud.PointCloud(freepts)
iros_free_map.add_points(pcfree,None)
if visualise:
iros_free_map.display(npts=1000000)
for fname in fnames:
print fname,
pc = get_pointcloud(fname)
bestpose = iros_map.align_points(pc, bestpose)[0]
iros_map.add_points(pc, bestpose)
if make_free:
freepts = get_freespace(pc.points, bestpose, iros_map)
pcfree = pointcloud.PointCloud(freepts)
iros_free_map.add_points(pcfree,None)
print bestpose
R = p.getMatrix()[0:3, 0:3]
# data is a dictionary with elements time, frame, dID, oID, R (3x3), T (3x1)
data = {
'time': time_out,
'frame': frame,
'oID': oID,
'dID': dIDs[i],
'R': R,
'T': T
}
output.write_data(data)
#Visualize
if 0: #viz:
pc = pointcloud.PointCloud(pts_pos.T)
P = poseutil.Pose3D()
P.set((0, 0, 0, 0, 0, 0))
print 'Adding new points for visualization...'
# pc.set(P)
vmap.add_points(pc, P)
# frame+=1
print 'Time per frame: ', np.mod(time.gmtime()[5] - time_[5],
60)
except:
# pdb.set_trace()
print "Error in detector?"
print "Previous file: ", saveDir + outputName
# img_pos, img_color = bag.pts2img(pts_pos, pts_color)
datadir + '/' + fname for fname in fnames
if fname.endswith('.png')
]
except:
print 'Need to specify a valid directory for input and file name for output'
sys.exit()
# variable initialisation
iros_map = mapper.VoxelMap(res, levels=3)
iros_free_map = mapper.VoxelMap(res, levels=1)
bestpose = poseutil.Pose3D()
if load_pc:
pc_xform = poseutil.Pose3D(X=(0, 0, 0, 0, 0, 0))
pc = pointcloud.load(fnames.pop(0))
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
pc_xform = poseutil.Pose3D(X=(0, 0, 0, -np.pi / 2., 0, -np.pi / 2.))
else:
pc = get_pointcloud(fnames.pop(0))
iros_map.add_points(pc, bestpose)
if visualise:
iros_map.display(changes=False)
if make_free:
freepts = get_freespace(pc.points, bestpose, iros_map)
pcfree = pointcloud.PointCloud(freepts)
iros_free_map.add_points(pcfree, None)
if visualise:
iros_free_map.display(npts=1000000)
for fname in fnames:
print fname,
import mrol_mapping.mapper as mapper
import mrol_mapping.mrol as mrol
import mrol_mapping.poseutil as poseutil
import mrol_mapping.pointcloud as pointcloud
import time
import pdb
res = .002
n_levels = 4
if len(sys.argv) > 1:
input_file = sys.argv[1]
else:
input_file = '/home/colin/repo/perception_repo/data/obj16_best.npy'
vmap = mapper.VoxelMap(res, levels=n_levels)
#vmap.display(npts=4e5)
input_pc = np.load(input_file)
pc = pointcloud.PointCloud(input_pc)
vmap.add_points(pc, poseutil.Pose3D())
point1 = np.array([-.2, 0, 0])
point2 = np.array([0, 0, 0])
tmp = vmap.ray_trace_pts(point1, point2, return_extras=False)
pdb.set_trace()
point_clouds = {}
PCs = []
vmap = mapper.VoxelMap(res, levels = n_levels)
vmap.display(npts=4e5)
'''Match -- original test data'''
if 0:
cloud1 = np.load(good_file_pc[0]) #use good_file_reg
xyzs = np.array((cloud1['x']*scale, cloud1['y']*scale, cloud1['z']*scale)).T
pc = pointcloud.PointCloud(xyzs)
#pc.boxfilter(xmin=bb_xmin, xmax=bb_xmax,ymin=bb_ymin, ymax=bb_ymax,zmin=bb_zmin, zmax=bb_zmax)
pose = pose_list[0]#np.load(good_file_reg_all[0])['pose']
pc.set(pose)
PCs.append(pc)
vmap.add_points(PCs[0], PCs[0].pose)
guessPose = PCs[0].pose
cloud2 = np.load(good_file_reg[1])
xyzs = np.array((cloud2['x'], cloud2['y'], cloud2['z'])).T
pc = pointcloud.PointCloud(xyzs)
start = time.time()
def setCheck(self, data1, data2):
pc1 = pointcloud.PointCloud(data1)
pc2 = pointcloud.PointCloud(data2)
self.assertEqual(pc1, pc2)
