def plotfakedata(ind=2, maxiter=500, interval=50):
global Rs, ts, firstplot
A, B, Ao = datafunctions[ind].__call__()
if firstplot:
R, t = combiner.probcombine(A, B)
Rs = combiner.Rs
ts = combiner.ts
firstplot = False
select = np.arange(0, maxiter, interval)
for s in select:
print s
R = Rs[s]
t = ts[s][:, None]
if la.det(R) < 0.1:
break
C = np.dot(R, B) + t
import matplotlib.pyplot as plt
plt.scatter(A[0, :], A[1, :], c="r", alpha=0.5)
plt.scatter(B[0, :], B[1, :], c="g", alpha=0.5)
plt.scatter(C[0, :], C[1, :], c="b", alpha=0.5)
plt.axis([-3, 3, -3, 3])
plt.savefig("figure%d" % s)
plt.clf()
plt.show()
def testfakedata(ind=0):
A, B, Ao = datafunctions[ind].__call__()
R, t = combiner.probcombine(A, B)
print R
print t
C = np.dot(R, B) + t
import matplotlib.pyplot as plt
plt.scatter(A[0, :], A[1, :], c="r", alpha=0.5)
plt.scatter(B[0, :], B[1, :], c="g", alpha=0.5)
plt.scatter(C[0, :], C[1, :], c="b", alpha=0.5)
plt.scatter(Ao[0, :], Ao[1, :], c="b", marker="+", alpha=0.5)
plt.axis([-3, 3, -3, 3])
def runalltests(iter=1):
for i in range(iter):
tra = np.random.randn(3)
rot = np.random.rand() * np.pi * 2
rotmat = np.array([[np.cos(rot), -np.sin(rot), 0], [np.sin(rot), np.cos(rot), 0], [0, 0, 1.0]])
for f in datafunctions:
A, B, Aori = f.__call__(rot=rot, t=tra)
R, t = combiner.probcombine(A, B)
normR = la.norm(R - rotmat, ord=2)
normt = la.norm(t - tra, ord=1)
print R
print t
print "test iter %d %s diffR = %f difft = %f" % (i, str(f), normR, normt)
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()
