def combineseries(clouds = 'case', trans = 'bear1301126296.mat', \ start = 0, numframe = None, mean = np.array([-0.02, 0, 0.8]), connectends = 0): global tss totalcloud = pointcloud.pointcloud() mats = scipy.io.loadmat('trans/%s' % trans) Rs = mats['Rs']; ts = mats['ts'] if connectends: Rs,ts2 = combiner.makeprodidentity(Rs,ts,connectends) if start == 0 and numframe == None: numframe = Rs.shape[0] tss = np.zeros((Rs.shape[0],1,3)) Rss = np.zeros((Rs.shape[0],3,3)) pointclouds = [] Rc = np.diag([1.,1.,1.]) tc = np.array([0.,0.,0.])[None,:] for i in range(0,Rs.shape[0]): print 'iteration %d' % i tss[i] = tc Rss[i] = Rc tc = np.dot(ts[i].T, Rc) + tc Rc = np.dot(Rs[i].T, Rc) if connectends: diff = tss[0] - tss[numframe-1] for i in range(0, numframe): tss[i] = tss[i] + (i+1)*diff/(numframe) for j in range(0,numframe): i = j+start fname = 'data/%s/f%d.mat' % (clouds, i % numframe + 1) mat = scipy.io.loadmat(fname) newcloud = pointcloud.pointcloud(mat['depth'], mat['rgb']) newcloud.vertex -= mean newcloud.clipCube(bottom = 0.05, left = -.3, right = .3) newcloud.vertex = np.dot(newcloud.vertex, Rss[i]) + tss[i] pointclouds += [newcloud] totalcloud = pointcloud.pointcloud() for cloud in pointclouds: totalcloud += cloud serial = time.time() f = open('poly/poly%d.ply' % serial,'w') totalcloud.toPly(f) f.close()
def databaseType(db_type: int, points): """ Returns db object depended on db type """ if db_type == 0: return pointcloud(points) elif db_type == 1: return eqlcells(points) elif db_type == 2: return kdtree(points, False, 10)
def combine(mean = np.array([-0.02, 0.8]), angle = 45.): totalcloud = pointcloud.pointcloud() xz = np.array([0,2]) for i in range(8): fname = 'data/soysauce/f%d.mat' % (i+1) rot = -angle*np.pi/180 rotmat = np.array([[np.cos(i*rot), -np.sin(i*rot)], [np.sin(i*rot), np.cos(i*rot)]] ) mat = scipy.io.loadmat(fname) newcloud = pointcloud.pointcloud(mat['depth'], mat['rgb']) newcloud.clipCube(bottom = -0.01) newcloud.vertex[:,xz] = np.dot(newcloud.vertex[:,xz] - mean, rotmat) totalcloud += newcloud serial = time.time() f = open('poly/poly%d.ply' % serial,'w') totalcloud.clipCube(bottom = -0.01) totalcloud.toPly(f) f.close()
def databaseType(db_type: int, points): """ Creates database from points according to id given and points :param points: ndarray, points xyz nx3 :param db_type: 0 = pointcloud, 1 = equalcells, 2 = kdtree :return: Database object """ if db_type == 0: return pointcloud(points) elif db_type == 1: return eqlcells(points) elif db_type == 2: return kdtree(points, False, 10)
def gettransform(mean = np.array([-0.02, 0, 0.8]), angle = 45., dataset = 'bear'): global Pi, pointclouds # confidence in initial guess confidence = 5 numframe = 18 pointclouds = [] numsample = 5000 Rs = np.zeros((numframe, 3, 3)) ts = np.zeros((numframe, 3, 1)) for i in range(0,numframe+1): print 'iteration %d' % i fname = 'data/%s/f%d.mat' % (dataset, i % numframe + 1) mat = scipy.io.loadmat(fname) newcloud = pointcloud.pointcloud(mat['depth'], mat['rgb']) newcloud.vertex -= mean newcloud.clipCube(bottom = 0.06, left = -.3, right = .3) if i > 0: i1 = prevcloud.vertex.shape[0] r1 = (np.random.rand(numsample) * i1).astype(int) i2 = newcloud.vertex.shape[0] r2 = (np.random.rand(numsample) * i2).astype(int) prevsample = prevcloud.vertex[r1,:] nextsample = newcloud.vertex[r2,:] R, t = combiner.probcombine(prevsample.T,\ nextsample.T, conf = confidence) Rs[i-1] = R; ts[i-1] = t pointclouds += [newcloud] prevcloud = newcloud # end for serial = time.time() scipy.io.savemat('trans/%s%d.mat' % (dataset, serial), {'Rs':Rs, 'ts':ts});
def testcombine(mean = np.array([-0.02, 0, 0.8]), angle = 45.): global Pi, pointclouds # confidence in initial guess confidence = 1 pointclouds = [] Rc = np.diag([1.,1.,1.]) tc = np.array([0.,0.,0.])[None,:] for i in range(0,20): print 'iteration %d' % i fname = 'data/case/f%d.mat' % (i+1) rot = -angle*np.pi/180 rotmat = np.array([[np.cos(i*rot), 0, -np.sin(i*rot)], [0 , 1, 0], [np.sin(i*rot), 0, np.cos(i*rot)]] ) mat = scipy.io.loadmat(fname) cref = np.zeros((480,640,3)) if i == 0: cref[:,:,0] = 255 else: cref[:,:,2] = 255 newcloud = pointcloud.pointcloud(mat['depth'], mat['rgb']) newcloud.vertex -= mean newcloud.clipCube(bottom = 0.0, left = -.3, right = .3) if i > 0: newcloud.vertex = np.dot(newcloud.vertex, Rc) + tc i1 = prevcloud.vertex.shape[0] r1 = (np.random.rand(2000) * i1).astype(int) i2 = newcloud.vertex.shape[0] r2 = (np.random.rand(2000) * i2).astype(int) prevsample = prevcloud.vertex[r1,:] nextsample = newcloud.vertex[r2,:] R, t = combiner.probcombine(prevsample.T,\ nextsample.T, conf = confidence) Rc = np.dot(Rc, R.T) tc = np.dot(tc, R.T) + t.T newcloud.vertex = np.dot(newcloud.vertex, R.T) + t.T #newcloud.color[r2,:] *= Pi[:,None]/Pi.max() #totalcloud.vertex = totalsample pointclouds += [newcloud] prevcloud = newcloud # end for totalcloud = pointcloud.pointcloud() for cloud in pointclouds: totalcloud += cloud serial = time.time() f = open('poly/poly%d.ply' % serial,'w') totalcloud.toPly(f) f.close()