def setUp(self):
#np.random.seed(6)
self.res = 0.04
self.vmap = mapper.VoxelMap(self.res, levels=3)
pc = get_PC(0)[0]
self.vmap.add_points(pc, pc.pose)
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 OFFtest_Zmapbuild_6DOF_feature(self):
# "Z" prepended to run this last cos it is slow
print "test_mapbuild_6DOF_feature"
#vmap = self.vmap
vmap = mapper.VoxelMap(self.res, levels=5)
pc, di = get_PC(0)
di_disconts = depth_image.depth_discont_sobel(di)
pc_disconts = depth_image.image_to_points(di_disconts)
vmap.add_points(pc_disconts, pc.pose)
guessPose = vmap.trajectory[0]
#guessPose = get_PC(0)[0].pose
if visualise:
import mrol_mapping.visualiser.robotvisualiser as robotvisualiser
vmap.display()
import pylab
pylab.ion()
pylab.imshow(di_disconts)
pylab.draw()
PC_vis = robotvisualiser.Visualiser(title='points')
PC_vis.setautominmax()
PC_vis.addmappts(pc_disconts.points)
#import pdb;pdb.set_trace()
start = time.time()
for i in range(1, 16):
print 'Aligning scan', i
pc, di = get_PC(i)
print "extracting feature set"
di_disconts = depth_image.depth_discont_sobel(di)
pc_disconts = depth_image.image_to_points(di_disconts)
if visualise:
pylab.imshow(di_disconts)
pylab.draw()
PC_vis.clear()
PC_vis.addmappts(pc_disconts.points)
#import pdb;pdb.set_trace()
print "performing alignment"
bestpose, maxcost = vmap.align_points(pc_disconts,
guessPose,
full_data=True)
#bestpose = pc.pose # use the true value
print 'Bestpose:', bestpose
#vmap.add_points(pc, bestpose)
vmap.add_points(pc_disconts, bestpose)
self.check_localise(bestpose, pc.pose)
guessPose = bestpose
#import pdb;pdb.set_trace()
taken = time.time() - start
print 'Mapbuild time:', taken, 'seconds'
print 'Map size (voxels):', len(vmap)
import pdb
pdb.set_trace()
print 'Perfect map has 19812 voxels, press "c" to continue.'
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 __init__(self, objectName, poses, boxFilter=[-.08, .08,-.08, .08, 0.012, .4] , \
mrolLevels = 4, res = .002, transThresh=.04, rotThresh=55.0, viz = 0):
self.objectName = objectName
self.boxFilter = boxFilter #[-x, x, -y, y, -z, z] for boxFilter
self.mrolLevels = mrolLevels #number of self.resolution layers for MROL
self.res = res #self.resolution in meters
self.transThresh = transThresh # Threshold for merging PCs -- in meters
self.rotThresh = rotThresh # Threshold for merging PCs -- in degrees
# self.baseDir = 'data/'+objectName+'/'
self.baseDir = sys.argv[0][0:sys.argv[0].rfind('/') +
1] + 'data/' + objectName + '/'
self.pose_list = []
# if poses == []:
# self.truth_T = truth.getTransformations()
# for i in range(self.truth_T.shape[2]):
# P = poseutil.Pose3D()
# P.setMatrix(self.truth_T[:,:,i])
# self.pose_list.append(P)
# else:
self.pose_list = poses
print self.pose_list
P = poseutil.Pose3D()
point_clouds = {}
PCs = []
self.vmap = mapper.VoxelMap(self.res, levels=self.mrolLevels)
if viz:
self.vmap.display(npts=4e5)
self.run()
if __name__ == '__main__':
try:
datadir = sys.argv[1]
outfname = sys.argv[2]+".map"
fnames = os.listdir(datadir)
fnames.sort()
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,
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()
if fname.startswith('mrolpc')
]
else:
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)
# Initialize CSV file
time_ = time.gmtime()
# saveDir = current_dir[0:current_dir.rfind('/')] + '/data/'
outputName = 'RUN' + '%04i' % int(
runNumber) + "_" + teamName + "_" + '%4i%02i%02i' % (
time_[0], time_[1], time_[2]) + "_" + '%02i' % time_[
3] + ':' + '%02i' % time_[5] + ':' + '%02i' % time_[6] + '.csv'
print baseDir + outputName
output = write_csv.write2csv(baseDir + outputName, runNumber)
# Initialize MROL for visusalization
# P = poseutil.Pose3D()
if viz:
vmap = mapper.VoxelMap(mrol_res, levels=mrol_levels)
vmap.display(npts=4e5)
# Look through data
if bagName == []:
print "Error: No bag name"
else:
bag = bag2model(bagName)
# Get Camera info once
if cameraInfo != []:
msg_camera = bag.bag.read_messages(topics=cameraInfo)
bag.camera_info(msg_camera)
if pose != None:
msg_pose = bag.bag.read_messages(topics=pose)
