Fixed PlanarProjectionFactor
parent
3e13c44802
commit
fa23a888f9
|
@ -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) {}
|
||||
|
||||
|
|
|
@ -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<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>"
|
||||
"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,
|
||||
|
|
Loading…
Reference in New Issue