From fa23a888f957c82e012d168b2295de7f80a4736e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 18:02:40 -0400 Subject: [PATCH] Fixed PlanarProjectionFactor --- gtsam/slam/PlanarProjectionFactor.h | 2 +- gtsam/slam/doc/PlanarProjectionFactor.ipynb | 89 +++++++++------------ 2 files changed, 41 insertions(+), 50 deletions(-) diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index d53cd0ae1..dc7e53677 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -201,7 +201,7 @@ namespace gtsam { const Cal3DS2& calib, const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), - NoiseModelFactorN(model, landmarkKey, poseKey), + NoiseModelFactorN(model, poseKey, landmarkKey), bTc_(bTc), calib_(calib) {} diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb index 55d76252d..3100fbb53 100644 --- a/gtsam/slam/doc/PlanarProjectionFactor.ipynb +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -15,9 +15,9 @@ "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", + "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`: world-to-body) in the ground plane.\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", @@ -41,7 +41,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -50,12 +50,16 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "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": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, @@ -65,13 +69,7 @@ "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" + "from gtsam.symbol_shorthand import X, L, C, O" ] }, { @@ -110,19 +108,19 @@ "isotropic dim=2 sigma=1.5\n", "\n", "Error at ground truth: 0.0\n", - "Error at noisy pose: 3317.647263749095\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", + "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", + "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_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", @@ -130,18 +128,19 @@ "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 = 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(pose_key, gt_pose2) # Error should be zero here\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(pose_key, noisy_pose2)\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}\")" ] @@ -175,37 +174,29 @@ "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 but requested type was class Eigen::Matrix", - "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 but requested type was class Eigen::Matrix" + "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": [ - "landmark_key = L(0)\n", - "\n", - "factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "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(pose_key, gt_pose2)\n", - "values.insert(landmark_key, landmark_pt) # Error should be zero\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(landmark_key, noisy_landmark)\n", + "values.update(L(0), noisy_landmark)\n", "error2_noisy = factor2.error(values)\n", "print(f\"Error with noisy landmark: {error2_noisy}\")" ] @@ -230,7 +221,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, "metadata": { "id": "factor3_example_code" }, @@ -239,11 +230,11 @@ "name": "stdout", "output_type": "stream", "text": [ - "Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", - "Noise model: diagonal sigmas [1.5; 1.5];\n", + "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: [8.38867847 0.63659684]\n" + "Error at ground truth: 0.0\n", + "Error with noisy calibration: 92.30212176019934\n" ] } ], @@ -251,12 +242,12 @@ "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 = 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(pose_key, gt_pose2)\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", @@ -271,7 +262,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -285,7 +276,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4,