def testLineAveraging(self):
seed = 1
sigma_T = 0.005
sigma_R = 0.002
N = 100
mapping = BivectorLineMapping
line_start, line_target = createRandomLines(2)
R_real_min, N_int = minimizeError([(line_start, line_target)],
mapping=BivectorLineMapping)
R_real = RotorLine2Line(line_start, line_target)
print("R_real ", R_real)
print("R_real_min", R_real_min)
print("B_real_min", ga_log(R_real_min))
print("B_real ", ga_log(R_real))
print("L_real_min", R_real_min * line_start * ~R_real_min)
traingingdata = [
line_start,
[perturbeObject(line_target, sigma_R, sigma_T) for _ in range(N)]
]
validationdata = [
line_start,
[perturbeObject(line_target, sigma_R, sigma_T) for _ in range(N)]
]
print(
"Training and validation sets created with sig_r = %f and sig_t = %f, N = %d"
% (sigma_R, sigma_T, N))
map_list = [BivectorLineEstimationMapping]
for map_obj in map_list:
np.random.seed(seed)
print(map_obj.name)
realtrainingcost, minimumvalidationcost, R_min = benchmarkMinimizeError(
R_real,
traingingdata,
validationdata,
N=N,
fileout=None,
mapping=map_obj)
print("L_real = ", line_target)
print("L_min = ", R_min * line_start * ~R_min)
print("L_example= ", validationdata[0])
def testNoisyRotationExtendedExtraction(self):
verbose = True
np.random.seed(2)
O1 = up(0)
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + 1*(3*e1 -1*e2 + 2*e3)*ninf + 0.1 * E0
R = np.exp(B)
N = 100
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*3))
sigma_R_model = 0.001
sigma_T_model = 0.001
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.001
sigma_T_image = 0.001
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
#using our noisy model and the noisy image of them to estimate R
R_min, Nint = minimizeError((lines_perturbed, lines_img_d), ExtendedBivectorLineImageMapping , x0 = None)
if verbose:
print("R: ", R)
print("R_min", R_min)
assert(MVEqual(R_min, R, rtol = 1e-2, atol = 1e-2, verbose = False)) #Hard coded values.
#Weird condition. But we hope to find a "better" solution than the true one for the data we see, but worse than the true projection
assert(sumImageFunction(R, lines, lines_img_d) > sumImageFunction(R_min, lines, lines_img_d) > sumImageFunction(R, lines, lines_img_d_real))
def testSLAM(self):
np.random.seed(1)
N = 10
K = 4
sigma_R_image = 0.0001
sigma_T_image = 0.0001
R_list, lines, lines_img_base_d, lines_img_base_d_real , lines_imgs_d, lines_imgs_d_real = self.setUpMultiView(N, K,
sigma_R_image = sigma_R_image,
sigma_T_image = sigma_T_image)
sigma_R_model = 0.005
sigma_T_model = 0.003
lines_model = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
slam = SLAM(lines_model, lines_img_base_d, lines_imgs_d[:3], R_start = R_list[:3])
#print(R_list[0])
print("cost: ", slam.cost())
slam.updateLocation()
print(slam.R_estimate[0])
print("Current cost: ", slam.cost(), "\n")
for i in range(3):
print("R_min ", slam.R_estimate[i])
print("R_real ", R_list[i])
#print("model_estimate", slam.model_estimate[i])
#print("Real line ", lines[i])
slam.updateModel()
print("Current cost: ", slam.cost(), "\n")
for i in range(3):
print("model_estimate", slam.model_estimate[i])
print("Real line ", lines[i])
slam.addImage(lines_imgs_d[3])
slam.updateLocation()
print("Current cost: ", slam.cost(), "\n")
for i in range(4):
print("R_min ", slam.R_estimate[i])
print("R_real ", R_list[i])
slam.updateModel()
print("Current cost: ", slam.cost(), "\n")
for i in range(4):
print("model_estimate", slam.model_estimate[i])
print("Real line ", lines[i])
def createNoisyLineSet(R_real, sigma_R, sigma_T, N=1, scale=1):
"""
Creates a set of 2-tuples with the base line and the converted and disturbed line.
"""
lineSet = []
lines = createRandomLines(N, scale=scale)
for line in lines:
line = line
newline = R_real * line * ~R_real
lineSet.append((line, perturbeObject(newline, sigma_T, sigma_R)))
return lineSet
def createNoisyPlaneSet(R_real, sigma_R, sigma_T, N=1, scale=1):
"""
Creates a set of 2-tuples with the base line and the converted and disturbed line.
"""
planeSet = []
planes = createRandomPlanes(N, scale=scale)
for plane in planes:
newplane = R_real * plane * ~R_real
planeSet.append((plane, perturbeObject(newplane, sigma_T, sigma_R)))
return planeSet
def testExtremeRotationExtraction(self):
verbose = True
np.random.seed(2)
O1 = up(0)
rot_scale = 10
tran_scale = 10
spread_scale = 10
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + rot_scale*(3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = spread_scale)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*tran_scale))
sigma_R_model = 0.001
sigma_T_model = 0.001
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.001
sigma_T_image = 0.001
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
mapping = BivectorLineImageMapping
x0 = mapping.inverserotorconversion(R)
x0[:3] += np.array([0.1, 0.9, -0.17])
R_start = mapping.rotorconversion(x0)
#using our noisy model and the noisy image of them to estimate R
R_min, Nint = minimizeError((lines_perturbed, lines_img_d), mapping, x0 = x0)
if verbose:
print("R: ", R)
print("R_min ", R_min/np.sign(R_min[0]))
print("R_start", R_start/np.sign(R_start[0]))
print("")
print("B: ", B)
print("B_min - B ", ga_log(R_min/np.sign(R_min[0])) - B)
print("B_start - B ", ga_log(R_start/np.sign(R_start[0])) - B)
def benchmarkImageCostFunction():
np.random.seed(123)
B = createRandomBivector()
R_real = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*3))
sigma_R_model = 0.01
sigma_T_model = 0.01
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R_real) for line in lines] #Real lines
sigma_R_image = 0.01
sigma_T_image = 0.01
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R_real), sigma_R_image, sigma_T_image) for line in lines]
traininglinesets = (lines_perturbed, lines_img_d)
benchmarkMinimizeError(R_real, traininglinesets, traininglinesets, fileout = None, mapping = BivectorLineImageMapping)
def plotCostFunctionEffect():
print("\nRunning plotCostFunctionEffect")
print("")
np.random.seed(1)
#Test extreme values
np.random.seed(2)
O1 = up(0)
rot_scale = 1
tran_scale = 10
spread_scale = 10
B = 0.1 * e12 + 0.2*e13 - 0.1 *e23 + rot_scale*(3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = spread_scale)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*tran_scale))
sigma_R_model = 0.01
sigma_T_model = 0.1
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.002
sigma_T_image = 0.01
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
mapping = BivectorLineImageMapping
x0 = mapping.inverserotorconversion(R)
x_test = x0[0]
y_test = x0[3]
N_rot = 50
N_tran = 50
rot_range = 0.4
translation_range = 10
rotation = np.linspace(-rot_range, rot_range, N_rot)
translation = np.linspace(-translation_range, translation_range, N_tran)
ans = np.zeros((N_rot, N_tran))
for i, rot in enumerate(rotation):
for j, tran in enumerate(translation):
x0[0] = x_test + tran
x0[3] = y_test + rot
ans[i, j] = np.log(mapping.costfunction(mapping.rotorconversion(x0), lines_perturbed, lines_img_d, O1))
xv, yv = np.meshgrid(translation, rotation)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel("Translation error")
ax.set_ylabel("Rotation error")
ax.set_zlabel("log(objective function)")
ax.plot_wireframe(xv, yv, ans)
plt.show()
def testPlotProjections(self):
np.random.seed(2)
#A, B = createRandomPoints(2, 100) #Real points
#L = createLine(A, B) #Real line
O1 = up(0)
F1 = up(e3) #Image origin
Q1 = up(e3 + e2) #Defines image rotation
#O1 = up(3*e1 + 4*e2)
#cPlane1 = createRandomPlane(2)
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + 1*(3*e1 -1*e2 + 2*e3)*ninf
#x0 = np.array([0.54, 0.85, 0.29, 1*3.1, -1.4 * 1, 1*1.89]) #Close to the real answer
N = 10
R = ga_exp(B)
O2 = R * O1 * ~R #O2
F2 = R * F1 * ~R
Q2 = R * Q1 * ~R
cPlane1 = (ninf + e3)*I5 #Camera plane 1
cPlane2 = R * cPlane1 * ~R
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*3))
sigma_R_model = 0.01
sigma_T_model = 0.05
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.0001
sigma_T_image = 0.0001
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
print("")
print("Inital cost", sumImageFunction(R, lines_perturbed, lines_img_d))
print("R_real: ", R)
R_min, Nint = minimizeError((lines_perturbed, lines_img_d), BivectorLineImageMapping, x0 = None)
print("R_min: ", R_min)
print("Nint = ", Nint)
print("Final cost= ", sumImageFunction(R_min, lines, lines_img_d))
lines_img_d_min = [projectLineToPlane(line, R_min) for line in lines]
lines_img_d_model = [projectLineToPlane(line, R_min) for line in lines_perturbed]
#Printing
color_print = ['m', 'y', 'k']
N_print = len(color_print)
plot_img = Plot2D()
for i in range(N_print):
Limg = lines_img_d[i]
Limg_min = lines_img_d_min[i]
Limg_real = lines_img_d_real[i]
Limg_model = lines_img_d_model[i]
plot_img.plotLine2D(Limg_min, color = 'g') #Green: estimate of the real line (hidden)
plot_img.plotLine2D(Limg_model, color = 'c') #Cyan: estimate of model line
plot_img.plotLine2D(Limg, color = 'b') #Blue: image (with image noise)
plot_img.plotLine2D(Limg_real, color = color_print[i]) #Other: real line (hidden)
plot = Plot3D()
plot.configure(5)
plot.addPoint(O1, color='r')
plot.addPoint(O2, color='b')
#plot.addPoint(F1, color='r')
plot.addPoint(F2, color='b')
#plot.addPoint(Q1, color='r')
plot.addPoint(Q2, color='b')
for i in range(N_print):
L = lines[i]
L_img = R*lines_img_d_real[i]*~R
L_perturbed = lines_perturbed[i]
plot.addLine(L_perturbed, color = 'c')
plot.addLine(L_img, color = color_print[i])
plot.addLine(L, color = color_print[i])
#plot.addPlane(cPlane1, center = F1, color='r')
plot.addPlane(cPlane2, center = F2, color='b')
plot_img.show(block = False)
plot.show(block = False)