Camera_Triangulation_Problem_Solution.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "Camera Triangulation Problem Solution",
      "provenance": [],
      "collapsed_sections": [],
      "include_colab_link": true
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/cakshay2013/Convex-Optimization-Problems-and-Solutions/blob/main/Camera_Triangulation_Problem_Solution.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "eCNumoGbFS31",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "431a047e-2331-4c39-d6f6-2bbf54558512"
      },
      "source": [
        "import numpy as np\n",
        "import cvxpy as cp\n",
        " \n",
        "# Solution of part (a):\n",
        " \n",
        "# fk(x) = (Ak@x+Bk)/(Ck.T@x+dk) and gk(x)=||fx(x)-yk||2\n",
        "# using epigraph form, gk(x)<=t (t is a variable)\n",
        "# on reformulating, gk(x) = ||(Ak@x+Bk)-(Ck.T@x+dk)*yk||2 <=t*(Ck.T@x+dk)\n",
        "# thus, gk(x) <= t is a SOCP form\n",
        "# but g(x) is maximum of gk(x), k=1,2,3,4, and we are minimizing this maximum error value\n",
        "# thus, we are minimizing infinity norm of a 2-norm SOCP function in x,t which is a quasi convex optimization, as the domain of gk(x)\n",
        "# includes set of x, Ck.T@x+dk>0.\n",
        " \n",
        "# Solution of part (b):\n",
        " \n",
        "A1 = np.array([[1, 0, 0], [0, 1, 0]])\n",
        "B1 = np.transpose(np.array([0, 0]))\n",
        "C1 = np.array([0, 0, 1])\n",
        "d1 = 0\n",
        " \n",
        "A2 = np.array([[1, 0, 0], [0, 0, 1]])\n",
        "B2 = np.transpose(np.array([0, 0]))\n",
        "C2 = np.array([0, -1, 0])\n",
        "d2 = 10\n",
        " \n",
        "A3 = np.array([[1, 1, 1], [-1, 1, 1]])\n",
        "B3 = np.transpose(np.array([-10, 0]))\n",
        "C3 = np.array([-1, -1, 1])\n",
        "d3 = 10\n",
        " \n",
        "A4 = np.array([[0, 1, 1], [0, -1, 1]])\n",
        "B4 = np.transpose(np.array([0, 0]))\n",
        "C4 = np.array([-1, 0, 0])\n",
        "d4 = 10\n",
        " \n",
        "y1 = np.transpose(np.array([0.98, 0.93]))\n",
        "y2 = np.transpose(np.array([1.01, 1.01]))\n",
        "y3 = np.transpose(np.array([0.95, 1.05]))\n",
        "y4 = np.transpose(np.array([2.04, 0.00]))\n",
        " \n",
        "u = 1\n",
        "l = 0\n",
        "epsilon = 1.0e-4\n",
        " \n",
        "while(u-l>=epsilon):\n",
        "  k = (u+l)/2\n",
        "  x = cp.Variable(3)\n",
        " \n",
        "  objective = cp.Minimize(0)\n",
        "  constraints = [cp.norm(((A1@x + B1)-(C1@x + d1)*y1), 2)<=k*(C1@x + d1), cp.norm(((A2@x + B2)-(C2@x + d2)*y2), 2)<=k*(C2@x + d2)]\n",
        "  constraints += [cp.norm(((A3@x + B3)-(C3@x + d3)*y3), 2)<=k*(C3@x + d3), cp.norm(((A4@x + B4)-(C4@x + d4)*y4), 2)<=k*(C4@x + d4)]\n",
        "  prob = cp.Problem(objective, constraints)\n",
        "  prob.solve()\n",
        " \n",
        "  if prob.status == 'optimal':\n",
        "    u = k\n",
        "    kopt = k\n",
        "  else:\n",
        "    l = k\n",
        " \n",
        "print(\"x values/position of object:\", x.value)\n",
        "print(\"Optimum K for feasibility:\", kopt)\n",
        "print(\"Linear fractional mapping of the object f1(x) from the first camera\", ((A1@x + B1)/(C1@x + d1)).value)\n",
        "print(\"Linear fractional mapping of the object f2(x) from the second camera\", ((A2@x + B2)/(C2@x + d2)).value)\n",
        "print(\"Linear fractional mapping of the object f3(x) from the third camera\", ((A3@x + B3)/(C3@x + d3)).value)\n",
        "print(\"Linear fractional mapping of the object f4(x) from the fourth camera\", ((A4@x + B4)/(C4@x + d4)).value)"
      ],
      "execution_count": null,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "x values/position of object: [4.91286976 5.01665726 5.19803717]\n",
            "Optimum K for feasibility: 0.04949951171875\n",
            "Linear fractional mapping of the object f1(x) from the first camera [0.9451394  0.96510608]\n",
            "Linear fractional mapping of the object f2(x) from the second camera [0.98585829 1.04308241]\n",
            "Linear fractional mapping of the object f3(x) from the third camera [0.97324747 1.00632333]\n",
            "Linear fractional mapping of the object f4(x) from the fourth camera [2.00794828 0.03565466]\n"
          ]
        }
      ]
    }
  ]
}