{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# EECS 16A: Homework 3" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Problem 2: Kinematic Model for a Simple Car\n", "\n", "This script helps to visualize the difference between a nonlinear model and a corresponding linear approximation for a simple car. What you should notice is that the linear model is similar to the nonlinear model when you are close to the point where the approximation is made." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "''' Problem/Model Setup'''\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "\n", "# Vehicle Model Constants\n", "L = 1.0 # length of the car, meters\n", "dt = 0.1 # time difference between timestep (k+1) and timestep k, seconds\n", "\n", "''' Nonlinear Vehicle Model Update Equation '''\n", "def nonlinear_vehicle_model(initial_state, inputs, num_steps):\n", " x = initial_state # x position, meters\n", " y = initial_state # y position, meters\n", " theta = initial_state # heading (wrt x-axis), radians\n", " v = initial_state # speed, meters per second\n", " \n", " a = inputs # acceleration, meters per second squared\n", " phi = inputs # steering angle, radians\n", " \n", " state_history = [] # array to hold state values as the time step k advances.\n", " state_history.append([x,y,theta,v]) # add the initial state (i.e. k = 0) to history.\n", " \n", " for i in range(0, num_steps):\n", " # Find the next state, at time k+1, by applying the nonlinear model to the current state, at time k.\n", " x_next = x + v * np.cos(theta) * dt\n", " y_next = y + v * np.sin(theta) * dt\n", " theta_next = theta + v/L * np.tan(phi) * dt\n", " v_next = v + a * dt\n", " \n", " # Add the next state to the history.\n", " state_history.append([x_next,y_next,theta_next,v_next])\n", " \n", " # Advance to the next state, at time k+1, to get ready for next loop iteration.\n", " x = x_next\n", " y = y_next\n", " theta = theta_next\n", " v = v_next\n", " \n", " return np.array(state_history)\n", "\n", "''' Linear Vehicle Model Update Equation '''\n", "def linear_vehicle_model(A, B, initial_state, inputs, num_steps):\n", " # Note: A should be a 4x4 matrix, B should be a 4x2 matrix for this linear model.\n", " \n", " x = initial_state # x position, meters\n", " y = initial_state # y position, meters\n", " theta = initial_state # heading (wrt x-axis), radians\n", " v = initial_state # speed, meters per second\n", " \n", " a = inputs # acceleration, meters per second squared\n", " phi = inputs # steering angle, radians\n", " \n", " state_history = [] # array to hold state values as the time step k advances.\n", " state_history.append([x,y,theta,v]) # add the initial state (i.e. k = 0) to history.\n", " \n", " for i in range(0, num_steps):\n", " # Find the next state, at time k+1, by applying the nonlinear model to the current state, at time k.\n", " state_next = np.dot(A, state_history[-1]) + np.dot(B, inputs)\n", " \n", " # Add the next state to the history.\n", " state_history.append(state_next)\n", " \n", " # Advance to the next state, at time k+1, to get ready for next loop iteration.\n", " state = state_next\n", " \n", " return np.array(state_history)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "''' Plotting Setup'''\n", "def make_model_comparison_plot(state_predictions_nonlinear, state_predictions_linear):\n", " f = plt.figure()\n", " plt.plot(state_predictions_nonlinear[0,0], state_predictions_nonlinear[0,1], 'go', label='Start')\n", " plt.plot(state_predictions_nonlinear[:,0], state_predictions_nonlinear[:,1], 'r', label='Nonlinear')\n", " plt.plot(state_predictions_linear[:,0], state_predictions_linear[:,1], 'k.', label='Linear')\n", " plt.legend(loc='upper left')\n", " plt.xlim([4, 8])\n", " plt.ylim([9, 12])\n", " plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### PART B\n", "\n", "TODO: Fill in the matrices A and B for the linear system approximating the nonlinear vehicle model under small heading and steering angle approximations.\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "A = np.array([[1,0,0,dt], \n", " [0,1,0,0], \n", " [0,0,1,0], \n", " [0,0,0,1]])\n", "\n", "B = np.array([[0,0], \n", " [0,0], \n", " [0,0], \n", " [dt,0]])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### PART C \n", " \n", "TODO: Your task is to fill out the state and input values from Problem C and look at the resulting plot. The plot should help you to visualize the difference between using a linear model and a nonlinear model for this specific starting state and input." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "x_init = 5.0\n", "y_init = 10.0\n", "theta_init = 0.0\n", "v_init = 2.0\n", "a_input = 1.0\n", "phi_input = 0.0001\n", "\n", "state_init = [x_init, y_init, theta_init, v_init]\n", "state_predictions_nonlinear = nonlinear_vehicle_model(state_init, [a_input, phi_input], 10)\n", "state_predictions_linear = linear_vehicle_model(A, B, state_init, [a_input, phi_input], 10)\n", "\n", "make_model_comparison_plot(state_predictions_nonlinear, state_predictions_linear)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### PART D\n", " \n", "TODO: Your task is to fill out the state and input values from Problem C and look at the resulting plot. The plot should help you to visualize the difference between using a linear model and a nonlinear model for this specific starting state and input." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "x_init = 5.0\n", "y_init = 10.0\n", "theta_init = 0.0\n", "v_init = 2.0\n", "a_input = 1.0\n", "phi_input = 0.5\n", "\n", "state_init = [x_init, y_init, theta_init, v_init]\n", "state_predictions_nonlinear = nonlinear_vehicle_model(state_init, [a_input, phi_input], 10)\n", "state_predictions_linear = linear_vehicle_model(A, B, state_init, [a_input, phi_input], 10)\n", "\n", "make_model_comparison_plot(state_predictions_nonlinear, state_predictions_linear)\n", "\n", "\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Problem 3: Image Stitching" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### [Optional] Visualization for Part (a) " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "''' Plotting Setup'''\n", "''' Please run this cell before moving to the following cells'''\n", "def transform_the_polygon_1(polygon, R): \n", " transformed_polygon = []\n", " for point in polygon:\n", " transformed_point = np.dot(R, point)\n", " transformed_polygon.append(transformed_point)\n", " return transformed_polygon\n", "def transform_the_polygon_2(polygon, t): \n", " transformed_polygon = []\n", " for point in polygon:\n", " transformed_point = point+t\n", " transformed_polygon.append(transformed_point)\n", " return transformed_polygon\n", "def plot_the_polygon(polygon, title):\n", " fig = plt.figure(figsize=(5,5))\n", " ax = fig.add_subplot(111, xlim = [-5, 5], ylim = [-5, 5])\n", " ax.grid(True)\n", " ax.axhline(y=0, color='k', linestyle = '--', linewidth = 1)\n", " ax.axvline(x=0, color='k', linestyle = '--', linewidth = 1)\n", " for i in range(len(polygon) - 1):\n", " ax.plot([polygon[i], polygon[i+1]],\n", " [polygon[i], polygon[i+1]], linewidth=2, linestyle='dashed')\n", " arrow = mpatches.FancyArrowPatch((0, 0), (polygon[i], polygon[i]), mutation_scale=15, ec ='blue')\n", " ax.add_patch(arrow)\n", " ax.plot([polygon[i+1], polygon], [polygon[i+1], polygon], linewidth=2, linestyle='dashed')\n", " arrow = mpatches.FancyArrowPatch((0, 0), (polygon[i+1], polygon[i+1]), mutation_scale=15, ec ='blue')\n", " ax.add_patch(arrow)\n", " ax.set_title(title)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Step 1:\n", "We are trying to visualize how $\\begin{bmatrix} 2 & 2 \\\\ -2 & 2 \\end{bmatrix}$ is transforming $\\vec{u}$. We have chosen four different values of $\\vec{u}$: $\\begin{bmatrix} 1 \\\\ 1 \\end{bmatrix}$, $\\begin{bmatrix} 1 \\\\ -1 \\end{bmatrix}$, $\\begin{bmatrix} -1 \\\\ -1 \\end{bmatrix}$, and $\\begin{bmatrix} -1 \\\\ 1 \\end{bmatrix}$. These four vectors are plotted in the \"$\\textbf{Original Vectors}$\" plot.\n", "\n", "Then we transform $\\vec{u}$ with $\\begin{bmatrix} 2 & 2 \\\\ -2 & 2 \\end{bmatrix}$, i.e. we find $\\vec{v_1}=\\begin{bmatrix} 2 & 2 \\\\ -2 & 2 \\end{bmatrix}\\vec{u}$ for all four values of $\\vec{u}$. These four transformed vectors are shown in the \"$\\textbf{Step 1 Transformation}$\" plot.\n", "\n", "### Step 2:\n", "Now we visualize how addition of $\\vec{w}=\\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix}$ geometrically transforms $\\vec{v_1}$. We utilize the four different values of $\\vec{v_1}$ from Step 1.\n", "\n", "Then we find $\\vec{v}=\\vec{v_1}+\\vec{w}$ for all four values of $\\vec{v_1}$. These four transformed vectors are shown in the \"$\\textbf{Step 2 Transformation}$\" plot.\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "import matplotlib.patches as mpatches\n", "\n", "sample_img = [np.array([1,1]), np.array([1,-1]), np.array([-1, -1]), np.array([-1,1])]\n", "plot_the_polygon(sample_img, 'Original Vectors')\n", "plt.show()\n", "\n", "A = np.array([[2, 2],\n", " [-2, 2]])\n", "transformed_img1 = transform_the_polygon_1(sample_img, A)\n", "plot_the_polygon(transformed_img1, 'Step 1 Transformation')\n", "\n", "w = np.array([0,1])\n", "transformed_img2 = transform_the_polygon_2(transformed_img1, w)\n", "plot_the_polygon(transformed_img2, 'Step 2 Transformation')\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### PART D" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "This section of the notebook continues the image stiching problem. Be sure to have the \"figures\" folder in the same directory. as the notebook. The folder should have the following files:\n", "\n", " Berkeley_banner_1.jpg\n", " Berkeley_banner_2.jpg\n", " stacked_pieces.jpg\n", " lefthalfpic.jpg\n", " righthalfpic.jpg\n", " \n", "Note: This structure is also present in the provided HW3 zip file." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Run the next block of code before proceeding\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "tags": [ "worksheet-0" ] }, "outputs": [], "source": [ "import numpy as np\n", "import numpy.matlib\n", "import matplotlib.pyplot as plt\n", "from mpl_toolkits.mplot3d import Axes3D\n", "from numpy import pi, cos, exp, sin\n", "import matplotlib.image as mpimg\n", "import matplotlib.transforms as mtransforms\n", "\n", "\n", "#%matplotlib inline\n", "\n", "#loading images\n", "image1=mpimg.imread('figures/Berkeley_banner_1.jpg')\n", "image1=image1/255.0\n", "image2=mpimg.imread('figures/Berkeley_banner_2.jpg')\n", "image2=image2/255.0\n", "image_stack=mpimg.imread('figures/stacked_pieces.jpg')\n", "image_stack=image_stack/255.0\n", "\n", "\n", "image1_marked=mpimg.imread('figures/lefthalfpic.jpg')\n", "image1_marked=image1_marked/255.0\n", "image2_marked=mpimg.imread('figures/righthalfpic.jpg')\n", "image2_marked=image2_marked/255.0\n", "\n", "def euclidean_transform_2to1(transform_mat,translation,image,position,LL,UL):\n", " new_position=np.round(transform_mat.dot(position)+translation)\n", " new_position=new_position.astype(int)\n", "\n", " \n", " if (new_position>=LL).all() and (new_position=LL).all() and (new_position 0.995 and image1[row,col,1] > 0.995 and image1[row,col,2] > 0.995:\n", " temp = euclidean_transform_2to1(matrix_transform,translation,image2,position,LL,UL)\n", " image_rec[row,col,:] = temp\n", " else:\n", " image_rec[row,col,:] = image1[row,col,:]\n", " \n", "\n", "plt.figure(figsize=(20,20))\n", "plt.imshow(image_rec)\n", "plt.axis('on')\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Part E: Failure Mode Points [Optional]\n", "\n", "\\begin{align} \n", "&&\\vec{p_1}=\\begin{bmatrix} 390 \\\\ 660 \\end{bmatrix}\n", "&&\\vec{p_2}=\\begin{bmatrix} 425 \\\\ 645 \\end{bmatrix}\n", "&&\\vec{p_3}=\\begin{bmatrix} 460 \\\\ 630 \\end{bmatrix}\\\\\n", "&&\\vec{q_1}=\\begin{bmatrix} 385 \\\\ 450 \\end{bmatrix}\n", "&&\\vec{q_2}=\\begin{bmatrix} 425 \\\\ 480 \\end{bmatrix}\n", "&&\\vec{q_3}=\\begin{bmatrix} 465 \\\\ 510 \\end{bmatrix}\n", "\\end{align}\n", "\n", "$\\textbf{This part will show errors, since the choice of these three points makes the columns linearly dependent.}$ So the approach of solving this problem will work for some sets of points, but will fail for other choices. Therefore, we need to pick common pairs of points which make the columns linearly independent." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Note that the following is a general template for solving for 6 unknowns from 6 equations represented as Az = b.\n", "# You do not have to use the following code exactly. \n", "# All you need to do is to find parameters r_xx, r_xy, r_yx, r_yy, t_x, t_y. \n", "# If you prefer finding them another way it is fine.\n", "\n", "# fill in the entries\n", "A = np.array([[390,660,0,0,1,0],\n", " [0,0,390,660,0,1],\n", " [425,645,0,0,1,0],\n", " [0,0,425,645,0,1],\n", " [460,630,0,0,1,0],\n", " [0,0,460,630,0,1]])\n", "\n", "# fill in the entries\n", "b = np.array([,,,,,])\n", "\n", "A = A.astype(float)\n", "b = b.astype(float)\n", "\n", "# solve the linear system for the coefficiens\n", "z = np.linalg.solve(A,b)\n", "\n", "#Parameters for our transformation\n", "r_xx = z[0,0]\n", "r_xy = z[1,0]\n", "r_yx = z[2,0]\n", "r_yy = z[3,0]\n", "t_x = z[4,0]\n", "t_y = z[5,0]" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "anaconda-cloud": {}, "kernel_info": { "name": "python3" }, "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.8.5" }, "name": "graphs_for_SOE.ipynb", "nteract": { "version": "0.11.6" } }, "nbformat": 4, "nbformat_minor": 1 }