def E_step(S):
'''
:param S : the Cayley-Gibbs-Rodrigu representation of camera rotation parameters
:return: weights, pixel_assignments
'''
R = emhelp.vector2matrix(S) # Note that the 'S' is just for optimization, it has to be converted to R during computation
pixel_assignments = []
scores = []
# E-step: Assign each pixel to one of the VPs by finding argmax of log-posterior
for pixel in pixel_indices: # compute log likelihood over all pixels, to avoid underflow
theta_grad = Gdir_pixels[int(pixel[0])][int(pixel[1])]
ori_pixel = np.array([pixel[1]*5+4, pixel[0]*5+4, 1])
theta_norm = emhelp.vp2dir(K, R, ori_pixel)
err = theta_norm - theta_grad # [4,]
err = emhelp.remove_polarity(err)
prob = np.zeros(4)
# normal distribution
prob[:3] = scipy.stats.norm.pdf(err,mu,sig) # TODO or can write your own normal
prob[3] = 1/(2*np.pi) # the last prob is given by unifrom distribution
score = prob*P_m_prior
#print('pixel score', pixel, ' ', score)
scores.append(score) #appends the probability that a pixel u belongs to each of the 4 cases (vp models)
pixel_assignments.append(np.argmax(score, axis=0))
# normalize scores
scores = np.array(scores)
weights=scores/np.sum(scores,axis=1)[:, np.newaxis]
# weights=scores/np.sum(scores,axis=1)[:, np.newaxis]
return weights, pixel_assignments
def error_fun(S, K, pixels, Gdir_pixels, weights):
'''
:param S : the variable we are going to optimize over
:param w_pm : weights from E-step
:return: error : the error we are going to minimize
'''
error = 0.0 # initial error setting to zero
R = emhelp.vector2matrix(
S
) # Note that the 'S' is just for optimization, it has to be converted to R during computation
sum_of_weighted_errors = 0.0
for i in range(len(pixels)):
pixel = pixels[i]
theta_grad = Gdir_pixels[int(pixel[0])][int(pixel[1])]
pixel = np.array([pixel[1] * 5, pixel[0] * 5, 1])
theta_norm = emhelp.vp2dir(K, R, pixel)
err = theta_norm - theta_grad # [4,]
err = emhelp.remove_polarity(err)
prob = np.zeros(4)
# normal distribution
prob[:3] = scipy.stats.norm.pdf(err, mu, sig)
prob[3] = 1 / (2 * np.pi
) # the last prob is given by unifrom distribution
log_likelihood = np.log(prob)
sum_of_weighted_errors += weights[i].dot(log_likelihood)
return sum_of_weighted_errors
def calculate_pixel_evidence(a, b, g, pixel, theta_grad):
# return a list of evidence scores over all 4 models for a single pixel
R = emhelp.angle2matrix(a,b,g)
theta_norm = emhelp.vp2dir(K, R, pixel) # only calculate 3 evidence scores, the last one is give by uniform distribution
err = theta_norm - theta_grad # [4,]
err = emhelp.remove_polarity(err)
prob = np.zeros(4)
# normal distribution
prob[:3] = scipy.stats.norm.pdf(err,mu,sig)
prob[3] = 1/(2*np.pi) # the last prob is given by unifrom distribution
# TODO I normalized prob to make it sums to 1
prob = prob/prob.sum()
return prob
def error_fun(S, K, pixels, Gdir_pixels, weights):
'''
:param S : the variable we are going to optimize over
:param w_pm : weights from E-step
:return: error : the error we are going to minimize
'''
R = emhelp.vector2matrix(S) # Note that the 'S' is just for optimization, it has to be converted to R during computation
sum_of_weighted_errors = 0.0
for i in range(len(pixels)):
pixel = pixels[i]
theta_grad = Gdir_pixels[int(pixel[0])][int(pixel[1])]
ori_pixel = np.array([pixel[1]*5+4, pixel[0]*5+4, 1])
theta_norm = emhelp.vp2dir(K, R, ori_pixel)
err = theta_norm - theta_grad # [3,]
err = emhelp.remove_polarity(err)
sum_of_weighted_errors += weights[i,:3].dot(err**2)
return sum_of_weighted_errors
def downsample_image(img_path):
# Load a jpeg figure and convert it to grayscale
image = cv2.imread(img_path)
image_gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
sobelx = cv2.Sobel(image_gray,cv2.CV_64F,1,0)#,ksize=3)
sobely = cv2.Sobel(image_gray,cv2.CV_64F,0,1)#,ksize=3)
# Calculate gradient magnitude
gradmag = np.sqrt(sobelx**2+sobely**2)
# Calculate gradient direction
graddir = np.arctan2(sobely, sobelx)
# Downsample the grayscale image
Gdir, idx = emhelp.down_sample(gradmag, graddir)
return Gdir, idx
Created on Sat Nov 2 20:58:25 2019
@author: olive
"""
# visualization, done in local machine not on server
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import numpy as np
from utils import EM_help_fucntions as emhelp
camparam = 'cameraParameters.mat'
vp_dir = np.array(
[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [0.0, 0.0, 0.0]],
dtype=np.float32) # (4,3)
K = emhelp.cam_intrinsics(camparam)
res_file = 'outputs2'
npzfile = np.load(res_file + '.npz')
pixel_indices_ori = npzfile['arr_0']
pixel_assignments = npzfile['arr_1']
vp_trans = npzfile['arr_2']
vp_trans_init = npzfile['arr_3']
# Convert vp to non-homogeneous coordinates [x, y]
vp_trans = vp_trans.transpose()
vp_final = np.array([[p[0] / p[2], p[1] / p[2]] for p in vp_trans])
pt_0 = []
pt_1 = []
pt_2 = []
pt_3 = []
for i in range(len(pixel_assignments)):
imgpath = 'P1080055.jpg'
camparam = 'cameraParameters.mat'
# Load and downsample image to use only the 1999 (TODO should be 2000) edge pixels (which are of the highest gradient magnitudes)
#Gdir_pixels, pixel_indices_ori = downsample_image(args.imgpath)
Gdir_pixels, pixel_indices_ori = downsample_image(imgpath)
# Convert indices to homogeneous coordinates
sh = pixel_indices_ori.shape
pixel_indices = np.ones((sh[0],sh[1]+1))
pixel_indices[:,:-1] = pixel_indices_ori
# print('Gdir_pixels ', Gdir_pixels.shape) # (96, 128)
# print('pixel_indices ', pixel_indices.shape) # (1999, 3)
# Initialize K
#K = emhelp.cam_intrinsics(args.camparam)
K = emhelp.cam_intrinsics(camparam)
print('Initialized K: ', K.shape, ' ', K ) # (3, 3)
# Initialize R, by adopting [1] a coarse-to-fine search over combinations of a, b, and g that maximizes towards a MAP objective
# STEP 1: find optimal b around the y-axis, fixing a, g to 0
print('========== Start step 1 ==============')
#search_step =0
max_score = -np.infty
score = 0
a_c = 0
#a_optimal = 0
#g_optimal = 0
for a in tqdm(np.arange(-np.pi/3, np.pi/3, np.pi/45)):
for u in pixel_indices:
gdir_u = Gdir_pixels[int(u[0])][int(u[1])]
# Represent downsampled pixel arrays in original image coordinates