Example_pixel_to_robot_coordinate.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "b8382752-97bc-4c0f-9d18-ee6f3e4b20dc",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Roboterkoordinate: (15.000000000000007, 30.000000000000014)\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "\n",
    "def pixel_to_robot_transform(pixel_points, robot_points):\n",
    "    \"\"\"\n",
    "    Berechnet eine affine Transformation basierend auf drei bekannten Punkten.\n",
    "\n",
    "    :param pixel_points: Liste mit drei Pixelkoordinaten [(x1, y1), (x2, y2), (x3, y3)]\n",
    "    :param robot_points: Liste mit drei Roboterkoordinaten [(X1, Y1), (X2, Y2), (X3, Y3)]\n",
    "    :return: Funktion, die eine Pixelkoordinate in eine Roboterkoordinate umwandelt\n",
    "    \"\"\"\n",
    "    # Erstellen der Matrizen für die Transformation\n",
    "    A = []\n",
    "    B = []\n",
    "\n",
    "    for (px, py), (rx, ry) in zip(pixel_points, robot_points):\n",
    "        A.append([px, py, 1, 0, 0, 0])\n",
    "        A.append([0, 0, 0, px, py, 1])\n",
    "        B.append(rx)\n",
    "        B.append(ry)\n",
    "\n",
    "    # Lösen des Gleichungssystems Ax = B\n",
    "    A = np.array(A)\n",
    "    B = np.array(B)\n",
    "    transform_params, _, _, _ = np.linalg.lstsq(A, B, rcond=None)\n",
    "\n",
    "    def transform(pixel_coord):\n",
    "        \"\"\"\n",
    "        Wandelt eine Pixelkoordinate in eine Roboterkoordinate um.\n",
    "\n",
    "        :param pixel_coord: Pixelkoordinate (x, y)\n",
    "        :return: Roboterkoordinate (X, Y)\n",
    "        \"\"\"\n",
    "        px, py = pixel_coord\n",
    "        rx = transform_params[0] * px + transform_params[1] * py + transform_params[2]\n",
    "        ry = transform_params[3] * px + transform_params[4] * py + transform_params[5]\n",
    "        return rx, ry\n",
    "\n",
    "    return transform\n",
    "\n",
    "# Beispielnutzung\n",
    "pixel_points = [(100, 200), (200, 400), (300, 600)]\n",
    "robot_points = [(10, 20), (20, 40), (30, 60)]\n",
    "\n",
    "# Funktion zur Umrechnung erstellen\n",
    "transform_function = pixel_to_robot_transform(pixel_points, robot_points)\n",
    "\n",
    "# Neue Pixelkoordinate in Roboterkoordinate umrechnen\n",
    "new_pixel = (150, 300)\n",
    "robot_coord = transform_function(new_pixel)\n",
    "print(\"Roboterkoordinate:\", robot_coord)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eb278d82-69a9-4651-aa21-044e33b6d1c3",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.11.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}