294 lines
11 KiB
Plaintext
294 lines
11 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# PlanarProjectionFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"The `PlanarProjectionFactor` variants provide camera projection constraints 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`: world-to-body) 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": null,
|
|
"metadata": {
|
|
"id": "pip_code",
|
|
"tags": [
|
|
"remove-cell"
|
|
]
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"%pip install --quiet gtsam"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"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 import symbol_shorthand\n",
|
|
"import graphviz\n",
|
|
"\n",
|
|
"X = symbol_shorthand.X\n",
|
|
"L = symbol_shorthand.L\n",
|
|
"C = symbol_shorthand.C # Calibration\n",
|
|
"O = symbol_shorthand.O # Offset"
|
|
]
|
|
},
|
|
{
|
|
"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.647263749095\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",
|
|
"pose_key = X(0)\n",
|
|
"factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n",
|
|
"factor1.print(\"Factor 1: \")\n",
|
|
"\n",
|
|
"# Evaluate error\n",
|
|
"values = Values()\n",
|
|
"values.insert(pose_key, 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(pose_key, 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 = { l0 x0 }\n",
|
|
"isotropic dim=2 sigma=1.5\n"
|
|
]
|
|
},
|
|
{
|
|
"ename": "RuntimeError",
|
|
"evalue": "Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue<class gtsam::Pose2> but requested type was class Eigen::Matrix<double,-1,1,0,-1,1>",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
|
|
"\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)",
|
|
"Cell \u001b[1;32mIn[4], line 10\u001b[0m\n\u001b[0;32m 8\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose2)\n\u001b[0;32m 9\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(landmark_key, landmark_pt) \u001b[38;5;66;03m# Error should be zero\u001b[39;00m\n\u001b[1;32m---> 10\u001b[0m error2_gt \u001b[38;5;241m=\u001b[39m \u001b[43mfactor2\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror2_gt\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 13\u001b[0m noisy_landmark \u001b[38;5;241m=\u001b[39m Point3(\u001b[38;5;241m2.1\u001b[39m, \u001b[38;5;241m0.45\u001b[39m, \u001b[38;5;241m0.55\u001b[39m)\n",
|
|
"\u001b[1;31mRuntimeError\u001b[0m: Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue<class gtsam::Pose2> but requested type was class Eigen::Matrix<double,-1,1,0,-1,1>"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"landmark_key = L(0)\n",
|
|
"\n",
|
|
"factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n",
|
|
"factor2.print(\"Factor 2: \")\n",
|
|
"\n",
|
|
"# Evaluate error\n",
|
|
"values = Values()\n",
|
|
"values.insert(pose_key, gt_pose2)\n",
|
|
"values.insert(landmark_key, 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(landmark_key, 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": 3,
|
|
"metadata": {
|
|
"id": "factor3_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n",
|
|
"Noise model: diagonal sigmas [1.5; 1.5];\n",
|
|
"\n",
|
|
"Error at ground truth: [-0. -0.]\n",
|
|
"Error with noisy calibration: [8.38867847 0.63659684]\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"offset_key = O(0)\n",
|
|
"calib_key = C(0)\n",
|
|
"\n",
|
|
"factor3 = PlanarProjectionFactor3(pose_key, 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(pose_key, 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": "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
|
|
}
|