Coordinate Transformations_backup.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from icecream import ic\n",
    "from tracker_coordinate_transform import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "ename": "ValueError",
     "evalue": "all the input array dimensions except for the concatenation axis must match exactly, but along dimension 1, the array at index 0 has size 2 and the array at index 1 has size 3",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mValueError\u001b[0m                                Traceback (most recent call last)",
      "Cell \u001b[1;32mIn[3], line 9\u001b[0m\n\u001b[0;32m      6\u001b[0m dst_points \u001b[38;5;241m=\u001b[39m [(\u001b[38;5;241m0\u001b[39m, \u001b[38;5;241m0\u001b[39m),(\u001b[38;5;241m1.5\u001b[39m, \u001b[38;5;241m0\u001b[39m),(\u001b[38;5;241m0\u001b[39m, \u001b[38;5;241m1.5\u001b[39m)]\n\u001b[0;32m      8\u001b[0m \u001b[38;5;66;03m# Compute the transformation matrix\u001b[39;00m\n\u001b[1;32m----> 9\u001b[0m transformation_matrix \u001b[38;5;241m=\u001b[39m ic(\u001b[43mcompute_transformation_matrix\u001b[49m\u001b[43m(\u001b[49m\u001b[43msrc_points\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mdst_points\u001b[49m\u001b[43m)\u001b[49m)\n\u001b[0;32m     11\u001b[0m \u001b[38;5;66;03m# Transform a point (x, y)\u001b[39;00m\n\u001b[0;32m     12\u001b[0m point1 \u001b[38;5;241m=\u001b[39m (\u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.179\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m3.207\u001b[39m)\n",
      "File \u001b[1;32mc:\\Users\\dat40\\OneDrive\\Documents\\Github\\VR_NetworkTables2024\\tracker_coordinate_transform.py:36\u001b[0m, in \u001b[0;36mcompute_transformation_matrix\u001b[1;34m(source_points, target_points)\u001b[0m\n\u001b[0;32m     33\u001b[0m transformation_matrix \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mlinalg\u001b[38;5;241m.\u001b[39msolve(source_points, target_points)\n\u001b[0;32m     35\u001b[0m \u001b[38;5;66;03m# Append a row for homogeneous coordinates\u001b[39;00m\n\u001b[1;32m---> 36\u001b[0m transformation_matrix \u001b[38;5;241m=\u001b[39m \u001b[43mnp\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mvstack\u001b[49m\u001b[43m(\u001b[49m\u001b[43m[\u001b[49m\u001b[43mtransformation_matrix\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43m[\u001b[49m\u001b[38;5;241;43m0\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m0\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m]\u001b[49m\u001b[43m]\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m     37\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m transformation_matrix\n",
      "File \u001b[1;32mc:\\Users\\dat40\\.conda\\envs\\FRC\\Lib\\site-packages\\numpy\\_core\\shape_base.py:291\u001b[0m, in \u001b[0;36mvstack\u001b[1;34m(tup, dtype, casting)\u001b[0m\n\u001b[0;32m    289\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m \u001b[38;5;28misinstance\u001b[39m(arrs, \u001b[38;5;28mtuple\u001b[39m):\n\u001b[0;32m    290\u001b[0m     arrs \u001b[38;5;241m=\u001b[39m (arrs,)\n\u001b[1;32m--> 291\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[43m_nx\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mconcatenate\u001b[49m\u001b[43m(\u001b[49m\u001b[43marrs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m0\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mdtype\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mdtype\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcasting\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mcasting\u001b[49m\u001b[43m)\u001b[49m\n",
      "\u001b[1;31mValueError\u001b[0m: all the input array dimensions except for the concatenation axis must match exactly, but along dimension 1, the array at index 0 has size 2 and the array at index 1 has size 3"
     ]
    }
   ],
   "source": [
    "# Example points\n",
    "#src_points = [(0, 0), (1, 0), (0, 1)]\n",
    "#dst_points = [(2, 3), (3, 3), (2, 4)]\n",
    "\n",
    "src_points = [(1.073488, -2.271037),(-0.387351, -1.991891),(1.363704, -0.797259)]\n",
    "dst_points = [(0, 0),(1.5, 0),(0, 1.5)]\n",
    "\n",
    "# Compute the transformation matrix\n",
    "transformation_matrix = ic(compute_transformation_matrix(src_points, dst_points))\n",
    "\n",
    "# Transform a point (x, y)\n",
    "point1 = (-2.179, -3.207)\n",
    "point2 = (-0.242, -2.690)\n",
    "point3 = (-0.773, -0.784)\n",
    "point4 = (-1.0,-1.0)\n",
    "\n",
    "for point in [point1, point2, point3, point4]:\n",
    "    transformed_point = ic(transform_point(point, transformation_matrix))\n",
    "\n",
    "#print(f\"Transformed Point: {transformed_point}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "\n",
    "# Separate the points into x and y coordinates for plotting\n",
    "src_x, src_y = zip(*src_points)\n",
    "dst_x, dst_y = zip(*dst_points)\n",
    "\n",
    "# Create the plot\n",
    "plt.figure(figsize=(8, 6))\n",
    "plt.scatter(src_x, src_y, color='blue', label='Source Points')\n",
    "plt.scatter(dst_x, dst_y, color='red', label='Destination Points')\n",
    "\n",
    "# Add lines connecting corresponding points\n",
    "for i in range(len(src_points)):\n",
    "    plt.plot([src_points[i][0], dst_points[i][0]], [src_points[i][1], dst_points[i][1]], 'gray', linestyle='--')\n",
    "\n",
    "# Add labels and title\n",
    "plt.xlabel('X Coordinate')\n",
    "plt.ylabel('Y Coordinate')\n",
    "plt.title('Source and Destination Points')\n",
    "plt.legend()\n",
    "plt.grid(True)\n",
    "\n",
    "# Show the plot\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "def calculate_circle(points):\n",
    "    # Convert the list of points to a NumPy array\n",
    "    points = np.array(points)\n",
    "    \n",
    "    # Prepare the matrices for the least squares calculation\n",
    "    x = points[:, 0]\n",
    "    y = points[:, 1]\n",
    "    \n",
    "    # Calculate the coefficients using the least squares method\n",
    "    A = np.c_[x, y, np.ones(len(x))]\n",
    "    b = x**2 + y**2\n",
    "    \n",
    "    # Solve the equation A * [h, k, C] = b\n",
    "    coeffs = np.linalg.lstsq(A, b, rcond=None)[0]\n",
    "    \n",
    "    h, k, C = coeffs\n",
    "    # The radius is calculated from the center\n",
    "    r = np.sqrt(h**2 + k**2 + C)\n",
    "    \n",
    "    return h, k, r\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# Example array of points on the circle\n",
    "points = np.array([\n",
    "    [1.2685409784317017,-1.2855279445648193],\n",
    "    [0.4610433280467987, -1.4789505004882812],\n",
    "    [0.10663176327943802, -2.030184745788574]\n",
    "])\n",
    "\n",
    "h, k, r = calculate_circle(points)\n",
    "\n",
    "print(f\"Center of the circle: ({h:.2f}, {k:.2f})\")\n",
    "print(f\"Radius of the circle: {r:.2f}\")\n",
    "print(f\"Circle equation: (x - {h:.2f})^2 + (y - {k:.2f})^2 = {r**2:.2f}\")\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "newPoints = [(0.1172790378332138, -2.0815389156341553), (0.13003531098365784, -2.0311286449432373), (0.1446872055530548, -1.980210781097412), (0.16282092034816742, -1.9294254779815674), (0.1850621998310089, -1.8782325983047485), (0.20989999175071716, -1.8281124830245972), (0.23808307945728302, -1.777306079864502), (0.2717342972755432, -1.7264182567596436), (0.3104494512081146, -1.6757774353027344), (0.35316213965415955, -1.6251628398895264), (0.4011218249797821, -1.5748447179794312), (0.451546311378479, -1.5388946533203125), (0.4983119070529938, -1.4879955053329468), (0.5429137945175171, -1.4376658201217651), (0.5934383869171143, -1.4096862077713013), (0.6442127823829651, -1.3851996660232544), (0.6945139765739441, -1.3694506883621216), (0.745102047920227, -1.3547362089157104), (0.7955855131149292, -1.3332085609436035), (0.8465850353240967, -1.317476511001587), (0.8975532650947571, -1.3095701932907104), (0.9483062624931335, -1.3017194271087646), (0.998540997505188, -1.2962006330490112), (1.0496501922607422, -1.294082522392273), (1.1003813743591309, -1.296460509300232), (1.1507503986358643, -1.2991588115692139), (1.2007719278335571, -1.2985177040100098), (1.2507834434509277, -1.3123250007629395)]\n",
    "src_x, src_y = zip(*newPoints)\n",
    "\n",
    "\n",
    "# Create the plot\n",
    "plt.figure(figsize=(8, 6))\n",
    "plt.scatter(src_x, src_y, color='blue', label='Source Points')\n",
    "\n",
    "# Add lines connecting corresponding points\n",
    "for i in range(len(src_points)):\n",
    "    plt.plot([src_points[i][0], dst_points[i][0]], [src_points[i][1], dst_points[i][1]], 'gray', linestyle='--')\n",
    "\n",
    "# Add labels and title\n",
    "\n",
    "plt.xlabel('X Coordinate')\n",
    "plt.ylabel('Y Coordinate')\n",
    "plt.title('Source and Destination Points')\n",
    "plt.legend()\n",
    "plt.grid(True)\n",
    "\n",
    "# Show the plot\n",
    "plt.show()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first point in the array is (0,r) in FRC coordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import circle_fit as circle\n",
    "\n",
    "points = [(0.37510570883750916, -0.8342843651771545), (0.3248334228992462, -0.8295403718948364), (0.2743622362613678, -0.826815664768219), (0.22401097416877747, -0.8275769352912903), (0.17294949293136597, -0.8313215374946594), (0.12292873114347458, -0.8372471332550049), (0.07161729037761688, -0.8454130291938782), (0.01786799728870392, -0.8572217226028442), (-0.03314237296581268, -0.8717595338821411), (-0.08444882184267044, -0.8877468109130859), (-0.1356864869594574, -0.909177303314209), (-0.1858465075492859, -0.931339681148529), (-0.23588553071022034, -0.9617758989334106), (-0.286144495010376, -1.0021570920944214), (-0.3378433287143707, -1.047621726989746), (-0.3886015713214874, -1.0919877290725708), (-0.4402550458908081, -1.1180927753448486), (-0.48958367109298706, -1.1719499826431274), (-0.5303281545639038, -1.2228316068649292), (-0.5668140053749084, -1.2742542028427124), (-0.6018678545951843, -1.3261003494262695), (-0.6521179676055908, -1.4362637996673584), (-0.6704638600349426, -1.4875242710113525), (-0.688291072845459, -1.5409655570983887), (-0.7009204030036926, -1.5936079025268555), (-0.7120699286460876, -1.64723539352417), (-0.7195438146591187, -1.699013352394104), (-0.7226626873016357, -1.7497444152832031), (-0.7238384485244751, -1.8031448125839233), (-0.7230956554412842, -1.853276252746582), (-0.7182620763778687, -1.9036403894424438), (-0.7122184634208679, -1.9541934728622437), (-0.7006244659423828, -2.011301279067993), (-0.6879056692123413, -2.063897132873535), (-0.674543559551239, -2.1150455474853516), (-0.6561917066574097, -2.1685309410095215), (-0.6420400142669678, -2.2203590869903564), (-0.6245172619819641, -2.2724807262420654), (-0.6038203835487366, -2.325279474258423), (-0.5710437297821045, -2.3755252361297607), (-0.5205249786376953, -2.4249510765075684), (-0.4698893129825592, -2.4555160999298096), (-0.42012810707092285, -2.506366729736328), (-0.3690338730812073, -2.5524473190307617), (-0.3183332085609436, -2.58925724029541), (-0.26611652970314026, -2.6258749961853027), (-0.21426011621952057, -2.654890537261963), (-0.16310039162635803, -2.6799442768096924), (-0.1130872517824173, -2.70158052444458), (-0.06033956632018089, -2.7201483249664307), (-0.007142858579754829, -2.736077070236206), (0.04541667550802231, -2.749640464782715), (0.096783347427845, -2.7590763568878174), (0.14940038323402405, -2.764655351638794), (0.20081856846809387, -2.7684860229492188), (0.2514599561691284, -2.7703349590301514), (0.30442681908607483, -2.7689523696899414), (0.35688185691833496, -2.7641441822052), (0.40710821747779846, -2.756194829940796), (0.4572412967681885, -2.7465760707855225), (0.5080640316009521, -2.734013080596924), (0.5590968728065491, -2.7164647579193115), (0.6093403100967407, -2.6901438236236572), (0.6608644127845764, -2.6637051105499268), (0.7123205065727234, -2.6389217376708984), (0.7634175419807434, -2.612981081008911), (0.8140966296195984, -2.5843396186828613), (0.8642556071281433, -2.547771453857422), (0.9158142805099487, -2.507453441619873), (0.9576622843742371, -2.456725597381592), (1.0026576519012451, -2.4065487384796143), (1.0446642637252808, -2.355674982070923), (1.0769941806793213, -2.3046677112579346), (1.1121665239334106, -2.2537355422973633), (1.1342887878417969, -2.203183889389038), (1.156294584274292, -2.1523985862731934), (1.174515724182129, -2.101193428039551), (1.1899540424346924, -2.0509696006774902), (1.2030394077301025, -1.998249888420105), (1.2121891975402832, -1.9468461275100708), (1.220396637916565, -1.8967406749725342), (1.2249929904937744, -1.8452216386795044), (1.2259081602096558, -1.7935140132904053), (1.2241673469543457, -1.74313223361969), (1.221623182296753, -1.6927318572998047), (1.2108800411224365, -1.6426976919174194), (1.2041176557540894, -1.5926928520202637), (1.194579839706421, -1.5421632528305054), (1.1786274909973145, -1.4920830726623535), (1.1577316522598267, -1.4420266151428223), (1.1334922313690186, -1.3864538669586182), (1.1093885898590088, -1.3352971076965332), (1.0833134651184082, -1.284781813621521), (1.0501214265823364, -1.2347010374069214), (1.0109409093856812, -1.184482455253601), (0.9670594334602356, -1.1338047981262207), (0.9165676832199097, -1.083878993988037), (0.8657959699630737, -1.0387115478515625), (0.8151199817657471, -1.0030828714370728), (0.7638719081878662, -0.9683163166046143), (0.7132079601287842, -0.9434131979942322), (0.6623708605766296, -0.9125620722770691), (0.6112207174301147, -0.8934564590454102), (0.559902012348175, -0.8762336373329163), (0.509800374507904, -0.8607336282730103), (0.45836520195007324, -0.8442651033401489), (0.4077111482620239, -0.8379084467887878), (0.35752221941947937, -0.8308752775192261), (0.3063535988330841, -0.8252866864204407), (0.2535272240638733, -0.8263100385665894), (0.20282530784606934, -0.8290849328041077)]\n",
    "xc, yc, r, sigma = circle.standardLSQ(points)\n",
    "print(sigma)\n",
    "print(xc,yc,r)\n",
    "circle.plot_data_circle(points,xc,yc,r)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "circle.plot_data_circle(points,0,0,r)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "FRC",
   "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.12.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}