def forward_pass(self, inputs, param_vector):
#n = mesh_traversal.get_neighs_sq(inputs)
#result = np.array(n)
#result = np.moveaxis(result, 0, 2)
# if inputs.shape[2] == 576:
new_shape = inputs.shape[:2]
for i in [0]:
pool_width = self.pool_shape[i]
img_width = inputs.shape[i + 2]
new_dim = int((np.sqrt(img_width) / np.sqrt(pool_width)))
new_shape += (new_dim * new_dim, )
result = []
for i in range(new_dim):
for j in range(new_dim):
x = (3 * j + 25) + 24 * 3 * i
n = mesh_traversal.get_neighs_sq2(adj_mtx, x)
a = inputs[:, :, n[0]]
b = inputs[:, :, n[1]]
c = inputs[:, :, n[2]]
d = inputs[:, :, n[3]]
e = inputs[:, :, n[4]]
f = inputs[:, :, n[5]]
g = inputs[:, :, n[6]]
h = inputs[:, :, n[7]]
temp = np.stack((a, b, c, d, e, f, g, h))
temp = np.amax(temp, axis=0)
result.append(temp)
result = np.stack(result)
result = np.moveaxis(result, 0, 2)
return result
def as_strided_dyn(b, patch=5, stride=1):
"""
Strides the input data as preprocessing for convolution.
for efficient implementation
:param b: array to be strided
:param patch: patch is the length of one side of the patch. Must be smaller than smallest dimension of b
:param stride: desired stride between patches
:return: strided form of input data, prepared for convolution
"""
dims = b.shape
ex_ct = dims[0]
if dims[3] != dims[4]:
exit(-1)
else:
out = []
for k in range(ex_ct):
arr = []
for i in range(0, dims[3] - patch + 1, stride):
arr2 = []
for j in range(0, dims[4] - patch + 1, stride):
if i + patch <= dims[3] and j + patch <= dims[4]:
arr2.append([b[k, :, :, i:i + patch, j:j + patch]])
arr.append([arr2])
out.append([arr])
out = np.array(out)
out = out[:, :, :, 0, :, 0, 0, :, :, :]
out = np.moveaxis(out, 4, 2)
return out
def convolve_tensor(a, b):
"""
Convolves two tensorized arrays, most efficient convolution method.
:param a: first array to be convoluted
:param b: second array to be convoluted
:return: convolved array
"""
b = as_strided_seq(b, 5, 1)
b = np.moveaxis(b, [0, 1, 2, 3, 4, 5], [0, 3, 4, 5, 1, 2])
b = np.moveaxis(b, 5, 1)
try:
out = npo.einsum(a, [12, 1, 10, 11], b, [4, 12, 10, 11, 8, 9])
out = np.swapaxes(out, 0, 1)
except:
a = a._value
out = npo.einsum(a, [12, 1, 10, 11], b, [4, 12, 10, 11, 8, 9])
out = np.swapaxes(out, 0, 1)
return out
def anp_partial_trace(rho, dims, axis=0):
"""
Takes partial trace over the subsystem defined by 'axis'
rho: a matrix
dims: a list containing the dimension of each subsystem
axis: the index of the subsytem to be traced out
(We assume that each subsystem is square)
"""
dims_ = np.array(dims)
reshaped_rho = np.reshape(rho, np.concatenate((dims_, dims_), axis=None))
reshaped_rho = np.moveaxis(reshaped_rho, axis, -1)
reshaped_rho = np.moveaxis(reshaped_rho, len(dims) + axis - 1, -1)
assert reshaped_rho.shape[-1] == reshaped_rho.shape[-2]
traced_out_rho = sum(
[reshaped_rho[:, :, i, i] for i in range(reshaped_rho.shape[-1])])
dims_untraced = np.delete(dims_, axis)
rho_dim = np.prod(dims_untraced)
return traced_out_rho.reshape([rho_dim, rho_dim])
def rgb_gauss_random_samples(N,
mean_or_means=None,
cov_or_covs=None,
im_sz=None):
means, covs, im_sz = _handle_kwargs(mean_or_means, cov_or_covs, im_sz)
samples_by_channel = np.asarray([
np.random.multivariate_normal(mean, cov, N).T
for mean, cov in zip(means, covs)
])
samples_batch_minor = np.moveaxis(samples_by_channel, [0, 1], [1, 0])
return samples_batch_minor
def transitionProb(self, child):
parents, parent_order = self.getParents(child, get_order=True)
ndim = len(parents) + 1
pi = np.copy(self.pis[ndim])
# If we know the latent state for child, then ensure that we
# transition there. Also make sure we're only using the possible
# parent latent states!!!!
modified = False
for parent, order in zip(parents, parent_order):
if (int(parent) in self.possible_latent_states):
parent_states = self.possible_latent_states[int(parent)]
impossible_parent_axes = np.setdiff1d(
np.arange(pi.shape[order]), parent_states)
index = [slice(0, s) for s in pi.shape]
index[order] = impossible_parent_axes
pi[tuple(index)] = np.NINF
modified = True
if (int(child) in self.possible_latent_states):
child_states = self.possible_latent_states[int(child)]
impossible_child_axes = np.setdiff1d(np.arange(pi.shape[-1]),
child_states)
pi[..., impossible_child_axes] = np.NINF
modified = True
if (modified == True):
with np.errstate(invalid='ignore'):
pi[..., :] -= logsumexp(pi, axis=-1)[..., None]
# In case entire rows summed to -inf
pi[np.isnan(pi)] = np.NINF
# Reshape pi's axes to match parent order
assert len(parents) + 1 == pi.ndim
assert parent_order.shape[0] == parents.shape[0]
pi = np.moveaxis(pi, np.arange(ndim),
np.hstack((parent_order, ndim - 1)))
return pi
def transitionProb(self, child, is_partial_graph_index=False):
parents, parent_order = self.getFullParents(
child,
get_order=True,
is_partial_graph_index=is_partial_graph_index,
return_partial_graph_index=True)
child_full = self.partialGraphIndexToFullGraphIndex(
child) if is_partial_graph_index == True else child
ndim = len(parents) + 1
group = self.node_groups[int(child_full)]
shape = []
for p, _ in sorted(zip(parents, parent_order), key=lambda po: po[1]):
full_p = int(self.partialGraphIndexToFullGraphIndex(p))
shape.append(self.getNodeDim(full_p))
shape.append(self.getNodeDim(int(child_full)))
shape = tuple(shape)
pi = np.copy(self.pis[group][shape])
# Reshape pi's axes to match parent order
assert len(parents) + 1 == pi.ndim
assert parent_order.shape[0] == parents.shape[0]
# Sort the parent dimensions by parent order
pi = np.moveaxis(pi, np.arange(ndim),
np.hstack((parent_order, ndim - 1)))
# If we know the latent state for child, then ensure that we
# transition there. This is intervention!
modified = False
for parent, order in zip(parents, parent_order):
parent_full = self.partialGraphIndexToFullGraphIndex(parent)
if (int(parent_full) in self.possible_latent_states):
parent_states = self.possible_latent_states[int(parent_full)]
impossible_parent_axes = np.setdiff1d(
np.arange(pi.shape[order]), parent_states)
index = [slice(0, s) for s in pi.shape]
index[order] = impossible_parent_axes
pi[tuple(index)] = np.NINF
modified = True
if (int(child_full) in self.possible_latent_states):
states = self.possible_latent_states[int(child_full)]
impossible_axes = np.setdiff1d(np.arange(pi.shape[-1]), states)
pi[..., impossible_axes] = np.NINF
modified = True
# In case entire rows summed to -inf
pi[np.isnan(pi)] = np.NINF
# Check if there are nodes in [ child, *parents ] that are in the fbs.
# If there are, then move their axes
fbsOffset = lambda x: self.fbsIndex(
x, is_partial_graph_index=True, within_graph=True) + 1
fbs_indices = [
fbsOffset(parent) for parent in parents
if self.inFeedbackSet(parent, is_partial_graph_index=True)
]
if (self.inFeedbackSet(child,
is_partial_graph_index=is_partial_graph_index)):
fbs_indices.append(
self.fbsIndex(child,
is_partial_graph_index=is_partial_graph_index,
within_graph=True) + 1)
if (len(fbs_indices) > 0):
expand_by = max(fbs_indices)
for _ in range(expand_by):
pi = pi[..., None]
# If there are parents in the fbs, move them to the appropriate axes
for i, parent in enumerate(parents):
if (self.inFeedbackSet(parent, is_partial_graph_index=True)):
pi = np.swapaxes(pi, i, fbsOffset(parent) + ndim - 1)
if (self.inFeedbackSet(
child, is_partial_graph_index=is_partial_graph_index)):
# If the child is in the fbs, then move it to the appropriate axis
pi = np.swapaxes(pi, ndim - 1, fbsOffset(child) + ndim - 1)
return fbsData(pi, ndim)
return fbsData(pi, -1)
def multiCamCalib(umeas, xworld, cameraMats, boardRotMats, boardTransVecs,
planeData, cameraData):
"""Compute the calibrated camera matrix, rotation matrix, and translation vector for each camera.
Inputs
-------------------------------------------
umeas 2 x nX*nY*nplanes x ncams array of image points in each camera
xworld 3 x nX*nY*nplanes of all world points
cameraMats 3 x 3 x ncams that holds all camera calibration matrices; first estimated in single camera calibration
boardRotMats 3 x 3*nplanes x ncams for each board in each camera; from single camera calibration.
boardTransVecs 3 x nplanes x ncams for each board in each camera; from single camera calibration
planeData struct of image related parameters
cameraData struct of camera related parameters
Returns
-------------------------------------------
cameraMats 3 x 3 x ncams; final estimate of the camera matrices
rotationMatsCam 3 x 3 x ncams; final estimate of the camera rotation matrices
transVecsCam 3 x ncams; final estimate of the camera translational vectors
"""
# loop over all camera pairs
# call kabsch on each pair
# store rotation matrices and translational vectors
# Calculate terms in sum, add to running sum
log.info("Multiple Camera Calibrations")
# Extract dimensional data.
ncams = cameraData.ncams
nplanes = planeData.ncalplanes
# Storage variables.
R_pair = np.zeros([3, 3, ncams, ncams])
t_pair = np.zeros([3, ncams, ncams])
xworld = np.transpose(xworld)
for i in range(0, ncams):
for j in range(0, ncams):
if i == j:
t_pair[:, i, j] = np.zeros(3)
R_pair[:, :, i, j] = np.eye(3, 3)
continue
log.VLOG(2, 'Correlating cameras (%d, %d)' % (i, j))
# Compute world coordinates used by kabsch.
Ri = np.concatenate(np.moveaxis(boardRotMats[:, :, :, i], 2, 0),
axis=0)
ti = boardTransVecs[:, :, i].reshape((-1, 1), order='F')
Rj = np.concatenate(np.moveaxis(boardRotMats[:, :, :, j], 2, 0),
axis=0)
tj = boardTransVecs[:, :, j].reshape((-1, 1), order='F')
# Compute world coordinates and use Kabsch
Xi = np.matmul(Ri, xworld) + ti
Xj = np.matmul(Rj, xworld) + tj
Xi = Xi.reshape((3, -1), order='F')
Xj = Xj.reshape((3, -1), order='F')
R_pair_ij, t_pair_ij = kabsch(Xi, Xj)
R_pair[:, :, i, j] = R_pair_ij
t_pair[:, i, j] = t_pair_ij
log.VLOG(
3, 'Pairwise rotation matrix R(%d, %d) = \n %s' %
(i, j, R_pair_ij))
log.VLOG(
3, 'Pairwise translation vector R(%d, %d) = %s' %
(i, j, t_pair_ij))
log.info("Kabsch complete. Now minimizing...")
##################### Solve minimization problems for Rotation and Translation Estimates ####################
# Solve linear least square problem to minimize translation vectors of all cameras.
# This array is contructed here per pair of cameras and later resrtcutured to set up the linear minimization problem
# Ax - b.
log.info(
"Minimizing for first estimates of translation vectors per camera.")
A = np.zeros((3, 3 * ncams, ncams, ncams))
b = np.zeros((3, ncams, ncams))
# Construct expanded matrix expression for minimization.
for i in range(0, ncams):
for j in range(0, ncams):
if i == j:
continue
# Rij = R_pair[:, :, min(i, j), max(i, j)]
# tij = t_pair[:, min(i, j), max(i, j)]
Rij = R_pair[:, :, i, j]
tij = t_pair[:, i, j]
A[:, 3 * i:3 * (i + 1), i, j] = -Rij
A[:, 3 * j:3 * (j + 1), i, j] = np.eye(3)
b[:, i, j] = tij
A = np.concatenate(np.concatenate(np.moveaxis(A, (2, 3), (0, 1)), axis=0),
axis=0)
b = b.reshape((-1, 1), order='F')
log.VLOG(4, 'Minimization matrix A for translation vectors \n %s' % A)
log.VLOG(4, 'Minimization vector b for translation vectors \n %s' % b)
# We want to constrain only the translational vector for the first camera
# Create a constraint array with a 3x3 identity matrix in the top left
constraint_array = np.zeros([3 * ncams, 3 * ncams])
constraint_array[0:3, 0:3] = np.eye(3)
# Solve the minimization, requiring the first translational vector to be the zero vector
# Initialize camera positions assuming the first camera is at the origin and the cameras
# are uniformly spaced by horizontal a displacement vector and a vertical vector such as in
# a rectangular grid of the dimensions to be specified.
def trans_cost(x):
return np.linalg.norm(np.matmul(A, np.array(x)) - b)
# trans_cost_deriv = autograd.grad(lambda *args: trans_cost(np.transpose(np.array(args))))
# Construct the problem.
x = cp.Variable((3 * ncams, 1))
objective = cp.Minimize(cp.sum_squares(A @ x - b))
constraints = [constraint_array @ x == np.zeros((3 * ncams, 1))]
prob = cp.Problem(objective, constraints)
print("Optimal value", prob.solve())
if prob.status == cp.OPTIMAL:
log.info("Minimization for Translation Vectors Succeeded!")
print('Optimized translation error: ', trans_cost(x.value))
t_vals = x.value
else:
log.error('Minimization Failed for Translation Vectors!')
return
# Translation vectors stored as columns.
t_vals = np.transpose(t_vals.reshape((-1, 3)))
for i in range(t_vals.shape[1]):
log.VLOG(3, 't(%d) = \n %s' % (i, t_vals[:, i]))
# log.info('Minimizing for translation vectors of cameras: %s', res.message)
# Solve linear least square problem to minimize rotation matrices.
log.info("Minimizing for first estimates of rotation matrices per camera.")
A = np.zeros((9, 9 * ncams, ncams, ncams))
# Construct expanded matrix expression for minimization.
for i in range(0, ncams):
for j in range(0, ncams):
if i == j:
continue
Rij = R_pair[:, :, i, j]
A[:, 9 * i:9 * (i + 1), i, j] = np.eye(9)
A[:, 9 * j:9 * (j + 1), i, j] = -np.kron(np.eye(3), Rij)
A = np.concatenate(np.concatenate(np.moveaxis(A, (2, 3), (0, 1)), axis=0),
axis=0)
b = np.zeros(A.shape[0])
log.VLOG(4, 'Minimization matrix A for rotation matrices \n %s' % A)
log.VLOG(4, 'Minimization vector b for rotation matrices \n %s' % b)
# We want to constrain only the rotation matrix for the first camera
# Create a constraint array with a 9x9 identity matrix in the top left
constraint_array = np.zeros([9 * ncams, 9 * ncams])
constraint_array[0:9, 0:9] = np.eye(9)
bound = np.zeros(9 * ncams)
bound[0] = 1
bound[4] = 1
bound[8] = 1
# Initialize all rotation matrices to identities assuming no camera is rotated with respect to another.
x0 = np.zeros(9 * ncams)
for i in range(ncams):
x0[9 * i] = 1
x0[9 * i + 4] = 1
x0[9 * i + 8] = 1
# Solve the minimization, requiring the first rotation matrix to be the identity matrix
# Construct the problem.
x = cp.Variable(9 * ncams)
objective = cp.Minimize(cp.sum_squares(A @ x - b))
constraints = [constraint_array @ x == bound]
prob = cp.Problem(objective, constraints)
print("Optimal value", prob.solve())
print('Minimization status: ', prob.status)
if prob.status == cp.OPTIMAL:
log.info("Minimization for Rotational Matrices Succeeded!")
R_vals = x.value
else:
log.error('Minimization Failed for Rotational Matrices!')
return
# Rotation matrices stored rows first
R_vals = np.moveaxis(R_vals.reshape(-1, 3, 3), 0, 2)
# Fit rotation matrices from optimized result.
for i in range(ncams):
R_vals[:, :, i] = fitRotMat(R_vals[:, :, i])
log.VLOG(3, 'R(%d) = \n %s' % (i, R_vals[:, :, i]))
log.info(
"Finding average rotation matrices and translational vectors from single camera calibration."
)
# Obtain average Rs and ts per images over cameras.
R_images = np.sum(boardRotMats, axis=3) / ncams
t_images = np.sum(boardTransVecs, axis=2) / ncams
for i in range(R_images.shape[2]):
log.VLOG(
3, 'Average rotation matrix for Image %d = \n %s' %
(i, R_images[:, :, i]))
for i in range(t_images.shape[1]):
log.VLOG(
3, 'Average translation vector for Image %d = \n %s' %
(i, t_images[:, i]))
####################### Final Minimization to Yield All Parameters######################
# K_c, R_c, R_n, t_c, t_n.
# cameraMats, R_vals, t_vals = interpretResults('../../../FILMopenfv-samples/sample-data/pinhole_calibration_data/calibration_results/results_000001438992543.txt')
# Pack all matrices into a very tall column vector for minimization
min_vec_ini = np.append(
cameraMats.reshape((-1)),
np.append(
R_vals.reshape((-1)),
np.append(R_images.reshape((-1)),
np.append(t_vals.reshape((-1)), t_images.reshape(
(-1))))))
planeVecs = {}
# Set up objective function and Jacobian sparsity structure for minimization.
def reproj_obj_lsq(min_vec):
# Compute the reprojection error at each intersection per each camera per each image.
cameraMats, rotMatsCam, rotMatsBoard, transVecsCam, transVecsBoard = unpack_reproj_min_vector(
cameraData, planeData, min_vec)
err = np.zeros(ncams * nplanes * nX * nY)
# Set pixel normalization factor to 1 here.
normal_factor = 1
for c in range(ncams):
for n in range(nplanes):
for i in range(nX):
for j in range(nY):
err[c * nplanes * nX * nY + n * nX * nY + i * nY + j] = \
reproj_error(normal_factor, umeas[:, n * nX * nY + i * nY + j, c],
xworld[:, nX * nY * n + i * nY + j], cameraMats[:, :, c],
rotMatsCam[:, :, c], rotMatsBoard[:, :, n], transVecsCam[:, c],
transVecsBoard[:, n])
return err
def bundle_adjustment_sparsity():
m = ncams * nplanes * nX * nY
n = min_vec_ini.shape[0]
A = lil_matrix((m, n), dtype=int)
num_nonzeros = np.sum([
np.prod(x.shape[:-1])
for x in [cameraMats, R_vals, t_vals, R_images, t_images]
])
for c in range(ncams):
# make boolean arrays for camera-indexed arrays
cams = np.zeros(cameraMats.shape)
cams[:, :, c] = np.ones(cameraMats.shape[:-1])
rots = np.zeros(R_vals.shape)
rots[:, :, c] = np.ones(R_vals.shape[:-1])
ts = np.zeros(t_vals.shape)
ts[:, c] = np.ones(t_vals.shape[:-1])
cams = cams.reshape(-1)
rots = rots.reshape(-1)
ts = ts.reshape(-1)
for n in range(nplanes):
# make boolean arrays for plane-indexed arrays
if n not in planeVecs:
rimgs = np.zeros(R_images.shape)
rimgs[:, :, n] = np.ones(R_images.shape[:-1])
timgs = np.zeros(t_images.shape)
timgs[:, n] = np.ones(t_images.shape[:-1])
planeVecs[n] = rimgs, timgs
else:
rimgs, timgs = planeVecs[n]
rimgs = rimgs.reshape(-1)
timgs = timgs.reshape(-1)
# create boolean array with changed values for jacobian
x0 = np.append(
cams.reshape((-1)),
np.append(
rots.reshape((-1)),
np.append(
rimgs.reshape((-1)),
np.append(ts.reshape((-1)), timgs.reshape((-1))))))
# set changing values in jacobian
A[nY * nX * (n + nplanes * c):nY * nX * (n + 1 + nplanes * c),
x0.nonzero()] = np.ones((nY * nX, num_nonzeros))
return A
A = bundle_adjustment_sparsity()
reproj_res = scipy.optimize.least_squares(reproj_obj_lsq,
min_vec_ini,
jac_sparsity=A,
verbose=2,
x_scale='jac',
ftol=1e-2)
if reproj_res.success:
finError = reproj_min_func(planeData, cameraData, umeas, xworld,
reproj_res.x)
log.info("Final error: {}".format(finError))
log.info("Reprojection Minimization Succeeded!")
cameraMats, rotationMatsCam, rotationMatsBoard, transVecsCam, transVecsBoard = unpack_reproj_min_vector(
cameraData, planeData, reproj_res.x)
else:
log.error('Reprojection Minimization Failed!')
return
return cameraMats, rotationMatsCam, rotationMatsBoard, transVecsCam, transVecsBoard, finError
def to_batch_minor(self, inputs):
"""Reorder [batch, channels, y, x]
to [y, x, channels, batch]
"""
return np.moveaxis(inputs, [0, 1, 2, 3], [3, 2, 0, 1])
def to_batch_major(self, inputs):
"""Reorder [y, x, channels, batch]
to [batch, channels, y, x]
"""
return np.moveaxis(inputs, [0, 1, 2, 3], [2, 3, 1, 0])