def test_motor_rotate_point():
motor = create_motor()
# print(np.array(motor))
motor = vsr.Vec(4,1,2).trs() * vsr.Rot(vsr.Biv(1,1,1).unit() * (np.pi/6))
point = vsr.Vec(1,2,3).null()
print(np.array(point.spin(motor)))
# print(motor.matrix()[:3,:3])
print(motor_rotate_point(np.array(motor).copy(), np.array(point).copy()))
def create_points(motor, gaussian=False, radius=10, n_points=10, points_std=0.8, noise_std=0.09):
points = []
for i in range(n_points):
if gaussian:
a = vsr.Vec(*np.random.normal(0.0, points_std, 3)).null()
else:
a = (vsr.Vec(*np.random.uniform(-1, 1, 3)).unit()
* np.random.uniform(0, radius)).null()
b = a.spin(motor)
t = vsr.Vec(*np.random.random(3)).unit() * \
np.random.normal(0.0, noise_std, 1)
noise_motor = t.trs() * vsr.Rot(1, 0, 0, 0)
bn = a.spin(noise_motor).spin(motor)
points.append((a, b, bn))
return points
def create_motor(d_lims=(0, 1), th_lims=(0, np.pi/2)):
translator = (vsr.Vec(*np.random.random(3)).unit()
* np.random.uniform(*d_lims)).trs()
rotator = vsr.Rot(vsr.Biv(*np.random.uniform(-1, 1, 3)).unit()
* np.random.uniform(*th_lims) * -0.5)
motor = translator * rotator
return motor
def hand_eye_pose_pairs(M_object_in_world, M_eye_in_hand, n):
pose_pairs = np.array([(M_hand_in_world,
(M_eye_in_hand.rev() * M_hand_in_world.rev() * M_object_in_world))
for M_hand_in_world in [vsr.Trs.from_vector(vsr.Vec(*np.random.rand(3))) *
vsr.Rot.from_bivector(vsr.Biv(*np.random.rand(3)).unit() *
np.random.rand() * np.pi)
for i in range(n)]])
n = len(pose_pairs)
As = pose_pairs[:,0]
Bs = pose_pairs[:,1]
LAs = []
LBs = []
for i in range(n):
for j in range(i+1,n):
LAs.append(((As[j].rev() * As[i]).log() * 0.5).unit())
LBs.append(((Bs[j] * Bs[i].rev()).log() * 0.5).unit())
return LAs, LBs
def estimate_motor(cost_function_num, parameterization_num, num_elements, points_a=None, points_b_noisy=None):
motor = vsr.Trs.from_vector(vsr.Vec(1,1,1)) * vsr.Rot.from_bivector(vsr.Biv(0,1,0) * np.pi/6.0)
return LAs, LBs
# motor = vsr.Mot.from_dir_ang_trs(vsr.Vec(0,1,0).unit(), np.pi/3, vsr.Vec(1,0,1))
def estimate_motor(cost_function_num, parameterization_num, num_elements, points_a=None, points_b_noisy=None):
motor = vsr.Trs.from_vector(vsr.Vec(1,1,1)) * vsr.Rot.from_bivector(vsr.Biv(0,1,0) * np.pi/6.0)
motor = M_eye_in_hand
error_motor = vsr.Mot.from_dir_ang_trs([1,0,0], np.pi/12, [0,0,0])
n = num_elements
# Generate initial point sets
if points_a is None and points_b_noisy is None:
points_a = [vsr.Vec(*np.random.normal(0.0, 0.8, 3)).null() for i in range(n)]
points_b = [point.spin(motor) for point in points_a]
mu = 0.0
sigma = 0.00
add_noise_to_point = lambda p : (point.to_array()[:3] + sigma * np.random.randn(1,3) + mu)[0]
points_b_noisy = [vsr.Vec(*add_noise_to_point(point)).null()
for point in points_b ]
lines_a = [(vsr.Vec(*np.random.normal(0.0, 0.8, 3)).null() ^
(vsr.Vec(*np.random.normal(0.0, 0.8, 3)).unit() ^ vsr.ni)).dual()
for i in range(n)]
lines_b = [line.spin(motor) for line in lines_a]
# initial_mot = motor.spin(error_motor)
# initial_mot = vsr.Mot(1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0)