12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
gtsam/gtsam/slam/doc/PlanarProjectionFactor.ipynb

285 lines
9.5 KiB
Plaintext
Raw Blame History

{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PlanarProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"The `PlanarProjectionFactor` variants provide camera projection factors specifically designed for scenarios where **the robot or camera moves primarily on a 2D plane** (e.g., ground robots with cameras).\n",
"They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n",
"* The robot's 2D pose (`Pose2` `wTb`: body in world frame) in the ground plane.\n",
"* The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n",
"* The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n",
"* The 3D landmark position in the world frame.\n",
"\n",
"The core projection logic involves converting the `Pose2` `wTb` to a `Pose3` assuming z=0 and yaw=theta, composing it with `bTc` to get the world-to-camera pose `wTc`, and then using a standard `PinholeCamera` model to project the landmark.\n",
"\n",
"Variants:\n",
"* `PlanarProjectionFactor1`: Unknown is robot `Pose2` (`wTb`). Landmark, `bTc`, and calibration are fixed. Useful for localization.\n",
"* `PlanarProjectionFactor2`: Unknowns are robot `Pose2` (`wTb`) and `Point3` landmark. `bTc` and calibration are fixed. Useful for planar SLAM.\n",
"* `PlanarProjectionFactor3`: Unknowns are robot `Pose2` (`wTb`), camera offset `Pose3` (`bTc`), and `Cal3DS2` calibration. Landmark is fixed. Useful for calibration."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PlanarProjectionFactor.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 (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n",
" PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n",
"from gtsam.symbol_shorthand import X, L, C, O"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_header_md"
},
"source": [
"## 1. `PlanarProjectionFactor1` (Localization)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_desc_md"
},
"source": [
"Used when the landmark, camera offset (`bTc`), and calibration (`calib`) are known, and we want to estimate the robot's `Pose2` (`wTb`)."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "factor1_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Ground Truth Pose2: (1, 0, 0.785398)\n",
"\n",
"Calculated Measurement: [ 909.25565099 1841.1002863 ]\n",
"Factor 1: keys = { x0 }\n",
"isotropic dim=2 sigma=1.5\n",
"\n",
"Error at ground truth: 0.0\n",
"Error at noisy pose: 3317.6472637491106\n"
]
}
],
"source": [
"# Known parameters\n",
"landmark_pt = Point3(2.0, 0.5, 0.5)\n",
"body_T_cam = Pose3(Rot3.Yaw(-np.pi / 2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n",
"calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)\n",
"measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n",
"\n",
"# Assume ground truth pose and calculate expected measurement\n",
"gt_pose2 = Pose2(1.0, 0.0, np.pi / 4)\n",
"gt_world_T_cam = Pose3(gt_pose2) * body_T_cam\n",
"gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n",
"measured_pt2 = gt_camera.project(landmark_pt)\n",
"print(f\"Ground Truth Pose2: {gt_pose2}\")\n",
"print(f\"Calculated Measurement: {measured_pt2}\")\n",
"\n",
"# Create the factor\n",
"factor1 = PlanarProjectionFactor1(\n",
" X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n",
")\n",
"factor1.print(\"Factor 1: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(X(0), gt_pose2) # Error should be zero here\n",
"error1_gt = factor1.error(values)\n",
"print(f\"\\nError at ground truth: {error1_gt}\")\n",
"\n",
"noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n",
"values.update(X(0), noisy_pose2)\n",
"error1_noisy = factor1.error(values)\n",
"print(f\"Error at noisy pose: {error1_noisy}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_header_md"
},
"source": [
"## 2. `PlanarProjectionFactor2` (Planar SLAM)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_desc_md"
},
"source": [
"Used when the camera offset (`bTc`) and calibration (`calib`) are known, but both the robot `Pose2` (`wTb`) and the `Point3` landmark position are unknown."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "factor2_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Factor 2: keys = { x0 l0 }\n",
"isotropic dim=2 sigma=1.5\n",
"\n",
"Error at ground truth: 0.0\n",
"Error with noisy landmark: 8066.192649473802\n"
]
}
],
"source": [
"factor2 = PlanarProjectionFactor2(\n",
" X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n",
")\n",
"factor2.print(\"Factor 2: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(X(0), gt_pose2)\n",
"values.insert(L(0), landmark_pt) # Error should be zero\n",
"error2_gt = factor2.error(values)\n",
"print(f\"\\nError at ground truth: {error2_gt}\")\n",
"\n",
"noisy_landmark = Point3(2.1, 0.45, 0.55)\n",
"values.update(L(0), noisy_landmark)\n",
"error2_noisy = factor2.error(values)\n",
"print(f\"Error with noisy landmark: {error2_noisy}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor3_header_md"
},
"source": [
"## 3. `PlanarProjectionFactor3` (Calibration)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor3_desc_md"
},
"source": [
"Used when the landmark position is known, but the robot `Pose2` (`wTb`), the camera offset `Pose3` (`bTc`), and the `Cal3DS2` calibration are unknown."
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"id": "factor3_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Factor 3: keys = { x0 o0 c0 }\n",
"isotropic dim=2 sigma=1.5\n",
"\n",
"Error at ground truth: 0.0\n",
"Error with noisy calibration: 92.30212176019934\n"
]
}
],
"source": [
"offset_key = O(0)\n",
"calib_key = C(0)\n",
"\n",
"factor3 = PlanarProjectionFactor3(X(0), offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n",
"factor3.print(\"Factor 3: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(X(0), gt_pose2)\n",
"values.insert(offset_key, body_T_cam)\n",
"values.insert(calib_key, calib) # Error should be zero\n",
"error3_gt = factor3.error(values)\n",
"print(f\"\\nError at ground truth: {error3_gt}\")\n",
"\n",
"noisy_calib = Cal3DS2(fx=510, fy=495, s=0, u0=322, v0=241, k1=0, k2=0, p1=0, p2=0)\n",
"values.update(calib_key, noisy_calib)\n",
"error3_noisy = factor3.error(values)\n",
"print(f\"Error with noisy calibration: {error3_noisy}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
Reference in New Issue View Git Blame Copy Permalink