def vxltoPng(filename):
"""Converts VXL file to a PNG using amplitude data. Uses open cv libraries"""
camsys = Voxel.CameraSystem()
r = Voxel.FrameStreamReader(filename, camsys)
bool1, cols = r.getStreamParamu("frameWidth")
bool2, rows = r.getStreamParamu("frameHeight")
if not bool1 or not bool2:
print("Cannot read the stream")
if not r.isStreamGood():
print("Stream is not good: " + filename)
numFrames = r.size()
meanAmplitudes = np.zeros((rows, cols), dtype='float')
for i in (range(numFrames)):
if not r.readNext():
print("Failed to read frame %d" % i)
break
tofFrame = Voxel.ToF1608Frame.typeCast(
r.frames[Voxel.DepthCamera.FRAME_RAW_FRAME_PROCESSED])
meanAmplitudes += np.array(tofFrame._amplitude, copy=True).reshape(
(rows, cols))
r.close()
meanAmplitudes = meanAmplitudes / np.max(meanAmplitudes) * 255
outFileName = os.path.splitext(filename)[0] + '.png'
cv2.imwrite(outFileName, meanAmplitudes.astype(np.uint8))
def commonPhaseOffset(file1,
distance,
modFreq1,
cx,
cy,
file2=None,
modFreq2=None,
window=4,
chipset='ti.tinitn'):
c = 299792458 # m/s
camsys = Voxel.CameraSystem()
averagePhase1, rows, cols = computeAveragePhases(
camsys, file1, cx, cy, window) #default window size = 4
ds1 = c / (2 * modFreq1 * 1E6)
phaseActual1 = distance / ds1 * 4096
phaseAverage1 = averagePhase1
phaseCorr = wrapPhaseToSignedInteger(int(phaseAverage1 - phaseActual1))
if file2 and modFreq2:
averagePhase2, rows, cols = computeAveragePhases(
camsys, file2, cx, cy, window)
ds2 = c / (2 * modFreq2 * 1E6)
phaseActual2 = distance / ds2 * 4096
phaseAverage2 = averagePhase2
phaseCorr2 = wrapPhaseToSignedInteger(int(phaseAverage2 -
phaseActual2))
else:
phaseCorr2 = 0
if chipset == 'ti.calculus': #additive phase
phaseCorr = -phaseCorr
phaseCorr2 = -phaseCorr2
return True, phaseCorr, phaseCorr2, rows, cols
def perPixelOffset(fileName, pathToSave=None, profileName=None):
"""Computes pixelwise phase offset for all pixels. Returns a numpy file containing the pixelwise offsets as well as the bin file
.. note: Copy the bin file to the /path/to/.voxel/conf folder path before using it in the conf file
"""
camsys = Voxel.CameraSystem()
if not profileName:
profileName = os.path.basename(fileName).split('.')[0]
ret = computeAveragePhases(camsys, fileName, 0)
if not ret:
print "Can't calculate the phase offsets. Check file"
sys.exit()
else:
phases, rows, cols, mtx, dist = ret
if pathToSave is None:
newFile = os.path.splitext(fileName)[0] + PHASE_FILENAME
else:
newFile = pathToSave + profileName + PHASE_FILENAME
np.save(newFile, phases.T)
cGrid = np.mgrid[0:rows, 0:cols].astype(np.float32)
cGrid1D = np.reshape(np.transpose(cGrid, (1, 2, 0)),
(rows * cols, 1, 2)).astype(np.float32)
cGridCorrected1D = cv2.undistortPoints(cGrid1D, mtx, dist)
cGridCorrected = np.transpose(
np.reshape(cGridCorrected1D, (rows, cols, 2)), (2, 0, 1))
ry = cGridCorrected[0]
rx = cGridCorrected[1]
# Uncomment to get undistortion mapping
#with open('temp2.txt', 'w') as f:
#for r in range(0, self.rows):
#for c in range(0, self.columns):
#f.write('(%d, %d) -> (%.2f, %.2f)\n'%(self.cGrid[1, r, c], self.cGrid[0, r, c], rx[r, c], ry[r, c]))
rad2DSquare = ((rx**2) + (ry**2))
cosA = 1 / ((rad2DSquare + 1)**0.5)
deltaPhase = phases - phases[int(mtx[0, 2]), int(mtx[1, 2])] / cosA
if pathToSave is None:
phaseOffsetFileName = os.path.splitext(fileName)[0] + PHASE_OFFSET_FILE
else:
phaseOffsetFileName = pathToSave + os.sep + profileName + PHASE_OFFSET_FILE
with open(phaseOffsetFileName, 'wb') as f:
f.write(struct.pack('H', rows))
f.write(struct.pack('H', cols))
f.write(struct.pack('H', 0))
np.reshape(deltaPhase, rows * cols).astype(np.short).tofile(f)
return True, phaseOffsetFileName, rows, cols
def commonPhaseOffset(file1,
distance,
modFreq1,
cx,
cy,
file2=None,
modFreq2=None,
window=4,
chipset='ti.tinitn'):
""" Computes the common phase offsets.
This function calculates the common phase offsets for a given file, taking VXL as input.
**Parameters**::
- file1, file2: File(s) for computing common phase offsets. These should be vxl files of a flat wall. The camera should be almost parallel to the wall.
- modFreq1, modFreq2 : Modulation Frequency (ies) when the data is captured. If dealiasing is enabled, two modulation frequencies are required
- cx, cy: The coordinates of the center pixel, found during lens calibration. If cx and cy are not available, the central value (120,160) is taken.
- window: The window size. By default, a window of 4x4 is required.
- chipset: The chipset being used.
**Returns**::
- phase_Corr1: Phase offset value for the first modulation frequency
- phase_Corr2: Phase offset value for the second modulation frequency
"""
c = 299792458 # m/s
camsys = Voxel.CameraSystem()
averagePhase1, rows, cols = computeAveragePhases(
camsys, file1, cx, cy, window) #default window size = 4
ds1 = c / (2 * modFreq1 * 1E6)
phaseActual1 = distance / ds1 * 4096
phaseAverage1 = averagePhase1
phaseCorr = wrapPhaseToSignedInteger(int(phaseAverage1 - phaseActual1))
if file2 and modFreq2:
averagePhase2, rows, cols = computeAveragePhases(
camsys, file2, cx, cy, window)
ds2 = c / (2 * modFreq2 * 1E6)
phaseActual2 = distance / ds2 * 4096
phaseAverage2 = averagePhase2
phaseCorr2 = wrapPhaseToSignedInteger(int(phaseAverage2 -
phaseActual2))
else:
phaseCorr2 = 0
if chipset == 'ti.calculus': #additive phase
phaseCorr = -phaseCorr
phaseCorr2 = -phaseCorr2
return True, phaseCorr, phaseCorr2, rows, cols
def __init__(self):
super(CalibrationWizard, self).__init__()
# self.depthCamera = Voxel.DepthCamera()
self.cameraSystem = Voxel.CameraSystem()
self.devices = self.cameraSystem.scan()
if self.devices:
self.depthCamera = self.cameraSystem.connect(self.devices[0])
else:
self.depthCamera = None
ret, self.profilePath = Voxel.Configuration().getLocalPath('profiles')
self.configPath = os.getcwd() + os.sep + '..'
# self.editIndex = editIndex
self.setMinimumHeight(600)
self.setMinimumWidth(400)
self.setWizardStyle(QtGui.QWizard.ModernStyle)
self.setWindowTitle('Calibration Wizard')
self.addPage(CalibrationInitPage(self))
self.addPage(CalibrationSelectPage(self))
self.profileName = None
self.calibs = OrderedDict()
self.paths = {}
self.dataCapture = []
self.pages = {}
self.calibParams = {}
self.chipset = None
self.previousConfiguration = []
self.previousConfigurations = {}
self.calibPages = [('lens', CalibrationLensPage(self)),
('capture', CalibrationDataCapturePage(self)),
('nonLinearity', CalibrationNonLinearityPage(self)),
('temperature', CalibrationTemperaturePage(self)),
('commonPhase', CalibrationCommonPhasePage(self)),
('hdrCommonPhase',
CalibrationHDRCommonPhasePage(self)),
('perPixel', CalibrationPerPixelOffsetPage(self))]
for name, page in self.calibPages:
self.addCalibrationPage(name, page)
self.addPage(CalibrationConclusionPage(self))
def hdrCommonPhaseOffset(fileName1,
distance,
modFreq1,
cx=0,
cy=0,
fileName2=None,
modFreq2=None,
window=4,
chipset='TintinCDKCamera'):
""" Calculates HDR phase offsets for ti chipsets.
.. note: Make sure that the hdr flag is on for the file before capturing the file"""
c = 299792458. # m/s
camsys = Voxel.CameraSystem()
ret1 = computeHDRAveragePhases(camsys, fileName1, cx, cy, window, chipset)
if not ret1:
return False
ret1, averagePhase1, rows, cols = ret1
ds1 = c / (2 * modFreq1 * 1E6)
phaseActual1 = distance / ds1 * 4096
hdrphaseCorr = wrapPhaseToSignedInteger(int(averagePhase1 - phaseActual1))
if chipset == 'TintinCDKCamera':
if fileName2 and modFreq2:
ret2, averagePhase2, _, _ = computeHDRAveragePhases(
camsys, fileName2, cx, cy, window)
if not ret2:
return True, hdrphaseCorr, 0
ds2 = c / (2 * modFreq2 * 1E6)
phaseActual2 = distance / ds2 * 4096
hdrphaseCorr2 = wrapPhaseToSignedInteger(
int(averagePhase2 - phaseActual2))
else:
hdrphaseCorr2 = 0
elif chipset == 'CalculusCDKCamera':
hdrphaseCorr = -hdrphaseCorr
hdrphaseCorr2 = 0
return True, hdrphaseCorr, hdrphaseCorr2
def flatWallLensCalibration(fileName1, distance1, fileName2, distance2, cx= 0 , cy = 0):
camsys = Voxel.CameraSystem()
nearPhase, _, _ = computeAveragePhases(camsys, fileName1, cx, cy, window= 0)
farPhase, _, _ = computeAveragePhases(camsys, fileName2, cx, cy, window = 0)
deltaPhase = farPhase - nearPhase
deltaPhaseSmooth = snd.uniform_filter (deltaPhase, 5, mode = 'nearest')
# 1. Compute normal pixel
ny, nx = np.unravel_index(np.argmin(deltaPhaseSmooth), deltaPhase.shape)
rows, columns = deltaPhase.shape
# 2. Construct a set of all possible distances from the Normal Pixel
# a is the set of all squared distances from the normal pixel
y = np.outer(np.linspace(0, rows - 1, rows), np.ones((columns,)))
x = np.outer(np.ones((rows,)), np.linspace(0, columns - 1, columns))
a = np.square(y - ny) + np.square(x - nx)
# a1 is the set of all unique distances from the normal pixel
(a1, inverse) = np.unique(a.reshape(-1), return_inverse=True)
# a_root is the matrix of all pixel distances from the normal pixel
a1Root = np.sqrt(a1)
# inverse1 is the matrix with the index in a1 of a pixel's distance from the normal pixel
inverse1 = inverse.reshape(rows, -1)
# 3. For every distance, take the set of pixels at the same distance from the normal pixel
# and take the mean of the corresponding cosThetas at each distance
cosThetas = []
cosThetaMatrix = np.zeros(inverse1.shape)
for index, value in enumerate(a1):
i = inverse1 == index
co = deltaPhaseSmooth[ny, nx]/np.mean(deltaPhaseSmooth[i])
cosThetas.append(co)
cosThetaMatrix[i] = co
cosThetas = np.array(cosThetas)
# This array is now the set of pixel distances vs cosThetas
# We can use this directly to compute per pixel offsets if we like
# Note: there might be some cosThetas > 1. Clamping it for now
cosThetas = np.clip(cosThetas, 0, 1)
cosThetaMatrix = np.clip(cosThetaMatrix, 0, 1)
# 4. Note: before computing tanThetas, we need to translate these to Cx, Cy - see step 4 in the PPT
cosThetas = cosThetas*deltaPhaseSmooth[ny, nx]/deltaPhaseSmooth[cy, cx]
cosThetaMatrix = cosThetaMatrix*deltaPhaseSmooth[ny, nx]/deltaPhaseSmooth[cy, cx]
tanThetas = np.tan(np.arccos(cosThetas))
# 5. Use either scipy.optimize.curve_fit or np.linalg.lstsq to fit the tanTheta function
# x is pixel distance (a1_root)
# and tanThetas is the measured tanTheta
# Note: should we skip a few distances at the beginning?
popt, pcov = sopt.curve_fit(lambda x, f, k1, k2, k3: f*x*(1 + x*x*(k1 + x*x*(k2 + x*x*k3))), tanThetas, a1Root)
#popt[] should now be f, k1, k2, k3
# And can be used for distortion matrix
# p1, p2 are assumed to be 0 (no tangential distortion) and fx, fy are assumed to be equal to f
if popt and pcov:
mtx = np.array([[popt[0], 0, cx], [0, popt[1], cy], [0 ,0 , 1]])
dist = np.array([popt[2], popt[3], 0., 0, popt[3]])
return True, mtx, dist
else:
return False
def __init__(self, device):
self.system = Voxel.CameraSystem()
self.resolution = VxlCameraInfo.deviceResolution[device]
import matplotlib.pyplot as plt
import numpy as np
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("-f",
"--file",
type=str,
help="Voxel file (.vxl)",
required=True)
args = parser.parse_args()
camsys = Voxel.CameraSystem()
r = Voxel.FrameStreamReader(args.file, camsys)
iAverage = None
qAverage = None
frameCount = 0
if not r.isStreamGood():
print 'Could not open stream'
sys.exit(1)
print 'Stream contains ', r.size(), ' frames'
count = r.size()
### point.i is intensity, which come from amplitude
### point.z is distance, which come from depth
# for index in range(pointCloudFrame.size()):
# point = pointCloudFrame.points[index]
# print("current point : index %s [ x : %s , y: %s ,z : %s ,i : %s]" %(index, point.x,point.y,point.z,point.i))
def stop(self):
if self.depthCamera:
self.depthCamera.stop()
self.depthCamera.clearAllCallbacks()
del self.depthCamera
self.depthCamera = None
cameraSystem = Voxel.CameraSystem()
devices = cameraSystem.scan()
if len(devices) == 1:
print(" Find one device.")
window = MainWindow(cameraSystem)
key = raw_input("Input enter key to quit.")
print(" Quit now.")
window.stop()
else:
print(" No device found.")
del cameraSystem
