def SimulateStereoMeas(surface, workspace, sensornoise=.001, subsample=True, numstereo=10):
"""
simulate measurements from stereo depth mapping for the test functions above
inputs:
*surface: a function defining a test surface
*rangeX, rangeY: boundaries of the regions
*gridSize: resolution for simulated stereo measurements
outputs:
*xx,yy, z, matrices
This functions would be replaced by data from experiments
"""
x = np.linspace(workspace.bounds[0][0], workspace.bounds[0][1], num = numstereo)
y = np.linspace(workspace.bounds[1][0], workspace.bounds[1][1], num = numstereo)
if subsample==False:
# if data is too large and GP is too slow, may want to subsample
z = getStereoDepthMap(surface)[:40,:40]
z = np.pad(z,((0,10),(0,10)),mode='edge')
else:
interpf = getInterpolatedStereoMeas(surface,workspace)
z = interpf(x,y)
z = z #+ np.random.randn(z.shape[0],1)*sensornoise
xx, yy, z = stereo_pad(x,y,z,workspace.bounds[0],workspace.bounds[1])
return xx, yy, z
def getInterpolatedStereoMeas(surface, workspace):
z = getStereoDepthMap(surface)
z[z<5]= 5
z[z>30]=5 #if dont know its baseline
res = z.shape[0]
x = np.linspace(workspace.bounds[0][0], workspace.bounds[0][1], num = res)
y = np.linspace(workspace.bounds[1][0], workspace.bounds[1][1], num = res)
f = interpolate.interp2d(x, y, z, kind='cubic')
return f
