12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed PlanarProjectionFactor

release/4.3a0
Frank Dellaert 2025-04-26 18:02:40 -04:00
parent 3e13c44802
commit fa23a888f9
2 changed files with 41 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/PlanarProjectionFactor.h
View File

@ -201,7 +201,7 @@ namespace gtsam {
const Cal3DS2& calib, const Cal3DS2& calib,
const SharedNoiseModel& model = {}) const SharedNoiseModel& model = {})
: PlanarProjectionFactorBase(measured), : PlanarProjectionFactorBase(measured),
NoiseModelFactorN(model, landmarkKey, poseKey), NoiseModelFactorN(model, poseKey, landmarkKey),
bTc_(bTc), bTc_(bTc),
calib_(calib) {} calib_(calib) {}

89
gtsam/slam/doc/PlanarProjectionFactor.ipynb
View File

@ -15,9 +15,9 @@
"id": "desc_md" "id": "desc_md"
}, },
"source": [ "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", "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 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 camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n",
"* The 3D landmark position in the world frame.\n", "* The 3D landmark position in the world frame.\n",
@ -41,7 +41,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "pip_code", "id": "pip_code",
"tags": [ "tags": [
@ -50,12 +50,16 @@
}, },
"outputs": [], "outputs": [],
"source": [ "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", "cell_type": "code",
"execution_count": 1, "execution_count": 2,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
@ -65,13 +69,7 @@
"import numpy as np\n", "import numpy as np\n",
"from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n", "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n",
" PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n", " PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n",
"from gtsam import symbol_shorthand\n", "from gtsam.symbol_shorthand import X, L, C, O"
"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"
] ]
}, },
{ {
@ -110,19 +108,19 @@
"isotropic dim=2 sigma=1.5\n", "isotropic dim=2 sigma=1.5\n",
"\n", "\n",
"Error at ground truth: 0.0\n", "Error at ground truth: 0.0\n",
"Error at noisy pose: 3317.647263749095\n" "Error at noisy pose: 3317.6472637491106\n"
] ]
} }
], ],
"source": [ "source": [
"# Known parameters\n", "# Known parameters\n",
"landmark_pt = Point3(2.0, 0.5, 0.5)\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", "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", "\n",
"# Assume ground truth pose and calculate expected measurement\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_world_T_cam = Pose3(gt_pose2) * body_T_cam\n",
"gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n", "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n",
"measured_pt2 = gt_camera.project(landmark_pt)\n", "measured_pt2 = gt_camera.project(landmark_pt)\n",
@ -130,18 +128,19 @@
"print(f\"Calculated Measurement: {measured_pt2}\")\n", "print(f\"Calculated Measurement: {measured_pt2}\")\n",
"\n", "\n",
"# Create the factor\n", "# Create the factor\n",
"pose_key = X(0)\n", "factor1 = PlanarProjectionFactor1(\n",
"factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n", " X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n",
")\n",
"factor1.print(\"Factor 1: \")\n", "factor1.print(\"Factor 1: \")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values = Values()\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", "error1_gt = factor1.error(values)\n",
"print(f\"\\nError at ground truth: {error1_gt}\")\n", "print(f\"\\nError at ground truth: {error1_gt}\")\n",
"\n", "\n",
"noisy_pose2 = Pose2(1.05, 0.02, np.pi/4 + 0.05)\n", "noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n",
"values.update(pose_key, noisy_pose2)\n", "values.update(X(0), noisy_pose2)\n",
"error1_noisy = factor1.error(values)\n", "error1_noisy = factor1.error(values)\n",
"print(f\"Error at noisy pose: {error1_noisy}\")" "print(f\"Error at noisy pose: {error1_noisy}\")"
] ]
@ -175,37 +174,29 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Factor 2: keys = { l0 x0 }\n", "Factor 2: keys = { x0 l0 }\n",
"isotropic dim=2 sigma=1.5\n" "isotropic dim=2 sigma=1.5\n",
] "\n",
}, "Error at ground truth: 0.0\n",
{ "Error with noisy landmark: 8066.192649473802\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": [ "source": [
"landmark_key = L(0)\n", "factor2 = PlanarProjectionFactor2(\n",
"\n", " X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n",
"factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", ")\n",
"factor2.print(\"Factor 2: \")\n", "factor2.print(\"Factor 2: \")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values = Values()\n", "values = Values()\n",
"values.insert(pose_key, gt_pose2)\n", "values.insert(X(0), gt_pose2)\n",
"values.insert(landmark_key, landmark_pt) # Error should be zero\n", "values.insert(L(0), landmark_pt) # Error should be zero\n",
"error2_gt = factor2.error(values)\n", "error2_gt = factor2.error(values)\n",
"print(f\"\\nError at ground truth: {error2_gt}\")\n", "print(f\"\\nError at ground truth: {error2_gt}\")\n",
"\n", "\n",
"noisy_landmark = Point3(2.1, 0.45, 0.55)\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", "error2_noisy = factor2.error(values)\n",
"print(f\"Error with noisy landmark: {error2_noisy}\")" "print(f\"Error with noisy landmark: {error2_noisy}\")"
] ]
@ -230,7 +221,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": 5,
"metadata": { "metadata": {
"id": "factor3_example_code" "id": "factor3_example_code"
}, },
@ -239,11 +230,11 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", "Factor 3: keys = { x0 o0 c0 }\n",
"Noise model: diagonal sigmas [1.5; 1.5];\n", "isotropic dim=2 sigma=1.5\n",
"\n", "\n",
"Error at ground truth: [-0. -0.]\n", "Error at ground truth: 0.0\n",
"Error with noisy calibration: [8.38867847 0.63659684]\n" "Error with noisy calibration: 92.30212176019934\n"
] ]
} }
], ],
@ -251,12 +242,12 @@
"offset_key = O(0)\n", "offset_key = O(0)\n",
"calib_key = C(0)\n", "calib_key = C(0)\n",
"\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", "factor3.print(\"Factor 3: \")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values = Values()\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(offset_key, body_T_cam)\n",
"values.insert(calib_key, calib) # Error should be zero\n", "values.insert(calib_key, calib) # Error should be zero\n",
"error3_gt = factor3.error(values)\n", "error3_gt = factor3.error(values)\n",
@ -271,7 +262,7 @@
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "gtsam", "display_name": "py312",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -285,7 +276,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.13.1" "version": "3.12.6"
} }
}, },
"nbformat": 4, "nbformat": 4,