239 lines
7.9 KiB
Plaintext
239 lines
7.9 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# GenericProjectionFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"`GenericProjectionFactor<POSE, LANDMARK, CALIBRATION>` is a versatile factor for monocular camera measurements.\n",
|
|
"It models the reprojection error between the predicted projection of a 3D `LANDMARK` (usually `Point3`) onto the image plane of a camera defined by `POSE` (usually `Pose3`) and `CALIBRATION` (e.g., `Cal3_S2`, `Cal3Bundler`, `Cal3DS2`), and a measured 2D pixel coordinate `measured_`.\n",
|
|
"\n",
|
|
"Key features:\n",
|
|
"- **Templated:** Works with different pose, landmark, and calibration types.\n",
|
|
"- **Fixed Calibration:** Assumes the `CALIBRATION` object (`K_`) is known and fixed (passed as a shared pointer).\n",
|
|
"- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform between the pose variable's frame (body) and the camera's sensor frame.\n",
|
|
"- **Cheirality Handling:** Can be configured to throw an exception or return a large error if the landmark projects behind the camera.\n",
|
|
"\n",
|
|
"The error is the 2D vector difference:\n",
|
|
"$$ \\text{error}(P, L) = \\text{project}(P \\cdot S, L) - z $$\n",
|
|
"where $P$ is the pose variable, $L$ is the landmark variable, $S$ is the optional `body_P_sensor` transform, `project` is the camera projection function including calibration, and $z$ is the `measured_` point."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/ProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {
|
|
"id": "pip_code",
|
|
"tags": [
|
|
"remove-cell"
|
|
]
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"%pip install --quiet gtsam-develop"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {
|
|
"id": "imports_code"
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"import gtsam\n",
|
|
"import numpy as np\n",
|
|
"from gtsam import Pose3, Point3, Point2, Rot3, Cal3_S2, Values\n",
|
|
"# The Python wrapper often creates specific instantiations\n",
|
|
"from gtsam import GenericProjectionFactorCal3_S2\n",
|
|
"from gtsam import symbol_shorthand\n",
|
|
"\n",
|
|
"X = symbol_shorthand.X\n",
|
|
"L = symbol_shorthand.L"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_header_md"
|
|
},
|
|
"source": [
|
|
"## Creating a GenericProjectionFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_desc_md"
|
|
},
|
|
"source": [
|
|
"Instantiate by providing:\n",
|
|
"1. The 2D measurement (`Point2`).\n",
|
|
"2. The noise model (typically 2D isotropic).\n",
|
|
"3. The key for the pose variable.\n",
|
|
"4. The key for the landmark variable.\n",
|
|
"5. A `shared_ptr` to the fixed calibration object.\n",
|
|
"6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n",
|
|
"7. (Optional) Cheirality handling flags."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {
|
|
"id": "create_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Factor with offset: GenericProjectionFactor, z = [\n",
|
|
"\t330;\n",
|
|
"\t250\n",
|
|
"]\n",
|
|
" sensor pose in body frame: R: [\n",
|
|
"\t6.12323e-17, 6.12323e-17, 1;\n",
|
|
"\t-1, 3.7494e-33, 6.12323e-17;\n",
|
|
"\t-0, -1, 6.12323e-17\n",
|
|
"]\n",
|
|
"t: 0.1 0 0.2\n",
|
|
" keys = { x0 l1 }\n",
|
|
" noise model: unit (2) \n",
|
|
"\n",
|
|
"Factor without offset: GenericProjectionFactor, z = [\n",
|
|
"\t330;\n",
|
|
"\t250\n",
|
|
"]\n",
|
|
" keys = { x0 l1 }\n",
|
|
" noise model: unit (2) \n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"measured_pt2 = Point2(330, 250)\n",
|
|
"pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # 1 pixel std dev\n",
|
|
"pose_key = X(0)\n",
|
|
"landmark_key = L(1)\n",
|
|
"\n",
|
|
"# Shared pointer to calibration\n",
|
|
"K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n",
|
|
"\n",
|
|
"# Optional sensor pose offset\n",
|
|
"body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n",
|
|
"\n",
|
|
"# Create factor with sensor offset\n",
|
|
"factor_with_offset = GenericProjectionFactorCal3_S2(\n",
|
|
" measured_pt2, pixel_noise, pose_key, landmark_key, K, body_P_sensor)\n",
|
|
"factor_with_offset.print(\"Factor with offset: \")\n",
|
|
"\n",
|
|
"# Create factor without sensor offset (implicitly identity)\n",
|
|
"factor_no_offset = GenericProjectionFactorCal3_S2(\n",
|
|
" measured_pt2, pixel_noise, pose_key, landmark_key, K)\n",
|
|
"factor_no_offset.print(\"\\nFactor without offset: \")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "eval_header_md"
|
|
},
|
|
"source": [
|
|
"## Evaluating the Error"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "eval_desc_md"
|
|
},
|
|
"source": [
|
|
"The error is the difference between the predicted projection and the measurement."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {
|
|
"id": "eval_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Error (no offset): 1175689.2145311693\n",
|
|
"Error (with offset): 50751.576015003826\n",
|
|
"Manual error calculation (no offset): [1370.63962025 687.55033305]\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"values = Values()\n",
|
|
"\n",
|
|
"# Example values\n",
|
|
"pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n",
|
|
"landmark = Point3(4.0, 2.0, 3.0)\n",
|
|
"\n",
|
|
"values.insert(pose_key, pose)\n",
|
|
"values.insert(landmark_key, landmark)\n",
|
|
"\n",
|
|
"# Evaluate factor without offset\n",
|
|
"error_no_offset = factor_no_offset.error(values)\n",
|
|
"print(f\"Error (no offset): {error_no_offset}\")\n",
|
|
"\n",
|
|
"# Evaluate factor with offset\n",
|
|
"error_with_offset = factor_with_offset.error(values)\n",
|
|
"print(f\"Error (with offset): {error_with_offset}\")\n",
|
|
"\n",
|
|
"# Manually verify projection (no offset case)\n",
|
|
"cam_no_offset = gtsam.PinholeCameraCal3_S2(pose, K)\n",
|
|
"predicted_no_offset = cam_no_offset.project(landmark)\n",
|
|
"manual_error = predicted_no_offset - measured_pt2\n",
|
|
"print(f\"Manual error calculation (no offset): {manual_error}\")\n",
|
|
"assert np.allclose(factor_no_offset.unwhitenedError(values), manual_error)"
|
|
]
|
|
}
|
|
],
|
|
"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
|
|
}
|