<jupyter_start><jupyter_text>GACSE Workshop<jupyter_code>from __future__ import print_function
import sys
sys.path.append('../build/')
%pylab inline
np.set_printoptions(precision=4, suppress=True)

import versor as vsr
from motor_estimation import MotorEstimationSolver
from motor_estimation_valkenburg_dorst import (point_matrix, dual_line_matrix, dual_plane_matrix)
# fig_width_pt =  332.89726               # Get this from LaTeX using \showthe\columnwidth
# inches_per_pt = 1.0/72.27               # Convert pt to inches
# golden_mean = (sqrt(5.0)-1.0)/2.0       # Aesthetic ratio
# fig_width = fig_width_pt*inches_per_pt  # width in inches
# fig_height = fig_width*golden_mean      # height in inches
# fig_size = [fig_width,fig_height]
# params = {
#     'axes.labelsize': 9,
#     'font.size': 9,
#     "font.family": "serif",
#     "font.serif": [],
#     'legend.fontsize': 9,
#     'xtick.labelsize': 8,
#     'ytick.labelsize': 8,
#     'text.usetex': True,
#     'text.latex.preamble':[r'\usepackage{amsmath}', r'\usepackage{bm}'],
#     'figure.figsize': fig_size,
#     'axes.linewidth': 0.5
# }
# matplotlib.rcParams.update(params)<jupyter_output><empty_output><jupyter_text>Valkenburg-Dorst Solver<jupyter_code>class VDMotorEstimationSolver(object):
    def __init__(self):
        self._point_matrix = np.zeros((8,8))
        self._dual_line_matrix = np.zeros((8,8))
        self._dual_plane_matrix = np.zeros((8,8))
        
    @property
    def L(self):
        return self._point_matrix + self._dual_line_matrix + self._dual_plane_matrix
    
    def add_point_observations(self, ps, qs):
        for p, q in zip(ps,qs):
            self._point_matrix += point_matrix(p,q)
    
    def add_dual_line_observations(self,ps,qs):
        for p, q in zip(ps,qs):
            self._dual_line_matrix += dual_line_matrix(p,q)
    
    def add_dual_plane_observations(self, ps,qs):
        for p, q in zip(ps,qs):
            self._dual_plane_matrix += dual_plane_matrix(p,q)
        
    def solve(self):
        L = self.L
        Lrr = L[:4,:4]
        Lrq = L[:4,4:]
        Lqr = L[4:,:4]
        Lqq = L[4:,4:]
        Lp = Lrr - np.dot(Lrq, np.dot(np.linalg.pinv(Lqq), Lqr))
        w,v = np.linalg.eig(Lp)
        r = v[:,np.argmax(w)]
        q = np.dot(-np.dot(np.linalg.pinv(Lqq), Lqr),r)
        return vsr.Mot(*np.array([r,q]).ravel())<jupyter_output><empty_output><jupyter_text>Ground truth motor<jupyter_code>motor = vsr.Vec(1,1,1).trs() * vsr.Rot(vsr.Biv(0,1,0) * np.pi/6.0)
print(motor)
np.arccos(np.array(vsr.Mot(1,0,0,0,0,0,0,0) * motor)[0]) - pi/6<jupyter_output><empty_output><jupyter_text>Motor Estimation from Points $$\min_{M \in \mathcal M} {1 \over 2} \sum_{i=1}^N \| \mathrm{P}_{\mathbb R_3^1}(M a_i \widetilde M) - \mathrm{P}_{\mathbb R_3^1}(b_i )  \|^2$$  Dataset generation<jupyter_code>n_points = 10
sigma = 0.09
points_a = [vsr.Vec(*np.random.normal(0.0, 0.8, 3)).null() 
            for i in range(n_points)]
points_b = [point.spin(motor) for point in points_a]
points_b_noisy = [vsr.Vec(*(np.array(point)[:3] 
                            + sigma * np.random.randn(3))).null() 
                  for point in points_b]<jupyter_output><empty_output><jupyter_text>Estimate motor using local parameterization<jupyter_code># initial_motor = vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
initial_motor = vsr.Vec(2,3,4).trs() * vsr.Rot(vsr.Biv(0,-1,0) * np.pi/6)
mes = MotorEstimationSolver(initial_motor)
for a, b in zip(points_a, points_b):
    mes.add_point_correspondences_residual_block(a,b)
mes.set_parameterization('BIVECTOR_GENERATOR')
mes.linear_solver_type = 'DENSE_QR'
(estimated_motor, summary_point_1, motors) = mes.solve()
print(summary_point_1['full_report'])
plt.plot([it['cost'] for it in summary_point_1['iterations'] ])<jupyter_output><empty_output><jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(estimated_motor))
print(np.allclose(np.array(motor), np.array(estimated_motor)))<jupyter_output>[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
True<jupyter_text>Estimate motor using Valkenburg-Dorst solver<jupyter_code>vd = VDMotorEstimationSolver()
vd.add_point_observations(points_a, points_b)
vd_estimated_motor = vd.solve()<jupyter_output><empty_output><jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(vd_estimated_motor))
print(np.allclose(np.array(motor), np.array(vd_estimated_motor)))<jupyter_output>[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
[-0.866  0.     0.5    0.     0.683  0.433  0.183  0.25 ]
False<jupyter_text>Motor Estimation from Lines  Dataset generation<jupyter_code>lines_a = [vsr.Dll(vsr.Vec(*np.random.normal(0.0,0.8,3)),
                   vsr.Vec(*np.random.normal(0.0,0.8,3)))
          for i in range(10)]
lines_b = [line.spin(motor) for line in lines_a]<jupyter_output><empty_output><jupyter_text>Estimate motor using 6 residuals $$ \min_{M \in \mathcal M} \frac{1}{2} \sum_{i=1}^n \| M \varLambda_{b_i} \tilde{M} - \varLambda_{a_i} \|^2 $$<jupyter_code>initial_motor = vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
motor = vsr.Vec(1,1,1).trs() * vsr.Rot(vsr.Biv(0,1,0) * np.pi/6.0)
print(motor)
mes = MotorEstimationSolver(initial_motor)
for a, b in zip(lines_a, lines_b):
    mes.add_line_correspondences_residual_block(a,b)
mes.set_parameterization('BIVECTOR_GENERATOR')
mes.linear_solver_type = "DENSE_QR"
# mes.gradient_tolerance = 1e-16
mes.parameter_tolerance = 1e-6
# mes.function_tolerance = 1e-3
estimated_motor, lin_summary_1, motors = mes.solve()
print(lin_summary_1['full_report'])<jupyter_output>Mot: [ 0.87 0 -0.5 0 -0.68 -0.43 -0.18 -0.25 ]

Solver Summary (v 1.11.0-eigen-(3.2.92)-lapack-suitesparse-(4.4.6)-openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  8                        8
Effective parameters                        6                        6
Residual blocks                            10                       10
Residual                                   60                       60

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                        DENSE_QR                 DENSE_QR
Threads                                     1                        1
Linear solver threads                       1                        1

Cost:
Initial           [...]<jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(estimated_motor))
print(np.allclose(np.array(motor), np.array(estimated_motor)))<jupyter_output>[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
[ 0.866 -0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
True<jupyter_text>Estimate motor using 4 residuals<jupyter_code>initial_motor = vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
mes = MotorEstimationSolver(initial_motor)
for a, b in zip(lines_a, lines_b):
    mes.add_line_angle_distance_residual_block(a,b)
mes.set_parameterization('BIVECTOR_GENERATOR')
mes.linear_solver_type = "DENSE_QR"
estimated_motor, lin_summary_2, motors = mes.solve()
print(lin_summary_2['full_report'])<jupyter_output>Solver Summary (v 1.11.0-eigen-(3.2.92)-lapack-suitesparse-(4.4.6)-openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  8                        8
Effective parameters                        6                        6
Residual blocks                            10                       10
Residual                                   40                       40

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                        DENSE_QR                 DENSE_QR
Threads                                     1                        1
Linear solver threads                       1                        1

Cost:
Initial                          4.739404e+00
Final              [...]<jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(estimated_motor))
print(np.allclose(np.array(motor), np.array(estimated_motor)))
print(np.allclose(np.array(motor), np.array(estimated_motor), 
                  rtol=1e-05, atol=1e-03))<jupyter_output>[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
[ 0.8746  0.001  -0.4848  0.0044 -0.6822 -0.4338 -0.2186 -0.2441]
False
False<jupyter_text>Estimate motor using 2 residuals<jupyter_code>initial_motor = vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
mes = MotorEstimationSolver(initial_motor)
for a, b in zip(lines_a, lines_b):
    mes.add_line_angle_distance_norm_residual_block(a,b)
mes.set_parameterization('BIVECTOR_GENERATOR')
mes.linear_solver_type = "DENSE_QR"
estimated_motor, lin_summary_3 = mes.solve()
print(lin_summary_3['full_report'])<jupyter_output>Solver Summary (v 1.12.0-eigen-(3.2.92)-lapack-suitesparse-(4.4.6)-openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  8                        8
Effective parameters                        6                        6
Residual blocks                            10                       10
Residual                                   20                       20

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                        DENSE_QR                 DENSE_QR
Threads                                     1                        1
Linear solver threads                       1                        1

Cost:
Initial                          1.493945e+01
Final              [...]<jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(estimated_motor))
print(np.allclose(np.array(motor), np.array(estimated_motor)))
print(np.allclose(np.array(motor), np.array(estimated_motor), 
                  rtol=1e-05, atol=1e-03))<jupyter_output>[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
[ 0.8659 -0.0003 -0.5002 -0.0002 -0.6829 -0.4329 -0.183  -0.2499]
False
True<jupyter_text>Estimate motor using Valkenburg-Dorst solver<jupyter_code>vd = VDMotorEstimationSolver()
vd.add_dual_line_observations(lines_a, lines_b)
vd_estimated_motor = vd.solve()
print(vd_estimated_motor)<jupyter_output>Mot: [ -0.87 -3e-17 0.5 4.3e-18 0 0 0 0 ]<jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(vd_estimated_motor))
print(np.allclose(motor, vd_estimated_motor))<jupyter_output>[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
[-0.866 -0.     0.5    0.     0.     0.     0.     0.   ]
False<jupyter_text>Plots<jupyter_code>fig, ax = plt.subplots(2, sharex=True)

ax[0].set_aspect('equal')
plt.set_cmap('gray')

almost_black = '#262626'
spines_to_remove = ['top', 'right']
for spine in spines_to_remove:
    ax[0].spines[spine].set_visible(False)
    ax[1].spines[spine].set_visible(False)
    
ax[0].xaxis.set_ticks_position('none')
ax[0].yaxis.set_ticks_position('none')
ax[1].xaxis.set_ticks_position('none')
ax[1].yaxis.set_ticks_position('none')

spines_to_keep = ['bottom', 'left']
for spine in spines_to_keep:
    ax[0].spines[spine].set_linewidth(0.5)
    ax[0].spines[spine].set_color(almost_black)
    ax[1].spines[spine].set_linewidth(0.5)
    ax[1].spines[spine].set_color(almost_black)
ax[0].xaxis.label.set_color(almost_black)
ax[0].yaxis.label.set_color(almost_black)
ax[1].xaxis.label.set_color(almost_black)
ax[1].yaxis.label.set_color(almost_black)

# ax.plot([it['cost'] for it in lin_summary_4['iterations'] ], label=r"commutator")
# ax[0].plot([it['cost'] for it in lin_summary_3['iterations'] ], label=r"$2$-residuals", c='red')
ax[0].plot([it['cost'] for it in lin_summary_2['iterations'] ], label=r"$4$-residuals")
ax[1].plot([it['cost'] for it in lin_summary_1['iterations'] ], label=r"$6$-residuals")

ax[0].set_yscale('log')
ax[1].set_yscale('log')

ax[0].grid(which="both")
ax[1].grid(which="both")

# ax[0].set_xscale('log')

# plt.yscale('log')

# ax[0].title.set_color(almost_black)
# ax[0].axhline(0, color=almost_black, linewidth=0.15)
# ax[0].axvline(0, color=almost_black, linewidth=0.15)
# ax[1].title.set_color(almost_black)
# ax[1].axhline(0, color=almost_black, linewidth=0.15)
# ax[1].axvline(0, color=almost_black, linewidth=0.15)

# Remove the line around the legend box, and instead fill it with a light grey
# Also only use one point for the scatterplot legend because the user will 
# get the idea after just one, they don't need three.
light_grey = np.array([float(248)/float(255)]*3)
legend = ax[0].legend(frameon=True, scatterpoints=1)
rect = legend.get_frame()
rect.set_facecolor(light_grey)
rect.set_linewidth(0.0)
legend = ax[1].legend(frameon=True, scatterpoints=1)
rect = legend.get_frame()
rect.set_facecolor(light_grey)
rect.set_linewidth(0.0)

fig.text(0.06, 0.5, 'Cost', ha='center', va='center', rotation='vertical')

# plt.tight_layout()
plt.xlabel(r'Iteration $k$')
# fig.ylabel(r'Cost')
# ax[1].ylabel(r'Cost')

# plt.savefig('/home/lars/Dropbox/Apps/ShareLaTeX/2016-gacse/figures/cost_lines2.pdf')<jupyter_output><empty_output><jupyter_text>Motor Estimation from planes  Dataset generation<jupyter_code>n_planes = 10
planes_a = []
for i in range(10):
    dir_vec = np.random.normal(0.0, 0.8, 4)
    dir_vec = dir_vec / np.linalg.norm(dir_vec)
    distance = np.random.normal(0.0, 0.8,1)
    arr = dir_vec + distance
    planes_a.append(vsr.Dlp(dir_vec[0],dir_vec[1],dir_vec[2],distance[0]))
planes_b = [plane.spin(motor) for plane in planes_a]<jupyter_output><empty_output><jupyter_text>Estimate motor using 2 residuals<jupyter_code>motor = vsr.Vec(1,1,1).trs() * vsr.Rot(vsr.Biv(0,1,0) * np.pi/6.0)
noise_motor_plane = vsr.Vec(0.1,0.1,0.1).trs() * vsr.Rot(vsr.Biv(1,0,0) * np.pi/24)
initial_motor = vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
mes = MotorEstimationSolver(initial_motor)
for a, b in zip(planes_a, planes_b):
#     mes.add_dual_plane_angle_error_residual_block(a,b.spin(noise_motor_plane))
    mes.add_dual_plane_angle_error_residual_block(a,b)

mes.set_parameterization('BIVECTOR_GENERATOR')
mes.linear_solver_type = "DENSE_QR"
mes.function_tolerance = 1e-8
mes.max_num_iterations = 100
estimated_motor, pln_summary_1 = mes.solve()
print(pln_summary_1['brief_report'])
print(np.array(motor))
print(np.array(estimated_motor))
print(np.allclose(np.array(motor), np.array(estimated_motor)))
print(np.allclose(np.array(motor), np.array(estimated_motor), 
                  rtol=1e-05, atol=1e-05))
fig, ax = plt.subplots(1)

# ax.set_aspect('equal')
plt.set_cmap('gray')

almost_black = '#262626'
spines_to_remove = ['top', 'right']
for spine in spines_to_remove:
    ax.spines[spine].set_visible(False)
ax.xaxis.set_ticks_position('none')
ax.yaxis.set_ticks_position('none')

spines_to_keep = ['bottom', 'left']
for spine in spines_to_keep:
    ax.spines[spine].set_linewidth(0.5)
    ax.spines[spine].set_color(almost_black)
ax.xaxis.label.set_color(almost_black)
ax.yaxis.label.set_color(almost_black)

ax.plot([it['cost'] for it in pln_summary_1['iterations'] ])

ax.title.set_color(almost_black)
ax.axhline(0, color=almost_black, linewidth=0.15)
ax.axvline(0, color=almost_black, linewidth=0.15)

# Remove the line around the legend box, and instead fill it with a light grey
# Also only use one point for the scatterplot legend because the user will 
# get the idea after just one, they don't need three.

# light_grey = np.array([float(248)/float(255)]*3)
# legend = ax.legend(frameon=True, scatterpoints=1)
# rect = legend.get_frame()
# rect.set_facecolor(light_grey)
# rect.set_linewidth(0.0)

plt.xlabel(r'Iteration $k$')
plt.ylabel(r'Cost')


# plt.savefig('/Users/lars/Dropbox/Apps/ShareLaTeX/2016-gacse/figures/cost_planes.pdf')<jupyter_output><empty_output><jupyter_text>Estimate motor using Valkenburg-Dorst solver<jupyter_code>vd = VDMotorEstimationSolver()
vd.add_dual_plane_observations(planes_a, planes_b)
vd_estimated_motor = vd.solve()
print(vd_estimated_motor)<jupyter_output>Mot: [ 0.87 -3.5e-17 -0.5 8.1e-18 0 0 0 0 ]<jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(vd_estimated_motor))
print(np.allclose(motor, vd_estimated_motor))<jupyter_output>[ 0.866  0.    -0.5    0.    -0.683 -0.433 -0.183 -0.25 ]
[ 0.866 -0.    -0.5    0.     0.     0.     0.     0.   ]
False<jupyter_text>Motor Estimation from heterogeneous conformal objects  Estimate motor using Valkenburg-Dorst solver<jupyter_code>vd = VDMotorEstimationSolver()
vd.add_point_observations(points_a, points_b)
vd.add_dual_line_observations(lines_a, lines_b)
vd.add_dual_plane_observations(planes_a, planes_b)
vd_estimated_motor = vd.solve()
print(vd_estimated_motor)<jupyter_output>Mot: [ -0.87 2.1e-18 0.5 -2.8e-17 0.68 0.43 0.18 0.25 ]<jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(vd_estimated_motor))
print(np.allclose(motor, vd_estimated_motor))
initial_motors = [vd_estimated_motor]
vd_summaries_no_noise = []
estimated_motors = []
for initial_motor in initial_motors:
    mes = MotorEstimationSolver(initial_motor)
    for a, b in zip(planes_a, planes_b):
        mes.add_dual_plane_angle_error_residual_block(a,b)
    for a, b in zip(lines_a, lines_b):
        mes.add_line_correspondences_residual_block(a,b)
    for a, b in zip(points_a, points_b):
        mes.add_point_correspondences_residual_block(a,b)
    mes.set_parameterization('BIVECTOR_GENERATOR')
    mes.linear_solver_type = "DENSE_QR"
    mes.max_num_iterations = 100
    mes.function_tolerance = 1e-2
    em, summary_het = mes.solve()
    estimated_motors.append(em)
    vd_summaries_no_noise.append(summary_het)
    print(np.array(initial_motor))
    print(np.array(motor))
    print(np.array(em))
    print(summary_het['brief_report'])
motor = vsr.Vec(1,1,1).trs() * vsr.Rot(vsr.Biv(0,1,0) * np.pi/6.0)
noise_motor = vsr.Vec(0.1,0.1,0.1).trs() * vsr.Rot(vsr.Biv(1,0,0) * np.pi/100)

noise_motor_plane = vsr.Vec(0.05,0.05,0.05).trs() * vsr.Rot(vsr.Biv(1,0,0) * np.pi/100)
noise_motor_lines = vsr.Vec(-0.01,-0.01,-0.01).trs() * vsr.Rot(vsr.Biv(0,1,0) * (-np.pi/100))
noise_motor_points = vsr.Vec(0.1,0.1,0.1).trs() * vsr.Rot(vsr.Biv(0,0,1) * (np.pi/100))

# initial_motor = vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)

initial_motors = [vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
                  vsr.Vec(0.1,0.1,0.1).trs() * vsr.Rot(vsr.Biv(0,1,0) * (np.pi/6.0 + np.pi/2.))]

labels = ["1", "$\pi$"]
summaries_no_noise = []
estimated_motors_no_noise = []
for initial_motor in initial_motors:
    mes = MotorEstimationSolver(initial_motor)
    for a, b in zip(planes_a, planes_b):
        mes.add_dual_plane_angle_error_residual_block(a,b)
    for a, b in zip(lines_a, lines_b):
        mes.add_line_correspondences_residual_block(a,b)
    for a, b in zip(points_a, points_b):
        mes.add_point_correspondences_residual_block(a,b)
    mes.set_parameterization('BIVECTOR_GENERATOR')
    mes.linear_solver_type = "DENSE_QR"
    mes.max_num_iterations = 100
    em, summary_het = mes.solve()
    estimated_motors_no_noise.append(em)
    summaries_no_noise.append(summary_het)
    print(initial_motor)
#     print(motor)
    print(em)

#     print(np.array(motor))
#     print(np.array(estimated_motor))
#     print(np.allclose(np.array(motor), np.array(estimated_motor)))
#     print(np.allclose(np.array(motor), np.array(estimated_motor), 
#                       rtol=1e-05, atol=1e-5))
    print(summary_het['brief_report'])
    print()
for s,l in zip(summaries_no_noise, labels):
    plt.plot([it['cost'] for it in s['iterations']], label=l)
plt.semilogy()
plt.legend()<jupyter_output><empty_output><jupyter_text>With noise<jupyter_code>motor = vsr.Vec(1,1,1).trs() * vsr.Rot(vsr.Biv(0,1,0) * np.pi/6.0)

noise_motor_planes = vsr.Vec(0.05,0.05,0.05).trs() * vsr.Rot(vsr.Biv(1,0,0) * np.pi/100)
noise_motor_lines = vsr.Vec(-0.01,-0.01,-0.01).trs() * vsr.Rot(vsr.Biv(0,1,0) * (-np.pi/100))
noise_motor_points = vsr.Vec(0.1,0.1,0.1).trs() * vsr.Rot(vsr.Biv(0,0,1) * (np.pi/100))

points_b_noisy = [b.spin(noise_motor_points) for b in points_b]
lines_b_noisy = [b.spin(noise_motor_lines) for b in lines_b]
planes_b_noisy = [b.spin(noise_motor_planes) for b in planes_b]<jupyter_output><empty_output><jupyter_text>Estimate motor using Valkenburg-Dorst solver<jupyter_code>vd = VDMotorEstimationSolver()
vd.add_point_observations(points_a, points_b_noisy)
vd.add_dual_line_observations(lines_a, lines_b_noisy)
vd.add_dual_plane_observations(planes_a, planes_b_noisy)
vd_estimated_motor = vd.solve()
print(vd_estimated_motor)<jupyter_output>Mot: [ -0.87 -0.0029 0.49 0.0083 0.76 0.45 0.25 0.24 ]<jupyter_text>Check solution<jupyter_code>print(np.array(motor))
print(np.array(vd_estimated_motor))
print(np.allclose(motor, vd_estimated_motor))
d = [np.linalg.norm(np.array(a.spin(vd_estimated_motor)) - np.array(a.spin(motor))) for a in points_a]
print(np.mean(d), np.std(d))
# initial_motors = [vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
#                   vsr.Vec(1,1,1).trs() * vsr.Rot(vsr.Biv(0,1,0) * (np.pi/6.0 + np.pi/2))]
summaries_noise = []
estimated_motors_noise = []
for initial_motor in initial_motors:
    mes = MotorEstimationSolver(initial_motor)
    for a, b in zip(planes_a, planes_b_noisy):
        mes.add_dual_plane_angle_error_residual_block(a,b)
    for a, b in zip(lines_a, lines_b_noisy):
        mes.add_line_correspondences_residual_block(a,b)
    for a, b in zip(points_a, points_b_noisy):
        mes.add_point_correspondences_residual_block(a,b)
    mes.set_parameterization('BIVECTOR_GENERATOR')
    mes.linear_solver_type = "DENSE_QR"
    mes.max_num_iterations = 100
#     mes.function_tolerance = 1e-4
    em, summary_het = mes.solve()
    estimated_motors_noise.append(em)
    summaries_noise.append(summary_het)
    print(np.array(initial_motor))
#     print(np.array(motor))
#     print(np.array(em))
    print(summary_het['brief_report'])
d = [[np.linalg.norm(np.array(a.spin(em)) - 
                np.array(a.spin(motor))) for a in points_a] for em in estimated_motors_noise]
for d in d:
    print(np.mean(d), np.std(d))
initial_motors = [vd_estimated_motor]
vd_summaries_noise = []
estimated_motors = []
for initial_motor in initial_motors:
    mes = MotorEstimationSolver(initial_motor)
    for a, b in zip(planes_a, planes_b_noisy):
        mes.add_dual_plane_angle_error_residual_block(a,b)
    for a, b in zip(lines_a, lines_b_noisy):
        mes.add_line_correspondences_residual_block(a,b)
    for a, b in zip(points_a, points_b_noisy):
        mes.add_point_correspondences_residual_block(a,b)
    mes.set_parameterization('BIVECTOR_GENERATOR')
    mes.linear_solver_type = "DENSE_QR"
    mes.max_num_iterations = 100
#     mes.function_tolerance = 1e-5
    em, summary_het = mes.solve()
    estimated_motors.append(em)
    vd_summaries_noise.append(summary_het)
    print(np.array(initial_motor))
    print(np.array(motor))
    print(np.array(em))
    print(summary_het['brief_report'])
for s,l in zip(summaries_no_noise, labels):
    plt.plot([it['cost'] for it in s['iterations']], label=l)
for s,l in zip(summaries_noise, labels):
    plt.plot([it['cost'] for it in s['iterations']], label=l)
plt.axhline(y=vd_summaries_noise[0]['iterations'][0]['cost'])
plt.axhline(y=vd_summaries_no_noise[0]['iterations'][0]['cost'])
plt.semilogy()
plt.legend()
for s in summaries:
    plt.plot([it['cost'] for it in s['iterations']])
plt.semilogy()
d = [[np.linalg.norm(np.array(a.spin(em)) - 
                np.array(a.spin(motor))) for a in points_a] for em in estimated_motors]
for d in d:
    print(np.mean(d), np.std(d))
fig, ax = plt.subplots(1)

ax.set_aspect('equal')

almost_black = '#262626'
spines_to_remove = ['top', 'right']
for spine in spines_to_remove:
    ax.spines[spine].set_visible(False)
ax.xaxis.set_ticks_position('none')
ax.yaxis.set_ticks_position('none')

spines_to_keep = ['bottom', 'left']
for spine in spines_to_keep:
    ax.spines[spine].set_linewidth(0.5)
    ax.spines[spine].set_color(almost_black)
ax.xaxis.label.set_color(almost_black)
ax.yaxis.label.set_color(almost_black)

# ax.grid(which="both")

ax.axhline(y=vd_summaries_noise[0]['iterations'][0]['cost'], 
           ls="-", lw=0.75,c='black',label="V\&D~[26], With noise")
ax.axhline(y=vd_summaries_no_noise[0]['iterations'][0]['cost'],
           ls="--", lw=0.75,c='black',label="V\&D~[26], Without noise")

ax.plot([it['cost'] for it in summaries_no_noise[1]['iterations']], label=r"Without noise, $\bar{\theta}_0 = \pi$")
ax.plot([it['cost'] for it in summaries_no_noise[0]['iterations']], label=r"Without noise, $\bar{\theta}_0 = {\pi \over 3}$")


ax.plot([it['cost'] for it in summaries_noise[1]['iterations']], label=r"With noise, $\bar{\theta}_0 = \pi$")
ax.plot([it['cost'] for it in summaries_noise[0]['iterations']], label=r"With noise, $\bar{\theta}_0 = {\pi \over 3}$")



ax.semilogy()

# ax.set_yscale('log')

ax.title.set_color(almost_black)
ax.axhline(0, color=almost_black, linewidth=0.15)
ax.axvline(0, color=almost_black, linewidth=0.15)

# Remove the line around the legend box, and instead fill it with a light grey
# Also only use one point for the scatterplot legend because the user will 
# get the idea after just one, they don't need three.

light_grey = np.array([float(248)/float(255)]*3)
legend = ax.legend(frameon=True, scatterpoints=1)
rect = legend.get_frame()
rect.set_facecolor(light_grey)
rect.set_linewidth(0.0)




plt.xlabel(r'Iteration $k$')
plt.ylabel(r'Cost')


plt.savefig('/home/lars/Dropbox/PhD/latex/2016-05-18_-_AACA-D-16-00064_Manuscript_Tingelstad_Egeland/cost_heterogeneous2.pdf')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.14626057787603872, 'fraction_numerical': 0.04619958832050011, 'mean_word_length': 3.3507462686567164, 'pattern_counts': {'":': 2, '<': 95, '<?xml version=': 0, '>': 95, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 48, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '27', 'dir': 'jupyter-structured-clean-dedup', 'id': '367892', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 11073, 'n_tokens_neox': 10181, 'n_words': 1633, 'path': 'python/2016-05-18-gacse.ipynb', 'repo_name': 'tingelst/game', 'size': '26234'}