<jupyter_start><jupyter_text>Laser Distance Sensor Calibration $$ R = \frac{1 + \mathrm{b}\mathrm{a}}{\sqrt{2(1 + \mathrm{b} \rfloor \mathrm{a})}} $$<jupyter_code>rotor_from_vectors = lambda a, b: (b * a + 1) / np.sqrt(2*(1+(b <= a)[0]))<jupyter_output><empty_output><jupyter_text>Create measurement poses<jupyter_code># laser direction in end-effector
l0 = vsr.Vec(1,0,0).spin( vsr.Rot.from_bivector(vsr.Biv(1,0,0) * pi/ 3))
p0 = vsr.Vec(0.1,0.2,0.3)
R0 = rotor_from_vectors(vsr.Vec(1,0,0), l0)
T0 = vsr.Trs.from_vector(p0)
M_laser_in_ee = T0 * R0
print(l0)
print(p0)
print(M_laser_in_ee)
# Laser sensor
lower = 0.065
upper = 0.095
# lower = 0.01
# upper = lower + 0.2
generate_measurement_distance = lambda : (upper - lower) * np.random.rand() + lower
def create_point_on_sphere():
    r = generate_measurement_distance()
    theta = np.random.rand() * (np.pi / 2.0) * 0.9 + (np.pi / 2.0) * 0.1
    phi = np.random.rand() * np.pi * 2.0 
    return vsr.Vec(r*np.sin(theta)*np.cos(phi),
                   r*np.sin(theta)*np.sin(phi),
                   r*np.cos(theta)), r<jupyter_output><empty_output><jupyter_text>Create point on upper hemisphere on measurement sphere:$$ x = r \sin (\theta) \cos(\varphi),\y = r \sin (\theta) \cos(\varphi),\ z = r \cos(\varphi) $$<jupyter_code>def generate_measurement_pose_and_plane_and_distance(M_laser_in_ee, M_plane):
    # Model laser as tangent vector in the e_1-direction at the origin
    lds_dir = vsr.Vec(1,0,0)
    lds_tnv = vsr.no ^ lds_dir
    # Model the measurement plane as plane through the orgin with
    # normal vector \hat{n} = e_3
    dlp = dlp = vsr.Dlp(0,0,1,0)
    # Create point on upper hemisphere of measurement sphere with 
    # radius equals the measurement distance
    p, radius = create_point_on_sphere()
    # Compute motor that aligns lds_dir with measurement vector from 
    # from point on circle to the origin
    R = rotor_from_vectors(lds_dir, -p.unit())
    T = vsr.Trs.from_vector(p)
    M_on_sphere = T * R * vsr.Rot.from_bivector(vsr.Vec(1,0,0).duale() * 
                                                np.random.rand() * 2 * pi)
    # Transform the everything the measurement plane
    x_in_plane = np.random.rand()
    y_in_plane = np.random.rand()
    T_in_plane = vsr.Trs.from_vector(vsr.Vec(x_in_plane, y_in_plane, 0.0)) * vsr.Rot(1.0,0.0,0.0,0.0)
    # Transform the plane
    dlp_meas = dlp.spin(M_plane)
    M2 = M_plane * T_in_plane * M_on_sphere * M_laser_in_ee.rev()   
    return M2, dlp_meas, radius
# M, dlp, r = generate_measurement_pose_and_plane_and_distance(M_laser_in_ee)
# t2 = transform_tangent_vector(M * M_laser_in_ee,  (vsr.no ^ vsr.Vec(1,0,0)))
# s = (dlp <= t2) / (vsr.ni <= (dlp <= t2))[0]
# np.allclose(r, np.sqrt((s <= s)[0]))
lin = (p0.null() ^ (l0 ^ vsr.ni))
print (p0.null() ^ vsr.ni)
M_plane = vsr.Mot.from_dir_ang_trs(vsr.Vec(0,0,1), 0, vsr.Vec(1,1,0))
measurements1 = [generate_measurement_pose_and_plane_and_distance(M_laser_in_ee, M_plane) for i in range(10)]
M_plane2 = vsr.Mot.from_dir_ang_trs(vsr.Vec(0,1,0), np.pi/3, vsr.Vec(1,1,0.0))
measurements2 = [generate_measurement_pose_and_plane_and_distance(M_laser_in_ee, M_plane2) for i in range(10)]
M_plane3 = vsr.Mot.from_dir_ang_trs(vsr.Vec(1,0,0), np.pi/2, vsr.Vec(1,1,0.0))
measurements3 = [generate_measurement_pose_and_plane_and_distance(M_laser_in_ee, M_plane3) for i in range(10)]
measurements = measurements1 + measurements2 + measurements3
measurements_copy = np.array(measurements).copy()
print M_plane
options = solver_options()
options['function_tolerance'] = 1e-12
options['parameter_tolerance'] = 1e-12
options['max_num_iterations'] = 100
options['num_linear_solver_threads'] = 100
options['num_threads'] = 100
# options['trust_region_strategy_type'] = 'DOGLEG'
# options['linear_solver_type'] = 'SPARSE_NORMAL_CHOLESKY'
M_plane = vsr.Mot.from_dir_ang_trs(vsr.Vec(0,0,1), 0, vsr.Vec(1,1,0))
measurements1 = [generate_measurement_pose_and_plane_and_distance(M_laser_in_ee, M_plane) for i in range(5)]
M_plane2 = vsr.Mot.from_dir_ang_trs(vsr.Vec(0,1,0), -np.pi/3, vsr.Vec(1,1,0.0))
measurements2 = [generate_measurement_pose_and_plane_and_distance(M_laser_in_ee, M_plane2) for i in range(0)]
M_plane3 = vsr.Mot.from_dir_ang_trs(vsr.Vec(1,0,0), -np.pi/6, vsr.Vec(1,1,0.0))
measurements3 = [generate_measurement_pose_and_plane_and_distance(M_laser_in_ee, M_plane3) for i in range(0)]
measurements = measurements1 + measurements2 + measurements3
measurements_copy = np.array(measurements).copy()

# laser direction in end-effector
l = vsr.Vec(1,0,0).unit().spin(vsr.Rot.from_bivector(vsr.Biv(1,0,0).unit() * np.pi/5.1))
p = vsr.Vec(0.1 + 0.02, 0.2 + 0.02, 0.3 + 0.02)
R = rotor_from_vectors(vsr.Vec(1,0,0), l)
T = vsr.Trs.from_vector(p)
M_initial = T * R

print(M_initial)

print(M_laser_in_ee)

lds_calibrator = LaserDistanceSensorCalibrator(M_initial, options)

for measurement in measurements:
    M, dlp_meas, r = measurement
    r += np.random.randn() * 0.01
    lds_calibrator.add_tangent_vector_residual_block(M, dlp_meas, r)

# lds_calibrator.set_parameterization('POLAR_DECOMPOSITION')

lds_calibrator.set_parameterization('BIVECTOR_GENERATOR')

motor, summary = lds_calibrator.solve()

print(motor)
print(summary['brief_report'])

iterations = summary['iterations']
plot([it['gradient_max_norm'] for it in iterations])
print(summary['full_report'])
errors = []
for measurement in measurements:
    dlp = measurement[1]
    t = transform_tangent_vector(measurement[0] * motor,  (vsr.no ^ vsr.Vec(1,0,0)))
    s = (dlp <= t) / (vsr.ni <= (dlp <= t))[0]
    r_est = np.sqrt((s <= s)[0])
    errors.append(np.abs(r_est - measurement[2]))
print(np.sum(errors) / 1000)
print(vsr.Vec(0,0,0).null().spin(motor).to_array())
np.allclose(vsr.Vec(0,0,0).null().spin(motor).to_array(), 
            vsr.Vec(0,0,0).null().spin(M_laser_in_ee).to_array())
iterations = summary['iterations']
plot([it['gradient_max_norm'] for it in iterations])<jupyter_output><empty_output><jupyter_text>Point direction<jupyter_code>p_initial = vsr.Vec(0,0,0).null().spin(M_initial)
p_initial = vsr.Vec(*p_initial.to_array()[:3])
l_initial = (vsr.Vec(1,0,0) ^ vsr.ni).spin(M_initial)
l_initial = vsr.Vec(*l_initial.to_array()[:3])
p_initial2 = vsr.Vec(*p_initial.to_array().copy()[:3])
l_initial2 = vsr.Vec(*l_initial.to_array().copy()[:3])
print(p0)
print(p_initial)
print(l0)
print(l_initial)
lds_calibrator = LaserDistanceSensorCalibrator(p_initial, l_initial, options)
for measurement in measurements:
    M, dlp_meas, r = measurement
    r += np.random.randn() * 0.0001
    lds_calibrator.add_point_direction_residual_block(M, dlp_meas, r)
lds_calibrator.set_direction_parameterization('NORMALIZE')
p, l, summary2 = lds_calibrator.solve_point_direction()
print(summary2['brief_report'])
print(p)
print(l.unit())
print(summary2['full_report'])
iterations = summary2['iterations']
plot([it['gradient_max_norm'] for it in iterations])
print p
p = p0.null().spin(measurements[0][0])
p = vsr.Vec(p[0], p[1], p[2])
l = (l0 ^ vsr.ni).spin(measurements[0][0])
l = vsr.Vec(l[0], l[1], l[2])
n = vsr.Vec(dlp[0], dlp[1], dlp[2])
num = (vsr.Vec(dlp[0]*dlp[3], dlp[1]*dlp[3], dlp[2]*dlp[3]) - p <= n)
den = (l <= n)
dist = num / den
print(dist)
p2 = p_initial2.null().spin(measurements[0][0])
p2 = vsr.Vec(p2[0], p2[1], p2[2])
l2 = (l_initial2 ^ vsr.ni).spin(measurements[0][0])
l2 = vsr.Vec(l2[0], l2[1], l2[2])
n = vsr.Vec(dlp[0], dlp[1], dlp[2])
num2 = (vsr.Vec(dlp[0]*dlp[3], dlp[1]*dlp[3], dlp[2]*dlp[3]) - p2 <= n)
den2 = (l <= n)
dist2 = num2 / den2
print(dist2)
measurements[0][2]
print (dist2 - dist) * 100
p_initial2.to_array()

# s = (dlp <= t2) / (vsr.ni <= (dlp <= t2))[0]
# np.allclose(r, np.sqrt((s <= s)[0]))
print(p)
0.001 * np.random.randn()
# for m in measurements:
#     print m[2]<jupyter_output><empty_output><jupyter_text>Point + angles<jupyter_code>lds_calibrator = LaserDistanceSensorCalibrator(p_initial, np.pi/3, 0.0, options)
for measurement in measurements:
    lds_calibrator.add_point_angles_residual_block(*measurement)
p, theta, phi, summary3 = lds_calibrator.solve_point_angles()
print(summary3['brief_report'])
print(p)
print(theta)
print(phi)
np.pi/3<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.16723800195886385, 'fraction_numerical': 0.04113614103819784, 'mean_word_length': 5.174603174603175, 'pattern_counts': {'":': 0, '<': 38, '<?xml version=': 0, '>': 21, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '5', 'dir': 'jupyter-structured-clean-dedup', 'id': '571988', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 3564, 'n_tokens_neox': 3442, 'n_words': 579, 'path': 'python/lds_calibration.ipynb', 'repo_name': 'tingelst/game', 'size': '8168'}