def test2M(self):
# 2 Meters away depth scan
pt = np.array([0, 0, 2, 1])
print "PC", pt
mat = matmath.pcToSoSMatrix()
npt = pt.dot(mat)
print "SoS ", npt
trans = np.array([0, 0, 0])
quaternion = matmath.axisAngleToQuaternion([1, 0, 0], np.radians(90))
mat = matmath.getPC2WorldMatrix(trans, quaternion)
npt = pt.dot(mat)
print "Device", npt
pt = np.array([0, 1, 2, 1])
print "PC", pt
mat = matmath.pcToSoSMatrix()
npt = pt.dot(mat)
print "SoS ", npt
mat = matmath.getPC2WorldMatrix(trans, quaternion)
npt = pt.dot(mat)
print "Device", npt
def processFile(datafile, imgfile):
inp = open(datafile, "r")
pic = Image.open(imgfile)
# Read translation
trans = np.array(struct.unpack("<3d", inp.read(3 * 8)))
print "Offset", trans
# Read rotation
quaternion = np.array(struct.unpack("<4d", inp.read(4 * 8)))
print "Quaternion", quaternion
(axis, angle) = quaternionToAxisAngle(quaternion)
print "Axis", axis, "Angle", degrees(angle)
# Read Field of VIew
(hFOV, vFOV) = struct.unpack("<2d", inp.read(2 * 8))
print "hFOV=", hFOV, "vFOV=", vFOV
# Read points
numPts = struct.unpack("<i", inp.read(4))[0]
print "Numpts", numPts
cvtmat = getPC2WorldMatrix(trans, quaternion)
(base, ext) = os.path.splitext(datafile)
out = open(base + ".ply", "w")
writeHeader(out, numPts)
data = []
for i in xrange(numPts):
buf = inp.read(3 * 4)
if buf == '':
break
cam = np.array(struct.unpack("<3f", buf))
dev = np.array([cam[0], cam[1], cam[2], 1])
imu = dev.dot(cvtmat)
(r, g, b) = findColor(pic, hFOV, vFOV, dev)
print >> out, imu[0], imu[1], imu[2], r, g, b
data.append((imu[0], imu[1], imu[2]))
# Read IJ (Not Yet Implemented)
ijPts = struct.unpack("<i", inp.read(4))[0]
print "ijPts", ijPts
out.close()
out2 = open(base + ".json", "w")
json.dump({'points': data}, out2)
out2.close()
def processFile(datafile, imgfile):
inp = open(datafile, "rb")
pic = Image.open(imgfile)
# Read translation
trans = np.array(struct.unpack("<3d", inp.read(3*8)))
print ("Offset", trans)
# Read rotation
quaternion = np.array(struct.unpack("<4d", inp.read(4*8)))
print "Quaternion", quaternion
(axis, angle) = quaternionToAxisAngle(quaternion)
print "Axis", axis, "Angle", degrees(angle)
# Read Field of VIew
(hFOV, vFOV) = struct.unpack("<2d", inp.read(2*8))
print "hFOV=", hFOV, "vFOV=", vFOV
# Read points
numPts=struct.unpack("<i", inp.read(4))[0]
print "Numpts", numPts
cvtmat = getPC2WorldMatrix(trans, quaternion)
(base, ext) = os.path.splitext(datafile)
out = open(base+".ply", "w")
writeHeader(out, numPts)
data = [];
for i in xrange(numPts):
buf = inp.read(3*4)
if buf == '':
break
cam = np.array(struct.unpack("<3f", buf))
dev = np.array([cam[0], cam[1], cam[2], 1])
imu = dev.dot(cvtmat)
(r, g, b) = findColor(pic, hFOV, vFOV, dev)
print >>out, imu[0], imu[1], imu[2], r, g, b
data.append((imu[0], imu[1], imu[2]))
# Read IJ (Not Yet Implemented)
ijPts = struct.unpack("<i", inp.read(4))[0]
print "ijPts", ijPts
out.close()
out2 = open(base+".json", "w")
json.dump({'points': data}, out2)
out2.close()
