def testeqandhash(self):
P = self.p
P = poseutil.Pose3D()
P.set([1.2, 2.3, 0.2, 0.12, 0.63, 0.9])
Q = poseutil.Pose3D()
Q.setMatrix(P.getMatrix())
self.assertEquals(P, Q)
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 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 load_poses(fname):
# skip comment lines
poses = {}
P = poseutil.Pose3D()
for line in open(fname):
if line.strip().startswith('#'):
continue
fields = line.split(None, 1)
fname = fields[0]
mat = np.asarray(eval(fields[1]))
P = poseutil.Pose3D()
# need to transpose from blender?
P.setMatrix(mat.T)
poses[fname] = P
return poses
def OFFtest_mapper_global_localisation(self):
dx = 0.5
dq = np.radians(15)
spread_pose = poseutil.Pose3D((dx, dx, 0, 0, 0, dq))
res = 0.04
map_mrol = mrol.MROL(res, levels=4)
#map_mrol.set_feature_range(8)
self.P1.volumetricsample(self.res + 0.01111)
self.P2.volumetricsample(0.08)
map_mrol.addpoints(self.P1)
pc1 = get_PC(1)[0]
guesspose = pc1.pose
bestpose, maxo = map_mrol.match(pc1, guesspose, spread_pose)
truepose = self.P2.pose
if visualise:
self.P1.display(color=(1, 0, 0))
self.P2.pose = bestpose
self.P2.display(color=(0, 1, 0))
print 'Best:', bestpose
print 'True:', truepose
poseerror = truepose.getCombined(bestpose.inverse())
pos_error = np.sum(np.abs(poseerror.get()[:3]))
ang_error = np.sum(np.abs(poseerror.get()[3:]))
print
print 'Total Position error (m): ', pos_error
print 'Total Angular error (degs):', np.degrees(ang_error)
angular_tol = np.radians(2)
position_tol = 0.05
self.assertTrue(bestpose.is_close(truepose, position_tol, angular_tol))
def get_scan_pose_from_file(voxel_test_query_pose_file):
Ppose = np.loadtxt(voxel_test_query_pose_file)
#T = transformations.translation_from_matrix(Ppose)
#R = transformations.euler_from_matrix(Ppose)
#return np.hstack((T, R))
newPose = poseutil.Pose3D()
newPose.setMatrix(Ppose)
return newPose.getTuple()
def testcombined(self):
p = self.p[0]
combo = p.getCombined(p)
self.assertTrue(np.allclose(combo.get(), p.get()))
p.set([1, 1, 0, 0, 0, np.radians(45)])
combo = p.getCombined(p)
expected = poseutil.Pose3D()
expected.set([1, 1 + 2**0.5, 0, 0, 0, np.radians(90)])
self.assertTrue(np.allclose(expected.get(), combo.get()))
def makePose3D(transform):
p = poseutil.Pose3D()
R = tf.transformations.quaternion_matrix(transform[1])
T = transform[0]
#M = [R,T;0,0,0,0]
M = np.zeros([4,4])
M = R
M[0:3,3:4] = np.asarray(T).reshape(3,1)
p.setMatrix(M)
return p
def randomsearch(mapset,res,Pall,guesspose):
''' Exhaustive search for ground truth
This function is used to generate the "true pose" which is used to generate the check data
sets for the segmentation. It should not be used widely until it is implemented
properly (see notes in implementation) and has more generic input.'''
poseobj = poseutil.Pose3D()
# TODO this is only valid for small poses should do it properly by using
# random rotations from transformations.py
dx = 0.1
dq = np.radians(1)
bestoverlap = 0
while True:
#start = time.time()
delta = np.hstack((np.random.normal(0, dx, 3), np.random.normal(0, dq, 3)))
poseobj.set(guesspose + delta)
overlap = occupiedlist.calccollisions(poseobj.getTuple(), mapset, res, Pall)
if overlap > bestoverlap:
bestoverlap = overlap
# TODO now search around new pose and refine? alternatively just do this randomly
print bestoverlap, ', ', poseobj
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)
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()
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()
### Pose Estimation
potential_names = [dIDs[i]]
# potential_names.append(dIDs[i])
scores, poses = poseEstimator.run(
pc, pose_centroid, potential_names)
# scores = 0
# poses = np.array([[0,0,0,0,0,0]])
bestPose = poses[0]
# print bestPose
# pdb.set_trace()
# bestPose = poses[np.argmin(scores)]
if bestPose != []:
T = bestPose[0:3]
p = poseutil.Pose3D()
p.set(bestPose)
R = p.getMatrix()[0:3, 0:3]
except:
print "Error getting pose for ", dIDs[i]
poses = np.array([[0, 0, 0, 0, 0, 0]])
T = bestPose[0:3]
p = poseutil.Pose3D()
p.set(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,
def testuniformposes(self):
centrepose = poseutil.Pose3D()
spreadpose = poseutil.Pose3D()
spreadpose.set([1, 1, 0, 0, 0, 1])
poses = poseutil.uniformposes(centrepose, spreadpose, 0.2, 0.1)
self.assertEquals(len(poses), 2541)
good_file_pc.append(root[i]+'/'+files[i][j])
elif files[i][j].find(".npz")>-1 and files[i][j].find('reg_')>-1:
good_file_reg_all.append(root[i]+'/'+files[i][j])
good_file_reg.sort()
good_file_reg_all.sort()
good_file_pc.sort()
file_count = len(good_file_pc)
pose_list = []
truth_T = truth.getTransformations()
for i in range(truth_T.shape[2]):
P = poseutil.Pose3D()
P.setMatrix(truth_T[:,:,i])
# pose_list.append(np.array([0,0,0,0,0,0]))
pose_list.append(P)
#Set centroid to zero
#for i in range(file_count):
# pose_list.append(np.load(good_file_reg_all[i])['pose'])
#
#centroid = np.mean(pose_list, axis=0)
#pose_list = np.array(pose_list)
#pose_list[:,0] -= centroid[0]
#pose_list[:,1] -= centroid[1]
#pose_list[:,2] -= centroid[2]
if load_pc:
fnames = [
datadir + '/' + fname for fname in fnames
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)
def setUp(self):
self.p = []
self.p.append(poseutil.Pose3D())
self.p.append(poseutil.Pose3D())
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,
pc = get_pointcloud(fname)
bestpose = iros_map.align_points(pc, bestpose)[0]
#sensor_model = mapper._build_sensor_model(0.1)
#
#SM_points = sensor_model.getpoints()
#inds = ol.calccollisions(bestpose, SM_points, query=True)
#inds = np.array(inds)
#hits = pointcloud.PointCloud(SM_points[inds > 0])
#
#
#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)