nonlinear_optimizer.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import tensorflow as tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# unit meter, the square in front of camera view, the robot origin offset is set as 0.6m\n",
    "source = np.array([[0, 0.5], [1, 0.5], [1, -0.5], [0, -0.5]], dtype=np.float64)\n",
    "source = source.transpose()\n",
    "\n",
    "# image pixel target\n",
    "target = np.array([[14.32 , 140.71], [ 120.78, 95.5], [199.49 ,96.84], [302.7 ,140.71]], dtype=np.float64)\n",
    "\n",
    "# transform to camera coordinate, not scaled yet\n",
    "target = np.array([160 - target[:, 1], target[:, 0]])\n",
    "\n",
    "# estimated scale factor for sensor"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "T = tf.constant(target)\n",
    "S = tf.constant(source)\n",
    "\n",
    "CameraOffset = tf.Variable(0.6, dtype=tf.float64)\n",
    "S = tf.stack([S[0, :] + CameraOffset, S[1, :]])\n",
    "\n",
    "ScaleFactor = tf.Variable(2000.0, dtype=tf.float64)\n",
    "T = T / ScaleFactor\n",
    "\n",
    "Cx = 0\n",
    "Cy = 320 / (2 * ScaleFactor)\n",
    "Cz = tf.Variable(0.5, dtype=tf.float64)\n",
    "\n",
    "Ex = 160 / (2 * ScaleFactor)\n",
    "Ey = Cy\n",
    "logEz = tf.Variable(np.log(0.04), dtype=tf.float64)\n",
    "Ez = -tf.exp(logEz)\n",
    "\n",
    "Sx, Sy = S[0, :], S[1, :]\n",
    "Tx, Ty = T[0, :], T[1, :]\n",
    "\n",
    "lossX = -Ex*(Cx + Ez - Sx) + Ez*(Cz + Ex) + Tx*(Cx + Ez - Sx)\n",
    "lossY = -Ey*(Cx + Ez - Sx) + Ez*(-Cy + Ey + Sy) + Ty*(Cx + Ez - Sx)\n",
    "\n",
    "ForwardTx = (Cx*Ex - Cz*Ez - Ex*Sx)/(Cx + Ez - Sx)\n",
    "ForwardTy = (Ey*(Cx + Ez - Sx) - Ez*(-Cy + Ey + Sy))/(Cx + Ez - Sx)\n",
    "\n",
    "loss = tf.reduce_mean(lossX**2 + lossY**2)\n",
    "\n",
    "learning_rate = tf.Variable(0.000001)\n",
    "optimizer = tf.train.GradientDescentOptimizer(learning_rate)\n",
    "\n",
    "offset_grad = optimizer.compute_gradients(loss, [CameraOffset])\n",
    "offset_train = optimizer.apply_gradients(offset_grad)\n",
    "\n",
    "scale_grad = optimizer.compute_gradients(loss, [ScaleFactor])\n",
    "scale_train = optimizer.apply_gradients(scale_grad)\n",
    "\n",
    "view_depth_grad = optimizer.compute_gradients(loss, [logEz])\n",
    "view_depth_train = optimizer.apply_gradients(view_depth_grad)\n",
    "\n",
    "height_grad = optimizer.compute_gradients(loss, [Cz])\n",
    "height_train = optimizer.apply_gradients(height_grad)\n",
    "\n",
    "train_op = optimizer.minimize(loss)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "sess = tf.Session()\n",
    "sess.run(tf.global_variables_initializer())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "loss: 0.000454365989491\n"
     ]
    }
   ],
   "source": [
    "#global train\n",
    "iter_range = 2000\n",
    "for i in range(iter_range):\n",
    "    loss_val,  _ = sess.run([loss, train_op])\n",
    "    if i == iter_range - 1:\n",
    "        print('loss:', loss_val)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "loss and depth: 0.000454374284969 0.040000036895\n"
     ]
    }
   ],
   "source": [
    "# optimize view depth\n",
    "iter_range = 2000\n",
    "for i in range(iter_range):\n",
    "    loss_val, Ez_val, _ = sess.run([loss, Ez, view_depth_train])\n",
    "    if i == iter_range - 1:\n",
    "        print('loss and depth:', loss_val, -Ez_val)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "loss and height: 0.000454370748116 0.499991589511\n"
     ]
    }
   ],
   "source": [
    "# optimize camera height\n",
    "iter_range = 20000\n",
    "for i in range(iter_range):\n",
    "    loss_val, Cz_val, _ = sess.run([loss, Cz, height_train])\n",
    "    if i == iter_range - 1:\n",
    "        print('loss and height:', loss_val, Cz_val)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "loss and scale: 0.000454370747939 2000.0\n"
     ]
    }
   ],
   "source": [
    "# optimize scale factor\n",
    "iter_range = 2000\n",
    "for i in range(iter_range):\n",
    "    loss_val, ScaleFactor_val, _ = sess.run([loss, ScaleFactor, scale_train])\n",
    "    if i == iter_range - 1:\n",
    "        print('loss and scale:', loss_val, ScaleFactor_val)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "loss and camera offset: 0.000454362805894 0.599996013518\n"
     ]
    }
   ],
   "source": [
    "# optimize CameraOffset\n",
    "iter_range = 2000\n",
    "for i in range(iter_range):\n",
    "    loss_val, CameraOffset_val, _ = sess.run([loss, CameraOffset, offset_train])\n",
    "    if i == iter_range - 1:\n",
    "        print('loss and camera offset:', loss_val, CameraOffset_val)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.00624981,  0.02682924,  0.02682924,  0.00624981])"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# validate forward inference\n",
    "sess.run(ForwardTx)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.04874978,  0.06780484,  0.09219516,  0.11125022])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sess.run(ForwardTy)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.009645,  0.03225 ,  0.03158 ,  0.009645],\n",
       "       [ 0.00716 ,  0.06039 ,  0.099745,  0.15135 ]])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sess.run(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[0.079999999999945434, 0.49999915892264074]"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sess.run([Cy, Cz])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[0.039999999999972717, 0.079999999999945434, -0.040000036913171277]"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sess.run([Ex, Ey, Ez])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[array([ 12.49962629,  53.65848987,  53.65848987,  12.49962629]),\n",
       " array([  97.49955662,  135.60967485,  184.39032515,  222.50044338])]"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sess.run([ScaleFactor*ForwardTx, ScaleFactor*ForwardTy])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.009645,  0.03225 ,  0.03158 ,  0.009645],\n",
       "       [ 0.00716 ,  0.06039 ,  0.099745,  0.15135 ]])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sess.run(T)"
   ]
  },
  {
   "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.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}