def testVersorTransformation(self):
#Testing versor decomposition
theta, translation = self.getParam()
V = parameters_to_versor(theta, translation)
T, R = versor_decomposition(V)
assert (MVEqual(R, rotation_to_rotor(theta)))
assert (MVEqual(T, translation_to_rotor(translation)))
theta_ver, t_ver = versor_to_param(V)
assert (np.allclose(translation, t_ver))
assert (np.allclose(theta_ver, theta))
def testMeet(self):
A, B, C, D = createRandomPoints(N=4, scale=50)
L = createLine(A, B)
L2 = createLine(A, C)
P1 = createPlane(A, B, C)
P2 = createPlane(A, B, D)
L_actual = Meet(P1, P2)
assert (MVEqual(L, L_actual))
#Plane to line
Q = (ninf ^ A).normal()
P3 = A ^ C ^ D ^ ninf
Q_actual = Meet(P3, L).normal() #How do we define order/direction?
assert (MVEqual(Q, Q_actual))
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 testLineProjection(self):
A, B = createRandomPoints(2)
R = ga_exp(createRandomBivector())
L = createLine(A, B)
L_img = projectLineToPlane(L, R)
A_img = projectPointToPlane(A, R)
B_img = projectPointToPlane(B, R)
L_img_actual = createLine(A_img, B_img)
assert(MVEqual(L_img, L_img_actual))
def testProjectionVersorTransformation(self):
#Testing all together
theta, translation = self.getParam()
V = parameters_to_versor(theta, translation)
P = full_projection_matrix(theta, translation)
V_proj = projection_to_versor(P)
P_test = versors_to_projection(V)
assert (MVEqual(V, V_proj))
assert (np.allclose(P, P_test))
def testCloseConversion(self):
"""
Test the difference between a noisy conversion and a regular conversion
"""
verbose = True
if verbose:
print("\nTestCloseConversion")
#Variables that should not be touched are marked with an underscore
_a = e1 + e2 * 3.451
_ma = (e1 * 3.451 - e2).normal()
_b = _a + _ma
_c = 4.371 * e2 + e1
_mc = (5 * e3 + 4.371 * e1 - e2).normal()
_d = _c + _mc
assert (_c | _mc == 0)
assert (_a | _ma == 0)
A, B, C, D = up(_a), up(_b), up(_c), up(_d)
L_base = createLine(A, B)
L_expected = createLine(C, D)
sigma_R = 0.5
sigma_T = 0.5
R_T = noisyRotor(_a, _c, _ma, _mc, sigma_R, sigma_T)
assert (R_T * ~R_T == ~R_T * R_T == 1)
L_actual = (R_T * L_base * ~R_T)
rotor_diff = RotorLine2Line(L_actual, L_expected)
c_real, m_c_real = findLineParams(L_actual)
c_calc, m_c_calc = findLineParams(L_expected)
L_final = Sandwich(L_actual, rotor_diff)
if verbose:
print("diff c: ", c_calc - c_real)
print("diff m_c: ", m_c_calc - m_c_real)
print("L_base: ", L_base)
print("L_expected:", L_expected)
print("L_actual ", L_actual)
print("rotor_diff:", rotor_diff)
print("L_final: ", L_final)
assert (MVEqual(L_final, L_expected))
def testRotationExtraction(self):
np.random.seed(2)
O1 = up(0)
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + (3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = 30)
cPlane1 = (ninf + e3)*I5 #Camera plane 1
cPlane2 = R * cPlane1 * ~R
lines_img_d = [projectLineToPlane(line, R) for line in lines]
R_min, Nint = minimizeError((lines, lines_img_d), BivectorLineImageMapping, x0 = None)
assert(MVEqual(R_min, R, rtol = 1e-2, atol = 1e-2, verbose = False)) #Hard coded values.
assert(sumImageFunction(R, lines, lines_img_d) < sumImageFunction(R_min, lines, lines_img_d))
def AssertMVUnEqual(actual, expected, rtol=1e-5, atol=1e-6, verbose=False):
assert (not MVEqual(actual, expected, rtol, atol, verbose))