{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Quaternion-Based Camera Calibration Tutorial\n", "\n", "In this tutorial, we will learn how to perform camera calibration using quaternions to represent the rotation part of the camera’s extrinsic parameters. Quaternions provide a compact and singularity‐free representation of rotations, which can be advantageous when optimizing the calibration parameters.\n", "\n", "**Outline:**\n", "1. [Introduction](#Introduction)\n", "2. [Background on Quaternions](#Background-on-Quaternions)\n", "3. [Camera Projection Model](#Camera-Projection-Model)\n", "4. [Synthetic Data Generation](#Synthetic-Data-Generation)\n", "5. [Formulating the Calibration Problem](#Formulating-the-Calibration-Problem)\n", "6. [Optimization and Estimation](#Optimization-and-Estimation)\n", "7. [Results and Visualization](#Results-and-Visualization)\n", "8. [Conclusion](#Conclusion)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n", "\"drawing\"" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Introduction\n", "\n", "Camera calibration is the process of determining the parameters of the camera model. In particular, we are interested in the intrinsic parameters (focal length, principal point, etc.) and extrinsic parameters (rotation and translation that relate the world coordinate system to the camera coordinate system).\n", "\n", "In this tutorial, we use **quaternions** to parameterize the rotation. This approach avoids some of the pitfalls (like gimbal lock) associated with Euler angles and often leads to more robust optimization.\n", "\n", "We will:\n", "- Review the necessary quaternion mathematics.\n", "- Define the camera projection model.\n", "- Generate synthetic calibration data.\n", "- Formulate a reprojection error cost function.\n", "- Use optimization (e.g., SciPy’s `least_squares`) to recover the calibration parameters.\n", "- Visualize the calibration results." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Background on Quaternions\n", "\n", "A quaternion is a four-dimensional vector $ q = [q_0, q_1, q_2, q_3] $ that can represent a rotation in 3D space. For a unit quaternion, the following holds:\n", "\n", "$$\n", "\\|q\\| = \\sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2} = 1\n", "$$\n", "\n", "Key steps include:\n", "- **Normalization:** Ensuring the quaternion is of unit length.\n", "- **Conversion to Rotation Matrix:** Converting the quaternion into a 3×3 rotation matrix that can be used in the camera model.\n", "\n", "\n", "#### Quaternion from an Axis and an Angle\n", "\n", "If you want to represent a rotation about a unit axis $ \\mathbf{u} = (x, y, z) $ by an angle $\\alpha$, the corresponding unit quaternion is given by:\n", "$$\n", "q = \\left[\\cos\\left(\\frac{\\alpha}{2}\\right),\\, x\\sin\\left(\\frac{\\alpha}{2}\\right),\\, y\\sin\\left(\\frac{\\alpha}{2}\\right),\\, z\\sin\\left(\\frac{\\alpha}{2}\\right)\\right].\n", "$$\n", "- $ \\cos\\left(\\frac{\\alpha}{2}\\right) $ is the scalar (real) part $ q_0 $.\n", "- $ x\\sin\\left(\\frac{\\alpha}{2}\\right),\\, y\\sin\\left(\\frac{\\alpha}{2}\\right),\\, z\\sin\\left(\\frac{\\alpha}{2}\\right) $ form the vector (imaginary) part $(q_1, q_2, q_3)$.\n", "\n", "*Note:* The axis $ \\mathbf{u} $ must be normalized (i.e., $ \\sqrt{x^2+y^2+z^2}=1 $) before applying the formula.\n", "\n", "\n", "### Converting a Quaternion to a Rotation Matrix\n", "\n", "Given a unit quaternion \n", "$$\n", "q = [q_0, q_1, q_2, q_3],\n", "$$\n", "where $q_0$ is the scalar (real) part and $[q_1, q_2, q_3]$ is the vector (imaginary) part, we can derive the corresponding 3×3 rotation matrix $R$ using the following formula:\n", "\n", "$$\n", "R = \\begin{bmatrix}\n", "1 - 2\\,(q_2^2 + q_3^2) & 2\\,(q_1q_2 - q_0q_3) & 2\\,(q_1q_3 + q_0q_2) \\\\\n", "2\\,(q_1q_2 + q_0q_3) & 1 - 2\\,(q_1^2 + q_3^2) & 2\\,(q_2q_3 - q_0q_1) \\\\\n", "2\\,(q_1q_3 - q_0q_2) & 2\\,(q_2q_3 + q_0q_1) & 1 - 2\\,(q_1^2 + q_2^2)\n", "\\end{bmatrix}.\n", "$$\n", "\n", "This matrix $R$ rotates a 3D vector according to the rotation encoded by the quaternion $q$." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from scipy.optimize import least_squares" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "\n", "\n", "def normalize_quaternion(q):\n", " \"\"\"Normalize a quaternion to unit length.\"\"\"\n", " return q / np.linalg.norm(q)\n", "\n", "def quaternion_to_rotation_matrix(q):\n", " \"\"\"Convert a normalized quaternion to a 3x3 rotation matrix.\n", " \n", " The quaternion should be in the form [q0, q1, q2, q3] with q0 as the scalar part.\n", " \"\"\"\n", " q = normalize_quaternion(q)\n", " q0, q1, q2, q3 = q\n", " # TODO: Compute the rotation matrix elements using the quaternion components.\n", " R = # YOUR CODE HERE\n", " return R\n", "\n", "# Example usage:\n", "q_example = np.array([0.95, 0.1, 0.1, 0.1])\n", "print(\"Normalized quaternion:\", normalize_quaternion(q_example))\n", "print(\"Rotation matrix:\\n\", quaternion_to_rotation_matrix(q_example))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Camera Projection Model\n", "\n", "The pinhole camera model is typically expressed as:\n", "\n", "$$\n", "\\mathbf{p} = \\mathbf{K} \\, [\\mathbf{R} \\, | \\, \\mathbf{t}] \\, \\mathbf{P}\n", "$$\n", "\n", "where:\n", "- **$\\mathbf{P}$** is a 3D point in the world (expressed in homogeneous coordinates).\n", "- **$\\mathbf{R}$** is the rotation matrix (which we obtain from the quaternion).\n", "- **$\\mathbf{t}$** is the translation vector.\n", "- **$\\mathbf{K}$** is the intrinsic calibration matrix.\n", "- **$\\mathbf{p}$** is the projected 2D point in the image (also in homogeneous coordinates).\n", "\n", "In our implementation, we will combine the rotation (via quaternion) and translation into the extrinsic parameters.\n", "\n", "\n", "\n", "The intrinsic matrix **$\\mathbf{K}$** encapsulates the internal parameters of the camera that are independent of the external world. It is a 3×3 matrix that transforms the camera’s 3D coordinate system (after applying the extrinsic parameters) into 2D pixel coordinates. The intrinsic matrix is typically represented as:\n", "\n", "$$\n", "\\mathbf{K} = \\begin{bmatrix}\n", "f_x & s & c_x \\\\\n", "0 & f_y & c_y \\\\\n", "0 & 0 & 1 \\\\\n", "\\end{bmatrix}\n", "$$\n", "\n", "where:\n", "- **$f_x$ and $f_y$**: These represent the focal lengths along the $x$ and $y$ axes, respectively, usually measured in pixels. They determine how strongly the camera converges or diverges light rays onto the image plane.\n", "- **$c_x$ and $c_y$**: These are the coordinates of the principal point, which is typically near the center of the image. The principal point is where the optical axis intersects the image plane.\n", "- **$s$**: The skew coefficient accounts for any non-orthogonality between the image axes. In most modern cameras, the sensor’s axes are perpendicular, so this term is often zero.\n", "\n", "The intrinsic matrix plays a crucial role in the projection process:\n", "- **Scaling:** The focal lengths $f_x$ and $f_y$ scale the normalized image coordinates into pixel units.\n", "- **Translation:** The principal point $(c_x, c_y)$ translates the origin of the normalized coordinates to the center (or another reference point) of the image.\n", "- **Skew:** The skew parameter $s$ adjusts for any non-rectangular pixel grids, though it is typically zero in most camera systems.\n", "\n", "Thus, after the extrinsic transformation $$\\mathbf{R} \\, | \\, \\mathbf{t}$$ has mapped a 3D world point $\\mathbf{P}$ into the camera coordinate system, the intrinsic matrix $\\mathbf{K}$ projects these coordinates onto the 2D image plane by accounting for the camera’s internal characteristics.\n", "\n", "In summary, the intrinsic matrix is essential for converting 3D camera coordinates into 2D pixel coordinates, effectively linking the geometry of the scene to the imaging sensor’s properties." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def project_points(P, q, t, K):\n", " \"\"\"\n", " Projects 3D points P (Nx3) into the image using a rotation (given as quaternion q),\n", " translation vector t, and camera intrinsic matrix K.\n", " \n", " Parameters:\n", " P : ndarray of shape (N, 3)\n", " 3D world points.\n", " q : ndarray of shape (4,)\n", " Quaternion representing rotation.\n", " t : ndarray of shape (3,)\n", " Translation vector.\n", " K : ndarray of shape (3, 3)\n", " Camera intrinsic matrix.\n", " \n", " Returns:\n", " projected_points : ndarray of shape (N, 2)\n", " 2D points in the image.\n", " \"\"\"\n", " R = quaternion_to_rotation_matrix(q)\n", " # TODO: Convert 3D points P to homogeneous coordinates by appending a column of ones.\n", " P_homog = # YOUR CODE HERE\n", " \n", " # TODO: Create the extrinsic matrix [R | t] by combining R and t.\n", " RT = # YOUR CODE HERE\n", " \n", " # TODO: Project the 3D points to 2D using the intrinsic matrix K and the extrinsic matrix [R | t].\n", " projected_homog = # YOUR CODE HERE\n", " \n", " # Normalize homogeneous coordinates\n", " projected = projected_homog[:2, :] / projected_homog[2, :]\n", " return projected.T\n", "\n", "# Example intrinsic matrix (assuming fx=fy=800, principal point at (320, 240))\n", "K_example = np.array([[800, 0, 320],\n", " [ 0, 800, 240],\n", " [ 0, 0, 1]])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Synthetic Data Generation\n", "\n", "To test our calibration procedure, we can generate synthetic data:\n", "- Create a set of 3D world points (e.g., points on a calibration grid).\n", "- Choose known ground-truth camera parameters (intrinsics, a quaternion for rotation, and translation).\n", "- Project the 3D points into the image using our camera model.\n", "- Optionally add noise to simulate measurement errors." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def generate_synthetic_data(num_points=50, noise_std=1.0):\n", " # TODO: Generate random 3D points P within a desired range.\n", " P = # YOUR CODE HERE\n", " \n", " # Define ground-truth parameters\n", " q_true = normalize_quaternion(np.array([0.9, 0.2, 0.1, 0.1]))\n", " t_true = np.array([0.5, -0.3, 3.0])\n", " \n", " # TODO: Project the 3D points P to 2D using the ground-truth parameters.\n", " pts_2d = # YOUR CODE HERE\n", " \n", " # TODO: Add Gaussian noise to the projected points to simulate measurement error.\n", " pts_2d_noisy = pts_2d + # YOUR CODE HERE\n", " \n", " return P, pts_2d_noisy, q_true, t_true\n", "\n", "# Generate the synthetic data\n", "P_world, pts_image, q_true, t_true = generate_synthetic_data()\n", "\n", "\n", "\n", "plt.figure(figsize=(6,6))\n", "plt.scatter(pts_image[:,0], pts_image[:,1], c='blue', label='Noisy Image Points')\n", "plt.title(\"Synthetic Image Points\")\n", "plt.xlabel(\"u (pixels)\")\n", "plt.ylabel(\"v (pixels)\")\n", "plt.legend()\n", "plt.gca().invert_yaxis() # Invert y-axis if needed (image coordinates)\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Formulating the Calibration Problem\n", "\n", "We now define a cost function that computes the **reprojection error** between the observed 2D points and the 2D points obtained by projecting the 3D world points with our estimated camera parameters.\n", "\n", "The parameter vector to optimize can include:\n", "- The quaternion $ q $ (with 4 parameters; normalization is enforced either explicitly or via a penalty/constraint).\n", "- The translation vector $ t $ (3 parameters).\n", "\n", "For simplicity, in this tutorial we assume that the intrinsic matrix $ K $ is known." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "\n", "\n", "def reprojection_error(params, P, pts_obs, K):\n", " \"\"\"\n", " Compute the reprojection error.\n", " \n", " Parameters:\n", " params : ndarray of shape (7,)\n", " First 4 elements are the quaternion, last 3 are the translation vector.\n", " P : ndarray of shape (N, 3)\n", " 3D world points.\n", " pts_obs : ndarray of shape (N, 2)\n", " Observed 2D image points.\n", " K : ndarray of shape (3, 3)\n", " Camera intrinsic matrix.\n", " \n", " Returns:\n", " error: Flattened reprojection error vector.\n", " \"\"\"\n", " q = params[:4]\n", " t = params[4:]\n", " \n", " # Ensure the quaternion is normalized\n", " q = normalize_quaternion(q)\n", " \n", " pts_proj = project_points(P, q, t, K)\n", " error = (pts_proj - pts_obs).ravel()\n", " return error\n", "\n", "# Test the error function with ground truth parameters\n", "params_gt = np.hstack((q_true, t_true))\n", "error_test = reprojection_error(params_gt, P_world, pts_image, K_example)\n", "print(\"Reprojection error (ground truth):\", np.linalg.norm(error_test))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Optimization and Estimation\n", "\n", "We now solve the calibration problem by minimizing the reprojection error using an optimization routine. We will use `scipy.optimize.least_squares` to recover the quaternion and translation vector." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# TODO: Provide an initial guess for the calibration parameters (perturb the ground truth).\n", "q_initial = # YOUR CODE HERE\n", "t_initial = # YOUR CODE HERE\n", "params_initial = np.hstack((q_initial, t_initial))\n", "\n", "# Run the optimization\n", "result = least_squares(reprojection_error, params_initial, args=(P_world, pts_image, K_example))\n", "\n", "params_est = result.x\n", "q_est = normalize_quaternion(params_est[:4])\n", "t_est = params_est[4:]\n", "print(\"Estimated quaternion:\", q_est)\n", "print(\"Estimated translation:\", t_est)\n", "\n", "# Compare with ground truth\n", "print(\"Ground truth quaternion:\", q_true)\n", "print(\"Ground truth translation:\", t_true)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Results and Visualization\n", "\n", "After the optimization, we can visualize how well the estimated parameters reproject the 3D points compared to the noisy observations. A good calibration will show a low reprojection error and a close match between the estimated and ground truth parameters." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Compute reprojected points using the estimated parameters\n", "pts_proj_est = project_points(P_world, q_est, t_est, K_example)\n", "\n", "plt.figure(figsize=(6,6))\n", "plt.scatter(pts_image[:,0], pts_image[:,1], c='blue', marker='x', label='Observed Points')\n", "plt.scatter(pts_proj_est[:,0], pts_proj_est[:,1], c='red', marker='o', facecolors='none', label='Reprojected Points')\n", "plt.title(\"Reprojection Comparison\")\n", "plt.xlabel(\"u (pixels)\")\n", "plt.ylabel(\"v (pixels)\")\n", "plt.legend()\n", "plt.gca().invert_yaxis()\n", "plt.show()\n", "\n", "# Compute final reprojection error\n", "final_error = np.linalg.norm(reprojection_error(params_est, P_world, pts_image, K_example))\n", "print(\"Final reprojection error:\", final_error)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Conclusion\n", "\n", "In this tutorial, we demonstrated a simple calibration procedure that uses quaternions to represent the rotation in a camera model. We:\n", "- Reviewed the necessary quaternion math.\n", "- Formulated the camera projection model.\n", "- Generated synthetic calibration data.\n", "- Defined and minimized a reprojection error cost function.\n", "- Compared the estimated calibration parameters with the ground truth.\n", "\n", "This framework can be extended to real data, include optimization over intrinsic parameters, or incorporate additional constraints. Experiment further to handle more complex calibration scenarios and noise models." ] } ], "metadata": { "kernelspec": { "display_name": "3.11.10", "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.10" } }, "nbformat": 4, "nbformat_minor": 2 }