Skip to content

Commit

Permalink
Funny
Browse files Browse the repository at this point in the history
  • Loading branch information
@tingelst
tingelst committed Mar 30, 2017
1 parent 30b70ca commit d927595
Show file tree
Hide file tree
Showing 2 changed files with 376 additions and 55 deletions.
284 changes: 284 additions & 0 deletions python/Lines-v2.ipynb
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,284 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Populating the interactive namespace from numpy and matplotlib\n"
]
}
],
"source": [
"from __future__ import print_function\n",
"import sys\n",
"sys.path.append('../build/')\n",
"%pylab inline\n",
"np.set_printoptions(precision=4, suppress=True)\n",
"import versor as vsr\n",
"from versor.drawing import *\n",
"from motor_estimation import MotorEstimationSolver\n",
"from game import VDMotorEstimationSolver"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Generate motors"
]
},
{
"cell_type": "code",
"execution_count": 50,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"n_motors = 10\n",
"motors = [((vsr.Vec(*np.random.random(3)).unit() * np.random.uniform(-100,100)).trs() * \n",
" vsr.Rot(vsr.Biv(*np.random.random(3)).unit() * np.random.uniform(-pi,pi))) for i in range(n_motors)]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Generate lines"
]
},
{
"cell_type": "code",
"execution_count": 51,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"10\n"
]
}
],
"source": [
"n_lines = 10\n",
"lines_a = [vsr.Dll(vsr.Vec(*np.random.random(3)).null(), \n",
" vsr.Vec(*np.random.random(3)).unit()).unit() for i in range(n_lines)]\n",
"lines_b = [[line.spin(motor) for line in lines_a] for motor in motors]\n",
"print(len(lines_b))"
]
},
{
"cell_type": "code",
"execution_count": 54,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Solver Summary (v 1.12.0-eigen-(3.3.3)-lapack-suitesparse-(4.5.4)-cxsparse-(3.1.9)-openmp)\n",
"\n",
" Original Reduced\n",
"Parameter blocks 1 1\n",
"Parameters 8 8\n",
"Effective parameters 6 6\n",
"Residual blocks 10 10\n",
"Residual 60 60\n",
"\n",
"Minimizer TRUST_REGION\n",
"\n",
"Dense linear algebra library EIGEN\n",
"Trust region strategy LEVENBERG_MARQUARDT\n",
"\n",
" Given Used\n",
"Linear solver DENSE_QR DENSE_QR\n",
"Threads 100 100\n",
"Linear solver threads 100 100\n",
"Linear solver ordering AUTOMATIC 1\n",
"\n",
"Cost:\n",
"Initial 9.478611e+02\n",
"Final 3.415861e-19\n",
"Change 9.478611e+02\n",
"\n",
"Minimizer iterations 16\n",
"Successful steps 11\n",
"Unsuccessful steps 5\n",
"\n",
"Time (in seconds):\n",
"Preprocessor 0.0001\n",
"\n",
" Residual evaluation 0.0026\n",
" Jacobian evaluation 0.0031\n",
" Linear solver 0.0003\n",
"Minimizer 0.0064\n",
"\n",
"Postprocessor 0.0000\n",
"Total 0.0065\n",
"\n",
"Termination: CONVERGENCE (Parameter tolerance reached. Relative step_norm: 2.141322e-11 <= 1.000000e-08.)\n",
"\n",
"Mot: [ 0.99 -0.031 -0.0099 -0.16 8.9 4.8 3.2 -1.5 ]\n"
]
}
],
"source": [
"initial_motor = vsr.Mot(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)\n",
"mes = MotorEstimationSolver(initial_motor)\n",
"for a, b in zip(lines_a, lines_b[0]):\n",
" mes.add_line_correspondences_residual_block(a,b)\n",
"# mes.add_line_commutator_residual_block(a,b)\n",
"# mes.add_line_dual_angle_residual_block(a,b)\n",
"mes.set_parameterization('BIVECTOR_GENERATOR')\n",
"mes.linear_solver_type = 'DENSE_QR'\n",
"mes.num_threads = 100\n",
"mes.num_linear_solver_threads = 100\n",
"(estimated_motor, summary, _) = mes.solve()\n",
"print(summary['full_report'])\n",
"print(estimated_motor)"
]
},
{
"cell_type": "code",
"execution_count": 34,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/plain": [
"Mot: [ 0.99 -0.029 -0.085 -0.089 -8.4 -7.3 -10 0.42 ]"
]
},
"execution_count": 34,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"motors[0]"
]
},
{
"cell_type": "code",
"execution_count": 35,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"def daniilidis_motor(LAs, LBs):\n",
" Ds = []\n",
" for LA, LB in zip(LAs, LBs):\n",
" D = np.zeros((8,8))\n",
" for i in range(8):\n",
" ei = vsr.Mot(0,0,0,0,0,0,0,0)\n",
" ei[i] = 1.0\n",
" D[:,i] = np.array(ei * LA - LB * ei)\n",
" Ds.append(D[1:7,:].copy())\n",
" \n",
" Ds = np.array(Ds).reshape(-1,8)\n",
" [U, s, Vt] = np.linalg.svd(Ds)\n",
"\n",
" v7 = Vt.T[:,-2].copy()\n",
" v8 = Vt.T[:,-1].copy()\n",
" \n",
" v7 = np.array([v7[0], v7[3], -v7[2], v7[1], -v7[7],v7[4], v7[5], v7[6]])\n",
" v8 = np.array([v8[0], v8[3], -v8[2], v8[1], -v8[7],v8[4], v8[5], v8[6]])\n",
" \n",
" u1 = v7[:4]\n",
" v1 = v7[4:]\n",
" u2 = v8[:4]\n",
" v2 = v8[4:]\n",
"\n",
" a = np.inner(u1,v1)\n",
" b = np.inner(u1,v2) + np.inner(u2,v1)\n",
" c = np.inner(u2,v2)\n",
" [s1, s2] = np.roots([a,b,c])\n",
"\n",
" val1 = (s1**2 * np.inner(u1,u1)) + (2 * s1 * np.inner(u1,u2)) + (np.inner(u2,u2))\n",
" val2 = (s2**2 * np.inner(u1,u1)) + (2 * s2 * np.inner(u1,u2)) + (np.inner(u2,u2))\n",
" \n",
" if val1 > val2:\n",
" s = s1\n",
" val = val1\n",
" else:\n",
" s = s2\n",
" val = val2\n",
"\n",
" lambda2 = np.sqrt(1./val)\n",
" lambda1 = s * lambda2\n",
" \n",
" m_arr = lambda1 * Vt.T[:,-2] + lambda2 * Vt.T[:,-1]\n",
"\n",
" return vsr.Mot(*m_arr)"
]
},
{
"cell_type": "code",
"execution_count": 36,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/plain": [
"Mot: [ -0.99 0.029 0.085 0.089 8.4 7.3 10 -0.42 ]"
]
},
"execution_count": 36,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"daniilidis_motor(lines_a, lines_b[0])"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.1"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
Loading

0 comments on commit d927595

Please sign in to comment.