314 lines
13 KiB
Plaintext
314 lines
13 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# SmartProjectionFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"`SmartProjectionFactor<CAMERA>` is a \"smart\" factor designed for Structure from Motion (SfM) or visual SLAM problems where **both camera poses and calibration are being optimized**.\n",
|
|
"It implicitly represents a 3D point landmark that has been observed by multiple cameras.\n",
|
|
"\n",
|
|
"Key characteristics:\n",
|
|
"- **Implicit Point:** The 3D point is not an explicit variable in the factor graph. Instead, the factor internally triangulates the point based on the current camera estimates whenever needed (e.g., during linearization or error calculation).\n",
|
|
"- **Marginalization:** When linearized (e.g., to a Hessian factor), it effectively marginalizes out the 3D point, creating a dense factor connecting only the cameras that observed the point.\n",
|
|
"- **`CAMERA` Template:** This template parameter must represent a camera model that includes *both* pose and calibration (e.g., `PinholeCameraCal3_S2`, `PinholeCameraBundler`).\n",
|
|
"- **`Values` Requirement:** When using this factor, the `Values` object passed to methods like `error` or `linearize` must contain `CAMERA` objects (not separate `Pose3` and `Calib` objects) associated with the factor's keys.\n",
|
|
"- **Configuration:** Its behavior (linearization method, handling of degenerate triangulations) is controlled by `SmartProjectionParams`.\n",
|
|
"\n",
|
|
"**Use Case:** Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses.\n",
|
|
"**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {
|
|
"id": "pip_code",
|
|
"tags": [
|
|
"remove-cell"
|
|
]
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"try:\n",
|
|
" import google.colab\n",
|
|
" %pip install --quiet gtsam-develop\n",
|
|
"except ImportError:\n",
|
|
" pass # Not running on Colab, do nothing"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {
|
|
"id": "imports_code"
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"import gtsam\n",
|
|
"import numpy as np\n",
|
|
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n",
|
|
" SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n",
|
|
" PinholeCameraCal3_S2, Cal3_S2)\n",
|
|
"from gtsam import symbol_shorthand\n",
|
|
"import graphviz\n",
|
|
"\n",
|
|
"C = symbol_shorthand.C # Key for Camera object"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_header_md"
|
|
},
|
|
"source": [
|
|
"## Creating and Adding Measurements"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_desc_md"
|
|
},
|
|
"source": [
|
|
"1. Create the factor with a noise model and parameters.\n",
|
|
"2. Add measurements (2D points) and the corresponding camera keys one by one or in batches."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {
|
|
"id": "create_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"ename": "AttributeError",
|
|
"evalue": "'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
|
|
"\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)",
|
|
"Cell \u001b[1;32mIn[3], line 8\u001b[0m\n\u001b[0;32m 5\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Add measurements and keys\u001b[39;00m\n\u001b[1;32m----> 8\u001b[0m \u001b[43msmart_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43madd\u001b[49m(Point2(\u001b[38;5;241m150\u001b[39m, \u001b[38;5;241m505\u001b[39m), C(\u001b[38;5;241m0\u001b[39m))\n\u001b[0;32m 9\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m470\u001b[39m, \u001b[38;5;241m495\u001b[39m), C(\u001b[38;5;241m1\u001b[39m))\n\u001b[0;32m 10\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m480\u001b[39m, \u001b[38;5;241m150\u001b[39m), C(\u001b[38;5;241m2\u001b[39m))\n",
|
|
"\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
|
|
"smart_params = SmartProjectionParams() # Use default params (HESSIAN, IGNORE_DEGENERACY)\n",
|
|
"\n",
|
|
"# Factor type includes the Camera type, e.g., SmartProjectionFactorPinholeCameraCal3_S2\n",
|
|
"smart_factor = SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n",
|
|
"\n",
|
|
"# Add measurements and keys\n",
|
|
"smart_factor.add(Point2(150, 505), C(0))\n",
|
|
"smart_factor.add(Point2(470, 495), C(1))\n",
|
|
"smart_factor.add(Point2(480, 150), C(2))\n",
|
|
"\n",
|
|
"print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n",
|
|
"smart_factor.print(\"SmartFactor: \")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "eval_header_md"
|
|
},
|
|
"source": [
|
|
"## Evaluating the Error"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "eval_desc_md"
|
|
},
|
|
"source": [
|
|
"The `.error(values)` method implicitly triangulates the point based on the `CAMERA` objects in `values` and computes the sum of squared reprojection errors."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {
|
|
"id": "eval_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Triangulated point result:\n",
|
|
"Status.DEGENERATE\n",
|
|
"\n",
|
|
"Triangulation failed, error calculation depends on degeneracyMode.\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"# Create Values containing CAMERA objects\n",
|
|
"values = Values()\n",
|
|
"K = Cal3_S2(500, 500, 0, 320, 240)\n",
|
|
"pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n",
|
|
"pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n",
|
|
"pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n",
|
|
"\n",
|
|
"values.insert(C(0), PinholeCameraCal3_S2(pose0, K))\n",
|
|
"values.insert(C(1), PinholeCameraCal3_S2(pose1, K))\n",
|
|
"values.insert(C(2), PinholeCameraCal3_S2(pose2, K))\n",
|
|
"\n",
|
|
"# Triangulate first to see the implicit point\n",
|
|
"point_result = smart_factor.point(values)\n",
|
|
"print(f\"Triangulated point result:\\n{point_result.status}\")\n",
|
|
"\n",
|
|
"if point_result.valid():\n",
|
|
" # Calculate error\n",
|
|
" total_error = smart_factor.error(values)\n",
|
|
" print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
|
|
"else:\n",
|
|
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "linearize_header_md"
|
|
},
|
|
"source": [
|
|
"## Linearization"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "linearize_desc_md"
|
|
},
|
|
"source": [
|
|
"The `.linearize(values)` method creates a linear factor (e.g., `RegularHessianFactor`) connecting the camera variables, effectively marginalizing the implicit point."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {
|
|
"id": "linearize_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"\n",
|
|
"Linearized Factor (showing HessianFactor structure):\n",
|
|
"RegularHessianFactor(9): density=100% keys={ C0 C1 C2 }\n",
|
|
"Augmented information matrix: (dimensions: 9, 9, 9) : \n",
|
|
"{\n",
|
|
"\t[ 4.383e+04, 2.596e+04, -1.466e+04, -6.732e+03, 1.357e+04, -4.327e+03, -2.004e+04, -1.143e+03, -3.459e+03; ]\n",
|
|
"\t[ 2.596e+04, 6.879e+04, 5.416e+03, 3.589e+03, -1.075e+04, 2.158e+04, 3.107e+04, -8.167e+03, -1.237e+04; ]\n",
|
|
"\t[ -1.466e+04, 5.416e+03, 2.336e+04, -1.026e+03, -3.572e+03, -1.124e+04, -1.497e+04, -5.631e+03, 4.149e+03; ]\n",
|
|
"\t[ -6.732e+03, 3.589e+03, -1.026e+03, 7.027e+03, 1.434e+03, 3.742e+03, 2.527e+03, 2.454e+03, -6.619e+03; ]\n",
|
|
"\t[ 1.357e+04, -1.075e+04, -3.572e+03, 1.434e+03, 1.511e+04, -8.935e+02, -1.484e+04, 3.811e+03, -1.993e+03; ]\n",
|
|
"\t[ -4.327e+03, 2.158e+04, -1.124e+04, 3.742e+03, -8.935e+02, 2.587e+04, 2.085e+04, -1.193e+04, -5.818e+03; ]\n",
|
|
"\t[ -2.004e+04, 3.107e+04, -1.497e+04, 2.527e+03, -1.484e+04, 2.085e+04, 3.128e+04, -5.349e+03, -6.557e+03; ]\n",
|
|
"\t[ -1.143e+03, -8.167e+03, -5.631e+03, 2.454e+03, 3.811e+03, -1.193e+04, -5.349e+03, 1.537e+04, -1.987e+03; ]\n",
|
|
"\t[ -3.459e+03, -1.237e+04, 4.149e+03, -6.619e+03, -1.993e+03, -5.818e+03, -6.557e+03, -1.987e+03, 1.043e+04; ]\n",
|
|
"}\n",
|
|
"Augmented Diagonal Block [0,0]:\n",
|
|
"[ 4.383e+04, 2.596e+04, -1.466e+04; ]\n",
|
|
"[ 2.596e+04, 6.879e+04, 5.416e+03; ]\n",
|
|
"[ -1.466e+04, 5.416e+03, 2.336e+04; ]\n",
|
|
"\n",
|
|
"Augmented Diagonal Block [1,1]:\n",
|
|
"[ 7.027e+03, 1.434e+03, 3.742e+03; ]\n",
|
|
"[ 1.434e+03, 1.511e+04, -8.935e+02; ]\n",
|
|
"[ 3.742e+03, -8.935e+02, 2.587e+04; ]\n",
|
|
"\n",
|
|
"Augmented Diagonal Block [2,2]:\n",
|
|
"[ 3.128e+04, -5.349e+03, -6.557e+03; ]\n",
|
|
"[ -5.349e+03, 1.537e+04, -1.987e+03; ]\n",
|
|
"[ -6.557e+03, -1.987e+03, 1.043e+04; ]\n",
|
|
"\n",
|
|
"Off-Diagonal Block [0,1]:\n",
|
|
"[ -6.732e+03, 1.357e+04, -4.327e+03; ]\n",
|
|
"[ 3.589e+03, -1.075e+04, 2.158e+04; ]\n",
|
|
"[ -1.026e+03, -3.572e+03, -1.124e+04; ]\n",
|
|
"\n",
|
|
"Off-Diagonal Block [0,2]:\n",
|
|
"[ -2.004e+04, -1.143e+03, -3.459e+03; ]\n",
|
|
"[ 3.107e+04, -8.167e+03, -1.237e+04; ]\n",
|
|
"[ -1.497e+04, -5.631e+03, 4.149e+03; ]\n",
|
|
"\n",
|
|
"Off-Diagonal Block [1,2]:\n",
|
|
"[ 2.527e+03, 2.454e+03, -6.619e+03; ]\n",
|
|
"[ -1.484e+04, 3.811e+03, -1.993e+03; ]\n",
|
|
"[ 2.085e+04, -1.193e+04, -5.818e+03; ]\n",
|
|
"\n",
|
|
"Error vector:\n",
|
|
"[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n",
|
|
"Constant error term: 103876\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"graph = NonlinearFactorGraph()\n",
|
|
"graph.add(smart_factor)\n",
|
|
"\n",
|
|
"# Linearize (default mode is HESSIAN)\n",
|
|
"linear_factor = smart_factor.linearize(values)\n",
|
|
"\n",
|
|
"if linear_factor:\n",
|
|
" print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n",
|
|
" # Cast to HessianFactor to print its details\n",
|
|
" hessian_factor = gtsam.RegularHessianFactorCal3_S2.Downcast(linear_factor)\n",
|
|
" if hessian_factor:\n",
|
|
" hessian_factor.print()\n",
|
|
" else:\n",
|
|
" print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n",
|
|
"else:\n",
|
|
" print(\"Linearization failed (likely due to triangulation degeneracy)\")"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "gtsam",
|
|
"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.13.1"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 0
|
|
}
|