247 lines
10 KiB
Plaintext
247 lines
10 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# GenericStereoFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"`GenericStereoFactor<POSE, LANDMARK>` is a factor for handling measurements from a **calibrated stereo camera**.\n",
|
|
"It relates a 3D `LANDMARK` (usually `Point3`) to a `StereoPoint2` measurement observed by a stereo camera system defined by a `POSE` (usually `Pose3`) and a fixed stereo calibration `Cal3_S2Stereo`.\n",
|
|
"\n",
|
|
"`StereoPoint2` contains $(u_L, u_R, v)$, the horizontal pixel coordinates in the left ($u_L$) and right ($u_R$) images, and the vertical pixel coordinate ($v$), which is assumed the same for both images in a rectified stereo setup.\n",
|
|
"`Cal3_S2Stereo` holds the intrinsic parameters (focal length, principal point) common to both cameras and the stereo baseline (distance between camera centers).\n",
|
|
"\n",
|
|
"Key features:\n",
|
|
"- **Templated:** Works with different pose and landmark types.\n",
|
|
"- **Fixed Calibration:** Assumes the `Cal3_S2Stereo` object (`K_`) is known and fixed.\n",
|
|
"- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform.\n",
|
|
"- **Cheirality Handling:** Can be configured for points behind the camera.\n",
|
|
"\n",
|
|
"The error is the 3D vector difference:\n",
|
|
"$$ \\text{error}(P, L) = \\text{projectStereo}(P \\cdot S, L) - z $$\n",
|
|
"where `projectStereo` uses the `StereoCamera` model, $P$ is the pose, $L$ the landmark, $S$ the optional offset, and $z$ is the `measured_` `StereoPoint2`."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/StereoFactor.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, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n",
|
|
"# The Python wrapper often creates specific instantiations\n",
|
|
"from gtsam import GenericStereoFactor3D\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 GenericStereoFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_desc_md"
|
|
},
|
|
"source": [
|
|
"Instantiate by providing:\n",
|
|
"1. The measurement (`StereoPoint2`).\n",
|
|
"2. The noise model (typically 3D).\n",
|
|
"3. The key for the pose variable.\n",
|
|
"4. The key for the landmark variable.\n",
|
|
"5. A `shared_ptr` to the fixed stereo calibration object (`Cal3_S2Stereo`).\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: keys = { x0 l1 }\n",
|
|
" noise model: unit (3) \n",
|
|
"Factor with offset: .z(330, 305, 250)\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",
|
|
"\n",
|
|
"Factor without offset: keys = { x0 l1 }\n",
|
|
" noise model: unit (3) \n",
|
|
"\n",
|
|
"Factor without offset: .z(330, 305, 250)\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"measured_stereo = StereoPoint2(330, 305, 250) # uL, uR, v\n",
|
|
"stereo_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) # 1 pixel std dev (ul, ur, v)\n",
|
|
"pose_key = X(0)\n",
|
|
"landmark_key = L(1)\n",
|
|
"\n",
|
|
"# Shared pointer to stereo calibration\n",
|
|
"K_stereo = Cal3_S2Stereo(500.0, 500.0, 0.0, 320.0, 240.0, 0.1) # fx, fy, s, u0, v0, baseline\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 = GenericStereoFactor3D(\n",
|
|
" measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo, body_P_sensor=body_P_sensor)\n",
|
|
"factor_with_offset.print(\"Factor with offset: \")\n",
|
|
"\n",
|
|
"# Create factor without sensor offset\n",
|
|
"factor_no_offset = GenericStereoFactor3D(\n",
|
|
" measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo)\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 3D difference between the predicted stereo projection and the measurement."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {
|
|
"id": "eval_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"ename": "AttributeError",
|
|
"evalue": "'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
|
|
"\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)",
|
|
"Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 4\u001b[0m pose \u001b[38;5;241m=\u001b[39m Pose3(Rot3\u001b[38;5;241m.\u001b[39mRodrigues(\u001b[38;5;241m0.1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m0.2\u001b[39m, \u001b[38;5;241m0.3\u001b[39m), Point3(\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m0.5\u001b[39m))\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Triangulate a point that *should* project to measured_stereo\u001b[39;00m\n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m expected_point_camera \u001b[38;5;241m=\u001b[39m \u001b[43mK_stereo\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mbackproject\u001b[49m(measured_stereo)\n\u001b[0;32m 8\u001b[0m landmark \u001b[38;5;241m=\u001b[39m pose\u001b[38;5;241m.\u001b[39mtransformFrom(expected_point_camera)\n\u001b[0;32m 9\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mExpected landmark point: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mlandmark\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n",
|
|
"\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"values = Values()\n",
|
|
"\n",
|
|
"# Example values\n",
|
|
"pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n",
|
|
"# Triangulate a point that *should* project to measured_stereo\n",
|
|
"# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n",
|
|
"expected_point_camera = K_stereo.backproject(measured_stereo)\n",
|
|
"landmark = pose.transformFrom(expected_point_camera)\n",
|
|
"print(f\"Expected landmark point: {landmark}\")\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\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be zero)\")\n",
|
|
"\n",
|
|
"# Evaluate factor with offset\n",
|
|
"# Need to recompute landmark based on offset pose\n",
|
|
"pose_with_offset = pose * body_P_sensor # This is world_P_sensor\n",
|
|
"expected_point_offset_cam = K_stereo.backproject(measured_stereo)\n",
|
|
"landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam)\n",
|
|
"values.update(landmark_key, landmark_offset)\n",
|
|
"error_with_offset = factor_with_offset.error(values)\n",
|
|
"print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be zero)\")\n",
|
|
"\n",
|
|
"# Evaluate with noisy landmark\n",
|
|
"noisy_landmark = landmark + Point3(0.1, -0.05, 0.1)\n",
|
|
"values.update(landmark_key, noisy_landmark)\n",
|
|
"error_no_offset_noisy = factor_no_offset.error(values)\n",
|
|
"print(f\"Error (no offset) at noisy landmark: {error_no_offset_noisy}\")"
|
|
]
|
|
}
|
|
],
|
|
"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
|
|
}
|