From d927595a7117111e46453a518ab35100864481b2 Mon Sep 17 00:00:00 2001 From: Lars Tingelstad Date: Thu, 30 Mar 2017 12:26:28 +0200 Subject: [PATCH] Funny --- python/Lines-v2.ipynb | 284 +++++++++++++++++++++++++++++++++++++++ src/motor_estimation.cpp | 147 ++++++++++++-------- 2 files changed, 376 insertions(+), 55 deletions(-) create mode 100644 python/Lines-v2.ipynb diff --git a/python/Lines-v2.ipynb b/python/Lines-v2.ipynb new file mode 100644 index 0000000..35f1929 --- /dev/null +++ b/python/Lines-v2.ipynb @@ -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 +} diff --git a/src/motor_estimation.cpp b/src/motor_estimation.cpp index 2cc6cce..4fafcd6 100644 --- a/src/motor_estimation.cpp +++ b/src/motor_estimation.cpp @@ -268,8 +268,7 @@ class MotorEstimationSolver { }; struct FlatPointCommutatorCostFunctor { - FlatPointCommutatorCostFunctor(const Pnt &a, const Pnt &b) - : a_(a), b_(b) {} + FlatPointCommutatorCostFunctor(const Pnt &a, const Pnt &b) : a_(a), b_(b) {} template auto operator()(const T *const motor, T *residual) const -> bool { @@ -290,8 +289,7 @@ class MotorEstimationSolver { }; struct DualPlaneCommutatorCostFunctor { - DualPlaneCommutatorCostFunctor(const Dlp &a, const Dlp &b) - : a_(a), b_(b) {} + DualPlaneCommutatorCostFunctor(const Dlp &a, const Dlp &b) : a_(a), b_(b) {} template auto operator()(const T *const motor, T *residual) const -> bool { @@ -314,9 +312,9 @@ class MotorEstimationSolver { // residual[5] = b_d[2]; // } // else { - residual[3] = d[3]; - residual[4] = d[4]; - residual[5] = d[5]; + residual[3] = d[3]; + residual[4] = d[4]; + residual[5] = d[5]; // } // for (int i = 0; i < 6; ++i) { @@ -395,7 +393,8 @@ class MotorEstimationSolver { }; struct LineProjectedCommutatorCostFunctor { - LineProjectedCommutatorCostFunctor(const Dll &a, const Dll &b) : a_(a), b_(b) {} + LineProjectedCommutatorCostFunctor(const Dll &a, const Dll &b) + : a_(a), b_(b) {} template auto operator()(const T *const motor, T *residual) const -> bool { @@ -405,10 +404,12 @@ class MotorEstimationSolver { DualLine c = a.spin(M); DualLine d = c * ~b; - // Point p = - // Flat::location(d, Vector{T(0.0),T(0.0), T(0.0)}.null(), true) * Scalar{T(0.5)}; + // Point p = + // Flat::location(d, Vector{T(0.0),T(0.0), T(0.0)}.null(), true) * + // Scalar{T(0.5)}; // p = p.unit(); - // Motor Tr{T(1.0), T(0.0), T(0.0), T(0.0), -p[0], -p[1], -p[2], T(0.0)}; + // Motor Tr{T(1.0), T(0.0), T(0.0), T(0.0), -p[0], -p[1], -p[2], + // T(0.0)}; // DualLine dt = d.spin(~Tr); residual[0] = d[0]; @@ -416,23 +417,22 @@ class MotorEstimationSolver { residual[2] = d[2]; Bivector A{d[0], d[1], d[2]}; - if( A.norm() > T(0.00000001) ) { - Vector f{d[3], d[4], d[5]}; - Vector b_d = Op::reject(f, A.unit()); - residual[3] = b_d[0]; - residual[4] = b_d[1]; - residual[5] = b_d[2]; - } - else { + if (A.norm() > T(0.00000001)) { + Vector f{d[3], d[4], d[5]}; + Vector b_d = Op::reject(f, A.unit()); + residual[3] = b_d[0]; + residual[4] = b_d[1]; + residual[5] = b_d[2]; + } else { std::cout << "Parallel!" << std::endl; residual[3] = d[3]; residual[4] = d[4]; residual[5] = d[5]; } - // residual[3] = d[3]; - // residual[4] = d[4]; - // residual[5] = d[5]; + // residual[3] = d[3]; + // residual[4] = d[4]; + // residual[5] = d[5]; // for (int i = 0; i < 6; ++i) { // residual[i] = dt[i]; @@ -563,6 +563,31 @@ class MotorEstimationSolver { struct LineCorrespondencesCostFunctor { LineCorrespondencesCostFunctor(const Dll &a, const Dll &b) : a_(a), b_(b) {} + template + auto operator()(const T *const motor, T *residual) const -> bool { + Motor M(motor); + DualLine a(a_); + DualLine b(b_); + DualLine c = a.spin(M); + DualLine d = c - b; + residual[0] = d[0]; + residual[1] = d[1]; + residual[2] = d[2]; + residual[3] = d[3]; + residual[4] = d[4]; + residual[5] = d[5]; + return true; + } + + private: + const Dll a_; + const Dll b_; + }; + + struct LineProjectedCorrespondencesCostFunctor { + LineProjectedCorrespondencesCostFunctor(const Dll &a, const Dll &b) + : a_(a), b_(b) {} + template auto operator()(const T *const motor, T *residual) const -> bool { Motor M(motor); @@ -576,23 +601,24 @@ class MotorEstimationSolver { residual[2] = d[2]; Bivector A{d[0], d[1], d[2]}; - if( A.norm() > T(0.00000001) ) { + if (A.norm() > T(0.00000001)) { Vector f{d[3], d[4], d[5]}; Vector b_d = Op::reject(f, A.unit()); residual[3] = b_d[0]; residual[4] = b_d[1]; residual[5] = b_d[2]; - } - else { + } else { std::cout << "Parallel!" << std::endl; residual[3] = d[3]; residual[4] = d[4]; residual[5] = d[5]; } - // Point p = - // Flat::location(d, Vector{T(0.0),T(0.0), T(0.0)}.null(), true) * Scalar{T(0.5)}; - // Motor Tr{T(1.0), T(0.0), T(0.0), T(0.0), -p[0], -p[1], -p[2], T(0.0)}; + // Point p = + // Flat::location(d, Vector{T(0.0),T(0.0), T(0.0)}.null(), true) * + // Scalar{T(0.5)}; + // Motor Tr{T(1.0), T(0.0), T(0.0), T(0.0), -p[0], -p[1], -p[2], + // T(0.0)}; // DualLine dt = d.spin(~Tr); @@ -747,8 +773,8 @@ class MotorEstimationSolver { bool AddDualPlaneCommutatorResidualBlock(const Dlp &a, const Dlp &b) { ceres::CostFunction *cost_function = - new ceres::AutoDiffCostFunction( - new DualPlaneCommutatorCostFunctor(a, b)); + new ceres::AutoDiffCostFunction( + new DualPlaneCommutatorCostFunctor(a, b)); problem_.AddResidualBlock(cost_function, NULL, &motor_[0]); return true; } @@ -802,23 +828,25 @@ class MotorEstimationSolver { } auto AddLineDualAngleResidualBlock(const Dll &a, const Dll &b) -> bool { ceres::CostFunction *cost_function = - new ceres::AutoDiffCostFunction( - new LineDualAngleCostFunctor(a, b)); + new ceres::AutoDiffCostFunction( + new LineDualAngleCostFunctor(a, b)); problem_.AddResidualBlock(cost_function, NULL, &motor_[0]); return true; } auto AddLineAntiCommutatorResidualBlock(const Dll &a, const Dll &b) -> bool { ceres::CostFunction *cost_function = - new ceres::AutoDiffCostFunction( - new LineAntiCommutatorCostFunctor(a, b)); + new ceres::AutoDiffCostFunction( + new LineAntiCommutatorCostFunctor(a, b)); problem_.AddResidualBlock(cost_function, NULL, &motor_[0]); return true; } - auto AddLineProjectedCommutatorResidualBlock(const Dll &a, const Dll &b) -> bool { + auto AddLineProjectedCommutatorResidualBlock(const Dll &a, const Dll &b) + -> bool { ceres::CostFunction *cost_function = - new ceres::AutoDiffCostFunction( - new LineProjectedCommutatorCostFunctor(a, b)); + new ceres::AutoDiffCostFunction( + new LineProjectedCommutatorCostFunctor(a, b)); problem_.AddResidualBlock(cost_function, NULL, &motor_[0]); return true; } @@ -830,6 +858,16 @@ class MotorEstimationSolver { problem_.AddResidualBlock(cost_function, NULL, &motor_[0]); return true; } + + auto AddLineProjectedCorrespondencesResidualBlock(const Dll &a, const Dll &b) + -> bool { + ceres::CostFunction *cost_function = + new ceres::AutoDiffCostFunction( + new LineProjectedCorrespondencesCostFunctor(a, b)); + problem_.AddResidualBlock(cost_function, NULL, &motor_[0]); + return true; + } auto AddLineCorrespondencesResidualBlock(const Dll &a, const Dll &b) -> bool { ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction( @@ -927,14 +965,11 @@ class MotorEstimationSolver { &motor_[0], new ceres::AutoDiffLocalParameterization); - } - else if (type == "OUTER_EXPONENTIAL") { - problem_.SetParameterization( - &motor_[0], - new ceres::AutoDiffLocalParameterization); - } - else if (type == "BIVECTOR_GENERATOR_ADEPT") { + } else if (type == "OUTER_EXPONENTIAL") { + problem_.SetParameterization( + &motor_[0], + new ceres::AutoDiffLocalParameterization); + } else if (type == "BIVECTOR_GENERATOR_ADEPT") { std::cout << "game:: ADEPT Using bivector generator (Versor)." << std::endl; problem_.SetParameterization( @@ -949,10 +984,10 @@ class MotorEstimationSolver { class UpdateMotorEachIterationCallback : public ceres::IterationCallback { public: UpdateMotorEachIterationCallback(const Mot *motor, std::vector *list) - : motor_(motor), list_(list), iteration_k(1) {} + : motor_(motor), list_(list), iteration_k(1) {} virtual ceres::CallbackReturnType operator()(const ceres::IterationSummary &summary) { - std::cout << "Iteration " << iteration_k++ << std::endl; + // std::cout << "Iteration " << iteration_k++ << std::endl; list_->push_back(Mot(*motor_)); return ceres::SOLVER_CONTINUE; } @@ -989,26 +1024,28 @@ PYBIND11_PLUGIN(motor_estimation) { &MotorEstimationSolver::AddTangentVectorPointAngleErrorResidualBlock) .def("add_dual_plane_angle_error_residual_block", &MotorEstimationSolver::AddDualPlaneAngleErrorResidualBlock) - .def("add_dual_plane_commutator_residual_block", - &MotorEstimationSolver::AddDualPlaneCommutatorResidualBlock) + .def("add_dual_plane_commutator_residual_block", + &MotorEstimationSolver::AddDualPlaneCommutatorResidualBlock) .def("add_dual_plane_difference_residual_block", &MotorEstimationSolver::AddDualPlaneDifferenceResidualBlock) .def("add_circle_difference_residual_block", &MotorEstimationSolver::AddCircleDifferenceResidualBlock) .def("add_circle_commutator_residual_block", &MotorEstimationSolver::AddCircleCommutatorResidualBlock) + .def("add_line_projected_correspondences_residual_block", + &MotorEstimationSolver::AddLineProjectedCorrespondencesResidualBlock) .def("add_line_correspondences_residual_block", &MotorEstimationSolver::AddLineCorrespondencesResidualBlock) .def("add_line_angle_distance_residual_block", &MotorEstimationSolver::AddLineAngleDistanceResidualBlock) .def("add_line_angle_distance_norm_residual_block", &MotorEstimationSolver::AddLineAngleDistanceNormResidualBlock) - .def("add_line_dual_angle_residual_block", - &MotorEstimationSolver::AddLineDualAngleResidualBlock) - .def("add_line_anticommutator_residual_block", - &MotorEstimationSolver::AddLineAntiCommutatorResidualBlock) - .def("add_line_projected_commutator_residual_block", - &MotorEstimationSolver::AddLineProjectedCommutatorResidualBlock) + .def("add_line_dual_angle_residual_block", + &MotorEstimationSolver::AddLineDualAngleResidualBlock) + .def("add_line_anticommutator_residual_block", + &MotorEstimationSolver::AddLineAntiCommutatorResidualBlock) + .def("add_line_projected_commutator_residual_block", + &MotorEstimationSolver::AddLineProjectedCommutatorResidualBlock) .def("add_line_commutator_residual_block", &MotorEstimationSolver::AddLineCommutatorResidualBlock) .def("add_line_inner_product_residual_block",