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

Fix SLAM notebook wrap problems and typos

release/4.3a0
p-zach 2025-04-26 20:17:53 -04:00
parent 43addb5ee9
commit 57dbbfd9e3
10 changed files with 298 additions and 363 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/slam/doc/FrobeniusFactor.ipynb
View File

@ -50,7 +50,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
@ -86,7 +86,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 4,
"metadata": { "metadata": {
"id": "fprior_example_code" "id": "fprior_example_code"
}, },
@ -95,12 +95,10 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"FrobeniusPriorRot3: FrobeniusPriorFactor on R0\n", "FrobeniusPriorRot3: keys = { r0 }\n",
"Error model: diagonal sigmas [0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01];\n", " noise model: unit (9) \n",
"\n", "\n",
"FrobeniusPrior error (vectorized matrix diff): (9,)\n", "FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05\n"
"[ 0.00054931 -0.00997917 0. -0.00997917 -0.00054931 0.\n",
" 0. 0. 0. ]\n"
] ]
} }
], ],
@ -121,7 +119,7 @@
"values = Values()\n", "values = Values()\n",
"values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n", "values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n",
"error_prior = prior_fro.error(values)\n", "error_prior = prior_fro.error(values)\n",
"print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior.shape}\\n{error_prior}\")" "print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior}\")"
] ]
}, },
{ {
@ -144,7 +142,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 7,
"metadata": { "metadata": {
"id": "ffactor_example_code" "id": "ffactor_example_code"
}, },
@ -155,18 +153,9 @@
"text": [ "text": [
"\n", "\n",
"FrobeniusFactorRot3: keys = { r0 r1 }\n", "FrobeniusFactorRot3: keys = { r0 r1 }\n",
" noise model: unit (9) \n" " noise model: unit (9) \n",
] "\n",
}, "FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05\n"
{
"ename": "RuntimeError",
"evalue": "Attempting to at the key \"r0\", which does not exist in the Values.",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[6], line 13\u001b[0m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;66;03m# Evaluate error\u001b[39;00m\n\u001b[0;32m 12\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(key2, Rot3\u001b[38;5;241m.\u001b[39mYaw(\u001b[38;5;241m0.115\u001b[39m)) \u001b[38;5;66;03m# R1 slightly different from R0\u001b[39;00m\n\u001b[1;32m---> 13\u001b[0m error_factor \u001b[38;5;241m=\u001b[39m \u001b[43mfactor_fro\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 14\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;124mFrobeniusFactor error (vectorized matrix diff): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;241m.\u001b[39mshape\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n",
"\u001b[1;31mRuntimeError\u001b[0m: Attempting to at the key \"r0\", which does not exist in the Values."
] ]
} }
], ],
@ -182,9 +171,10 @@
"factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n", "factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values.insert(key1, Rot3.Yaw(0.11))\n",
"values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n", "values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n",
"error_factor = factor_fro.error(values)\n", "error_factor = factor_fro.error(values)\n",
"print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor.shape}\\n{error_factor}\")" "print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor}\")"
] ]
}, },
{ {
@ -207,7 +197,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": 9,
"metadata": { "metadata": {
"id": "fbetween_example_code" "id": "fbetween_example_code"
}, },
@ -217,17 +207,15 @@
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"\n", "\n",
"FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<gtsam::Rot3>(R0,R1)\n", "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<class gtsam::Rot3>(r0,r1)\n",
" T12: R: [\n", " T12: [\n",
"\t0.999875, -0.0149991, 0;\n", "\t0.999988, -0.00499998, 0;\n",
"\t0.0149991, 0.999875, 0;\n", "\t0.00499998, 0.999988, 0;\n",
"\t0, 0, 1\n", "\t0, 0, 1\n",
"]\n", "]\n",
" noise model: unit (9) \n",
"\n", "\n",
" noise model: diagonal sigmas [0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005];\n", "FrobeniusBetweenFactor error: 1.925929944387236e-34\n"
"\n",
"FrobeniusBetweenFactor error: (9,)\n",
"[-0. 0. 0. 0. -0. 0. 0. 0. 0.]\n"
] ]
} }
], ],
@ -241,7 +229,7 @@
"\n", "\n",
"# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n", "# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n",
"error_between = between_fro.error(values)\n", "error_between = between_fro.error(values)\n",
"print(f\"\\nFrobeniusBetweenFactor error: {error_between.shape}\\n{error_between}\")" "print(f\"\\nFrobeniusBetweenFactor error: {error_between}\")"
] ]
} }
], ],
@ -261,7 +249,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.11.9" "version": "3.13.1"
} }
}, },
"nbformat": 4, "nbformat": 4,

71
gtsam/slam/doc/OrientedPlane3Factor.ipynb
View File

@ -48,32 +48,17 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 2,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
"outputs": [ "outputs": [],
{
"ename": "ImportError",
"evalue": "cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, OrientedPlane3, Point3, Rot3, Values\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m OrientedPlane3Factor, OrientedPlane3DirectionPrior\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
"from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n", "from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n",
"from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n", "from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n",
"from gtsam import symbol_shorthand\n", "from gtsam.symbol_shorthand import X, P"
"\n",
"X = symbol_shorthand.X\n",
"P = symbol_shorthand.P # For Plane"
] ]
}, },
{ {
@ -97,7 +82,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 4,
"metadata": { "metadata": {
"id": "factor_example_code" "id": "factor_example_code"
}, },
@ -106,13 +91,21 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"OrientedPlane3Factor: OrientedPlane3Factor Factor with keys: x0 P0\n", "OrientedPlane3Factor: \n",
"\n", "OrientedPlane3Factor Factor (x0, p0)\n",
"Error model: diagonal sigmas [0.05; 0.05; 0.05];\n", "Measured Plane : 0 0 1 1\n",
"\n", "isotropic dim=3 sigma=0.05\n"
"Error at ground truth: [-0. -0. -0.]\n", ]
"\n", },
"Error with noisy plane: [ -0.0150837 -0.00503354 -49.83283361]\n" {
"ename": "TypeError",
"evalue": "insert(): incompatible function arguments. The following argument types are supported:\n 1. (self: gtsam.gtsam.Values, values: gtsam.gtsam.Values) -> None\n 2. (self: gtsam.gtsam.Values, j: int, vector: numpy.ndarray[numpy.float64[m, 1]]) -> None\n 3. (self: gtsam.gtsam.Values, j: int, matrix: numpy.ndarray[numpy.float64[m, n]]) -> None\n 4. (self: gtsam.gtsam.Values, j: int, point2: numpy.ndarray[numpy.float64[2, 1]]) -> None\n 5. (self: gtsam.gtsam.Values, j: int, point3: numpy.ndarray[numpy.float64[3, 1]]) -> None\n 6. (self: gtsam.gtsam.Values, j: int, rot2: gtsam.gtsam.Rot2) -> None\n 7. (self: gtsam.gtsam.Values, j: int, pose2: gtsam.gtsam.Pose2) -> None\n 8. (self: gtsam.gtsam.Values, j: int, R: gtsam.gtsam.SO3) -> None\n 9. (self: gtsam.gtsam.Values, j: int, Q: gtsam.gtsam.SO4) -> None\n 10. (self: gtsam.gtsam.Values, j: int, P: gtsam.gtsam.SOn) -> None\n 11. (self: gtsam.gtsam.Values, j: int, rot3: gtsam.gtsam.Rot3) -> None\n 12. (self: gtsam.gtsam.Values, j: int, pose3: gtsam.gtsam.Pose3) -> None\n 13. (self: gtsam.gtsam.Values, j: int, similarity2: gtsam.gtsam.Similarity2) -> None\n 14. (self: gtsam.gtsam.Values, j: int, similarity3: gtsam.gtsam.Similarity3) -> None\n 15. (self: gtsam.gtsam.Values, j: int, unit3: gtsam.gtsam.Unit3) -> None\n 16. (self: gtsam.gtsam.Values, j: int, cal3bundler: gtsam.gtsam.Cal3Bundler) -> None\n 17. (self: gtsam.gtsam.Values, j: int, cal3f: gtsam.gtsam.Cal3f) -> None\n 18. (self: gtsam.gtsam.Values, j: int, cal3_s2: gtsam.gtsam.Cal3_S2) -> None\n 19. (self: gtsam.gtsam.Values, j: int, cal3ds2: gtsam.gtsam.Cal3DS2) -> None\n 20. (self: gtsam.gtsam.Values, j: int, cal3fisheye: gtsam.gtsam.Cal3Fisheye) -> None\n 21. (self: gtsam.gtsam.Values, j: int, cal3unified: gtsam.gtsam.Cal3Unified) -> None\n 22. (self: gtsam.gtsam.Values, j: int, E: gtsam.gtsam.EssentialMatrix) -> None\n 23. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.FundamentalMatrix) -> None\n 24. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.SimpleFundamentalMatrix) -> None\n 25. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Bundler) -> None\n 26. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3f) -> None\n 27. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3_S2) -> None\n 28. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3DS2) -> None\n 29. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Fisheye) -> None\n 30. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Unified) -> None\n 31. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Bundler) -> None\n 32. (self: gtsam.gtsam.Values, j: int, camera: gtsam::PinholePose<gtsam::Cal3f>) -> None\n 33. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3_S2) -> None\n 34. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3DS2) -> None\n 35. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Fisheye) -> None\n 36. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Unified) -> None\n 37. (self: gtsam.gtsam.Values, j: int, constant_bias: gtsam::imuBias::ConstantBias) -> None\n 38. (self: gtsam.gtsam.Values, j: int, nav_state: gtsam::NavState) -> None\n 39. (self: gtsam.gtsam.Values, j: int, c: float) -> None\n\nInvoked with: Values with 1 values:\nValue x0: (class gtsam::Pose3)\nR: [\n\t0.995004, -0.0998334, 0;\n\t0.0998334, 0.995004, 0;\n\t0, 0, 1\n]\nt: 0.5 0 0\n\n, 8070450532247928832, : 0 0 1 1\n",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[4], line 22\u001b[0m\n\u001b[0;32m 20\u001b[0m values \u001b[38;5;241m=\u001b[39m Values()\n\u001b[0;32m 21\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose)\n\u001b[1;32m---> 22\u001b[0m \u001b[43mvalues\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minsert\u001b[49m\u001b[43m(\u001b[49m\u001b[43mplane_key\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mgt_plane_world\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 23\u001b[0m error1 \u001b[38;5;241m=\u001b[39m plane_factor\u001b[38;5;241m.\u001b[39merror(values)\n\u001b[0;32m 24\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;00merror1\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n",
"\u001b[1;31mTypeError\u001b[0m: insert(): incompatible function arguments. The following argument types are supported:\n 1. (self: gtsam.gtsam.Values, values: gtsam.gtsam.Values) -> None\n 2. (self: gtsam.gtsam.Values, j: int, vector: numpy.ndarray[numpy.float64[m, 1]]) -> None\n 3. (self: gtsam.gtsam.Values, j: int, matrix: numpy.ndarray[numpy.float64[m, n]]) -> None\n 4. (self: gtsam.gtsam.Values, j: int, point2: numpy.ndarray[numpy.float64[2, 1]]) -> None\n 5. (self: gtsam.gtsam.Values, j: int, point3: numpy.ndarray[numpy.float64[3, 1]]) -> None\n 6. (self: gtsam.gtsam.Values, j: int, rot2: gtsam.gtsam.Rot2) -> None\n 7. (self: gtsam.gtsam.Values, j: int, pose2: gtsam.gtsam.Pose2) -> None\n 8. (self: gtsam.gtsam.Values, j: int, R: gtsam.gtsam.SO3) -> None\n 9. (self: gtsam.gtsam.Values, j: int, Q: gtsam.gtsam.SO4) -> None\n 10. (self: gtsam.gtsam.Values, j: int, P: gtsam.gtsam.SOn) -> None\n 11. (self: gtsam.gtsam.Values, j: int, rot3: gtsam.gtsam.Rot3) -> None\n 12. (self: gtsam.gtsam.Values, j: int, pose3: gtsam.gtsam.Pose3) -> None\n 13. (self: gtsam.gtsam.Values, j: int, similarity2: gtsam.gtsam.Similarity2) -> None\n 14. (self: gtsam.gtsam.Values, j: int, similarity3: gtsam.gtsam.Similarity3) -> None\n 15. (self: gtsam.gtsam.Values, j: int, unit3: gtsam.gtsam.Unit3) -> None\n 16. (self: gtsam.gtsam.Values, j: int, cal3bundler: gtsam.gtsam.Cal3Bundler) -> None\n 17. (self: gtsam.gtsam.Values, j: int, cal3f: gtsam.gtsam.Cal3f) -> None\n 18. (self: gtsam.gtsam.Values, j: int, cal3_s2: gtsam.gtsam.Cal3_S2) -> None\n 19. (self: gtsam.gtsam.Values, j: int, cal3ds2: gtsam.gtsam.Cal3DS2) -> None\n 20. (self: gtsam.gtsam.Values, j: int, cal3fisheye: gtsam.gtsam.Cal3Fisheye) -> None\n 21. (self: gtsam.gtsam.Values, j: int, cal3unified: gtsam.gtsam.Cal3Unified) -> None\n 22. (self: gtsam.gtsam.Values, j: int, E: gtsam.gtsam.EssentialMatrix) -> None\n 23. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.FundamentalMatrix) -> None\n 24. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.SimpleFundamentalMatrix) -> None\n 25. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Bundler) -> None\n 26. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3f) -> None\n 27. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3_S2) -> None\n 28. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3DS2) -> None\n 29. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Fisheye) -> None\n 30. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Unified) -> None\n 31. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Bundler) -> None\n 32. (self: gtsam.gtsam.Values, j: int, camera: gtsam::PinholePose<gtsam::Cal3f>) -> None\n 33. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3_S2) -> None\n 34. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3DS2) -> None\n 35. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Fisheye) -> None\n 36. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Unified) -> None\n 37. (self: gtsam.gtsam.Values, j: int, constant_bias: gtsam::imuBias::ConstantBias) -> None\n 38. (self: gtsam.gtsam.Values, j: int, nav_state: gtsam::NavState) -> None\n 39. (self: gtsam.gtsam.Values, j: int, c: float) -> None\n\nInvoked with: Values with 1 values:\nValue x0: (class gtsam::Pose3)\nR: [\n\t0.995004, -0.0998334, 0;\n\t0.0998334, 0.995004, 0;\n\t0, 0, 1\n]\nt: 0.5 0 0\n\n, 8070450532247928832, : 0 0 1 1\n"
] ]
} }
], ],
@ -126,7 +119,7 @@
"# Measurement: transform the world plane into the camera frame\n", "# Measurement: transform the world plane into the camera frame\n",
"# measured_plane = gt_plane_world.transform(gt_pose)\n", "# measured_plane = gt_plane_world.transform(gt_pose)\n",
"# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:\n", "# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:\n",
"measured_plane_coeffs = gt_plane_world.coeffs()\n", "measured_plane_coeffs = gt_plane_world.planeCoefficients()\n",
"plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n", "plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n",
"\n", "\n",
"pose_key = X(0)\n", "pose_key = X(0)\n",
@ -170,7 +163,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 6,
"metadata": { "metadata": {
"id": "prior_example_code" "id": "prior_example_code"
}, },
@ -179,11 +172,21 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"OrientedPlane3DirectionPrior: Factor OrientedPlane3DirectionPrior on P0\n", "OrientedPlane3DirectionPrior: \n",
"Noise model: diagonal sigmas [0.02; 0.02; 0.02];\n", "OrientedPlane3DirectionPrior: Prior Factor on p0\n",
"\n", "Measured Plane : 0 0 1 0\n",
"Error for prior on noisy_plane: [-0.5 -0.5 0.5]\n", "isotropic dim=3 sigma=0.02\n"
"Error for prior on gt_plane_world: [0. 0. 0.]\n" ]
},
{
"ename": "RuntimeError",
"evalue": "Attempting to at the key \"p0\", which does not exist in the Values.",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[6], line 9\u001b[0m\n\u001b[0;32m 6\u001b[0m prior_factor\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOrientedPlane3DirectionPrior: \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 8\u001b[0m \u001b[38;5;66;03m# Evaluate error using the 'noisy_plane' from the previous step\u001b[39;00m\n\u001b[1;32m----> 9\u001b[0m error_prior \u001b[38;5;241m=\u001b[39m \u001b[43mprior_factor\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 \u001b[38;5;66;03m# values still contains plane_key -> noisy_plane\u001b[39;00m\n\u001b[0;32m 10\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 for prior on noisy_plane: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror_prior\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 12\u001b[0m \u001b[38;5;66;03m# Evaluate error for ground truth plane\u001b[39;00m\n",
"\u001b[1;31mRuntimeError\u001b[0m: Attempting to at the key \"p0\", which does not exist in the Values."
] ]
} }
], ],
@ -192,7 +195,7 @@
"measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n", "measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n",
"direction_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.02)\n", "direction_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.02)\n",
"\n", "\n",
"prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.coeffs(), direction_noise)\n", "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), direction_noise)\n",
"prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n", "prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n",
"\n", "\n",
"# Evaluate error using the 'noisy_plane' from the previous step\n", "# Evaluate error using the 'noisy_plane' from the previous step\n",

43
gtsam/slam/doc/PoseRotationPrior.ipynb
View File

@ -49,27 +49,15 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
"outputs": [ "outputs": [],
{
"ename": "ImportError",
"evalue": "cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[2], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseRotationPriorPose\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPriorPose3\n", "from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPrior3D\n",
"from gtsam import symbol_shorthand\n", "from gtsam import symbol_shorthand\n",
"\n", "\n",
"X = symbol_shorthand.X" "X = symbol_shorthand.X"
@ -95,7 +83,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 2,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
@ -104,14 +92,13 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"PoseRotationPrior: PoseRotationPriorFactor on x0\n", "PoseRotationPrior: PoseRotationPrior keys = { x0 }\n",
"Noise model: diagonal sigmas [0.05; 0.05; 0.05];\n", "isotropic dim=3 sigma=0.05\n",
"Measured Rotation R: [\n", "Measured Rotation [\n",
"\t0.707107, -0.707107, 0;\n", "\t0.707107, -0.707107, 0;\n",
"\t0.707107, 0.707107, 0;\n", "\t0.707107, 0.707107, 0;\n",
"\t0, 0, 1\n", "\t0, 0, 1\n",
"]\n", "]\n"
"\n"
] ]
} }
], ],
@ -122,13 +109,13 @@
"# Noise model on rotation (3 dimensions for Rot3)\n", "# Noise model on rotation (3 dimensions for Rot3)\n",
"rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n", "rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n",
"\n", "\n",
"# Factor type includes the Pose type, e.g. PoseRotationPriorPose3\n", "# Factor type includes the Pose type, e.g. PoseRotationPrior3D\n",
"factor = PoseRotationPriorPose3(pose_key, measured_rotation, rotation_noise)\n", "factor = PoseRotationPrior3D(pose_key, measured_rotation, rotation_noise)\n",
"factor.print(\"PoseRotationPrior: \")\n", "factor.print(\"PoseRotationPrior: \")\n",
"\n", "\n",
"# Alternative constructor: extract rotation from a full Pose3 prior\n", "# Alternative constructor: extract rotation from a full Pose3 prior\n",
"full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored\n", "full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored\n",
"factor_from_pose = PoseRotationPriorPose3(pose_key, full_pose_prior, rotation_noise)\n", "factor_from_pose = PoseRotationPrior3D(pose_key, full_pose_prior, rotation_noise)\n",
"# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical"
] ]
}, },
@ -152,7 +139,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 3,
"metadata": { "metadata": {
"id": "eval_example_code" "id": "eval_example_code"
}, },
@ -161,9 +148,9 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Error with correct rotation: [0. 0. 0.] (Should be near zero)\n", "Error with correct rotation: 0.0 (Should be near zero)\n",
"Error with incorrect rotation: [-0. -0. 2.00000004] (Should be non-zero)\n", "Error with incorrect rotation: 1.9999999999999951 (Should be non-zero)\n",
"Error with different translation: [-0. -0. 2.00000004] (Should be same as error2)\n" "Error with different translation: 1.9999999999999951 (Should be same as error2)\n"
] ]
} }
], ],

44
gtsam/slam/doc/PoseTranslationPrior.ipynb
View File

@ -19,8 +19,6 @@
"It ignores the rotation component of the pose variable during error calculation.\n", "It ignores the rotation component of the pose variable during error calculation.\n",
"The error is calculated as the difference between the translation component of the pose variable and the measured prior translation, expressed in the tangent space (which is typically just vector subtraction for `Point2` or `Point3`).\n", "The error is calculated as the difference between the translation component of the pose variable and the measured prior translation, expressed in the tangent space (which is typically just vector subtraction for `Point2` or `Point3`).\n",
"\n", "\n",
"Error: $ \\text{pose.translation}() - \\text{measured} $ (potentially rotated into world frame if translation is in body frame - check `evaluateError` implementation details in C++). *Correction based on C++ code*: The error is `traits<Translation>::Local(measured_, pose.translation())`, calculated in the world frame, and the Jacobian involves the pose's rotation.\n",
"\n",
"This is useful when you have information about the absolute position of a pose but little or no information about its orientation (e.g., GPS measurement)." "This is useful when you have information about the absolute position of a pose but little or no information about its orientation (e.g., GPS measurement)."
] ]
}, },
@ -49,27 +47,15 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 2,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
"outputs": [ "outputs": [],
{
"ename": "ImportError",
"evalue": "cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n", "from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPrior3D\n",
"from gtsam import symbol_shorthand\n", "from gtsam import symbol_shorthand\n",
"\n", "\n",
"X = symbol_shorthand.X" "X = symbol_shorthand.X"
@ -95,7 +81,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 4,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
@ -104,9 +90,13 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"PoseTranslationPrior: Factor PoseTranslationPrior on x0\n", "PoseTranslationPrior: PoseTranslationPrior keys = { x0 }\n",
"Noise model: diagonal sigmas [0.5; 0.5; 0.5];\n", "isotropic dim=3 sigma=0.5\n",
"Measured Translation10 20 5\n" "Measured Translation[\n",
"\t10;\n",
"\t20;\n",
"\t5\n",
"]\n"
] ]
} }
], ],
@ -118,12 +108,12 @@
"translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev\n", "translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev\n",
"\n", "\n",
"# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3\n", "# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3\n",
"factor = PoseTranslationPriorPose3(pose_key, measured_translation, translation_noise)\n", "factor = PoseTranslationPrior3D(pose_key, measured_translation, translation_noise)\n",
"factor.print(\"PoseTranslationPrior: \")\n", "factor.print(\"PoseTranslationPrior: \")\n",
"\n", "\n",
"# Alternative constructor: extract translation from a full Pose3 prior\n", "# Alternative constructor: extract translation from a full Pose3 prior\n",
"full_pose_prior = Pose3(Rot3.Yaw(np.pi/2), measured_translation) # Rotation is ignored\n", "full_pose_prior = Pose3(Rot3.Yaw(np.pi/2), measured_translation) # Rotation is ignored\n",
"factor_from_pose = PoseTranslationPriorPose3(pose_key, full_pose_prior, translation_noise)\n", "factor_from_pose = PoseTranslationPrior3D(pose_key, full_pose_prior, translation_noise)\n",
"# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical"
] ]
}, },
@ -147,7 +137,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 5,
"metadata": { "metadata": {
"id": "eval_example_code" "id": "eval_example_code"
}, },
@ -156,9 +146,9 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Error with correct translation: [0. 0. 0.] (Should be near zero)\n", "Error with correct translation: 0.0 (Should be near zero)\n",
"Error with incorrect translation: [ 0.39999999 -0.19999999 0.19999999] (Should be non-zero)\n", "Error with incorrect translation: 0.11999999999999986 (Should be non-zero)\n",
"Error with different rotation: [ 0.39999999 -0.19999999 0.19999999] (Should reflect Jacobian change)\n", "Error with different rotation: 0.11999999999999986 (Should reflect Jacobian change)\n",
"Unwhitened error with different rotation: [ 0.2 -0.1 0.1] (Should be [0.2, -0.1, 0.1])\n" "Unwhitened error with different rotation: [ 0.2 -0.1 0.1] (Should be [0.2, -0.1, 0.1])\n"
] ]
} }

18
gtsam/slam/doc/ReferenceFrameFactor.ipynb
View File

@ -52,6 +52,24 @@
"%pip install --quiet gtsam-develop" "%pip install --quiet gtsam-develop"
] ]
}, },
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[]\n"
]
}
],
"source": [
"import gtsam\n",
"print([k for k in gtsam.__dict__.keys() if \"ReferenceFrame\" in k])"
]
},
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 1,

54
gtsam/slam/doc/RotateFactor.ipynb
View File

@ -50,19 +50,7 @@
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
"outputs": [ "outputs": [],
{
"ename": "ImportError",
"evalue": "cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 4\u001b[0m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Rot3, Point3, Unit3, Values\n\u001b[1;32m----> 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m RotateFactor, RotateDirectionsFactor\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m R \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mR \u001b[38;5;66;03m# For Rotation key\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
@ -93,7 +81,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 2,
"metadata": { "metadata": {
"id": "factor1_example_code" "id": "factor1_example_code"
}, },
@ -102,14 +90,14 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"RotateFactor: Factor RotateFactor on R0\n", "RotateFactor: keys = { r0 }\n",
"Noise model: diagonal sigmas [0.001; 0.001; 0.001];\n", "isotropic dim=3 sigma=0.001\n",
"RotateFactor:]\n", "RotateFactor:]\n",
"p: [0.01 0.02 0.03]\n", "p: 0.01 0.02 0.03\n",
"z: [ 0.03 -0.01 0.02]\n", "z: 0.03 -0.01 0.02\n",
"\n", "\n",
"Error at ground truth R: [-0. -0. 0.]\n", "Error at ground truth R: 700.0\n",
"Error at noisy R: [ 0.039987 0.00999887 -0.00998887]\n" "Error at noisy R: 699.2869223608442\n"
] ]
} }
], ],
@ -158,7 +146,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 8,
"metadata": { "metadata": {
"id": "factor2_example_code" "id": "factor2_example_code"
}, },
@ -167,15 +155,23 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"RotateDirectionsFactor: Factor RotateDirectionsFactor on R0\n", "RotateDirectionsFactor: keys = { r0 }\n",
"Noise model: diagonal sigmas [0.01; 0.01];\n", "isotropic dim=2 sigma=0.01\n",
"RotateDirectionsFactor:\n", "RotateDirectionsFactor:\n",
"pUnit3: (1, 0, 0)\n", "p:1\n",
"zUnit3: (0, 1, 0)\n", "0\n",
"0\n",
"z:0\n",
"1\n",
"0\n",
"\n", "\n",
"Check: gt_R * z_body = [ 1.0000000e+00 -6.1232340e-17 0.0000000e+00]\n", "Check: gt_R * z_body = \n",
"Error at ground truth R: [-0. 0.]\n", ": 1\n",
"Error at noisy R: [-0.99995 -0.99995]\n" "6.12323e-17\n",
" 0\n",
"\n",
"Error at ground truth R: 1.874699728327322e-29\n",
"Error at noisy R: 0.999933335111092\n"
] ]
} }
], ],
@ -190,7 +186,7 @@
"# Ground truth R rotates z_body (0,1,0) to p_world (1,0,0)\n", "# Ground truth R rotates z_body (0,1,0) to p_world (1,0,0)\n",
"# This corresponds to a -90 deg yaw\n", "# This corresponds to a -90 deg yaw\n",
"gt_R_dir = Rot3.Yaw(-np.pi/2)\n", "gt_R_dir = Rot3.Yaw(-np.pi/2)\n",
"print(f\"\\nCheck: gt_R * z_body = {gt_R_dir * z_body.point3()}\")\n", "print(f\"\\nCheck: gt_R * z_body = \\n{gt_R_dir.rotate(z_body)}\")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values_dir = Values()\n", "values_dir = Values()\n",

72
gtsam/slam/doc/SmartProjectionFactor.ipynb
View File

@ -40,7 +40,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "pip_code", "id": "pip_code",
"tags": [ "tags": [
@ -49,28 +49,20 @@
}, },
"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"
}, },
"outputs": [ "outputs": [],
{
"ename": "ImportError",
"evalue": "cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m PinholeCameraCal3_S2, Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
@ -104,42 +96,20 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 3,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
"outputs": [ "outputs": [
{ {
"name": "stdout", "ename": "AttributeError",
"output_type": "stream", "evalue": "'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'",
"text": [ "output_type": "error",
"Smart factor involves 3 measurements.\n", "traceback": [
"SmartFactor: SmartProjectionFactor\n", "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"linearizationMode: 0\n", "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)",
"triangulationParameters:\n", "Cell \u001b[1;32mIn[3], line 8\u001b[0m\n\u001b[0;32m 5\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Add measurements and keys\u001b[39;00m\n\u001b[1;32m----> 8\u001b[0m \u001b[43msmart_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43madd\u001b[49m(Point2(\u001b[38;5;241m150\u001b[39m, \u001b[38;5;241m505\u001b[39m), C(\u001b[38;5;241m0\u001b[39m))\n\u001b[0;32m 9\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m470\u001b[39m, \u001b[38;5;241m495\u001b[39m), C(\u001b[38;5;241m1\u001b[39m))\n\u001b[0;32m 10\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m480\u001b[39m, \u001b[38;5;241m150\u001b[39m), C(\u001b[38;5;241m2\u001b[39m))\n",
"rankTolerance = 1e-09\n", "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'"
"enableEPI = false\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"\n",
"\n",
"result:\n",
"Degenerate\n",
"\n",
"SmartFactorBase, z = \n",
"measurement 0, px = \n",
"[150; 505]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 1, px = \n",
"[470; 495]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 2, px = \n",
"[480; 150]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"Factor on C0 C1 C2\n"
] ]
} }
], ],
@ -179,7 +149,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 7,
"metadata": { "metadata": {
"id": "eval_example_code" "id": "eval_example_code"
}, },
@ -189,9 +159,9 @@
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Triangulated point result:\n", "Triangulated point result:\n",
"Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", "Status.DEGENERATE\n",
"\n", "\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" "Triangulation failed, error calculation depends on degeneracyMode.\n"
] ]
} }
], ],
@ -209,7 +179,7 @@
"\n", "\n",
"# Triangulate first to see the implicit point\n", "# Triangulate first to see the implicit point\n",
"point_result = smart_factor.point(values)\n", "point_result = smart_factor.point(values)\n",
"print(f\"Triangulated point result:\\n{point_result}\")\n", "print(f\"Triangulated point result:\\n{point_result.status}\")\n",
"\n", "\n",
"if point_result.valid():\n", "if point_result.valid():\n",
" # Calculate error\n", " # Calculate error\n",

72
gtsam/slam/doc/SmartFactorParams.ipynb → gtsam/slam/doc/SmartProjectionParams.ipynb
View File

@ -83,7 +83,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 9,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
@ -92,18 +92,27 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Default Parameters:\n" "Default Parameters:\n",
] "linearizationMode: 0\n",
}, " degeneracyMode: 0\n",
{ "rankTolerance = 1\n",
"ename": "TypeError", "enableEPI = 0\n",
"evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=<LinearizationMode.JACOBIAN_Q: 2>, degeneracyMode=<DegeneracyMode.ZERO_ON_DEGENERACY: 1>, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001", "landmarkDistanceThreshold = -1\n",
"output_type": "error", "dynamicOutlierRejectionThreshold = -1\n",
"traceback": [ "useLOST = 0\n",
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", "noise model\n",
"\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", "\n",
"Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mDefault Parameters:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# default_params.print() # TODO\u001b[39;00m\n\u001b[0;32m 5\u001b[0m \n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Custom parameters\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m custom_params \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionParams\u001b[49m\u001b[43m(\u001b[49m\n\u001b[0;32m 8\u001b[0m \u001b[43m \u001b[49m\u001b[43mlinearizationMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mLinearizationMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mJACOBIAN_Q\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 9\u001b[0m \u001b[43m \u001b[49m\u001b[43mdegeneracyMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mDegeneracyMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mZERO_ON_DEGENERACY\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 10\u001b[0m \u001b[43m \u001b[49m\u001b[43mthrowCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mFalse\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[43m \u001b[49m\u001b[43mverboseCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 12\u001b[0m \u001b[43m \u001b[49m\u001b[43mretriangulationTh\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;241;43m1e-3\u001b[39;49m\n\u001b[0;32m 13\u001b[0m \u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;66;03m# Modify triangulation parameters directly\u001b[39;00m\n\u001b[0;32m 15\u001b[0m custom_params\u001b[38;5;241m.\u001b[39mtriangulation\u001b[38;5;241m.\u001b[39mrankTolerance \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m1e-8\u001b[39m\n", "\n",
"\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=<LinearizationMode.JACOBIAN_Q: 2>, degeneracyMode=<DegeneracyMode.ZERO_ON_DEGENERACY: 1>, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001" "Custom Parameters:\n",
"linearizationMode: 2\n",
" degeneracyMode: 1\n",
"rankTolerance = 1e-08\n",
"enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = 1\n",
"useLOST = 0\n",
"noise model\n",
"\n"
] ]
} }
], ],
@ -115,25 +124,19 @@
"\n", "\n",
"# Custom parameters\n", "# Custom parameters\n",
"custom_params = SmartProjectionParams(\n", "custom_params = SmartProjectionParams(\n",
" linearizationMode = LinearizationMode.JACOBIAN_Q,\n", " linMode = LinearizationMode.JACOBIAN_Q,\n",
" degeneracyMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n", " degMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n",
" throwCheirality = False,\n", " throwCheirality = False,\n",
" verboseCheirality = True,\n", " verboseCheirality = True,\n",
" retriangulationTh = 1e-3\n", " retriangulationTh = 1e-3\n",
")\n", ")\n",
"# Modify triangulation parameters directly\n", "# Modify triangulation parameters directly\n",
"custom_params.triangulation.rankTolerance = 1e-8\n", "custom_params.setRankTolerance(1e-8)\n",
"custom_params.triangulation.enableEPI = False\n", "custom_params.setEnableEPI(False)\n",
"custom_params.triangulation.dynamicOutlierRejectionThreshold = 3.0 # Reject points with reproj error > 3*sigma\n", "custom_params.setDynamicOutlierRejectionThreshold(3.0) # Reject points with reproj error > 3*sigma\n",
"\n", "\n",
"print(\"\\nCustom Parameters:\")\n", "print(\"\\nCustom Parameters:\")\n",
"custom_params.print()\n", "custom_params.print()"
"\n",
"# Accessing parameters\n",
"print(f\"\\nAccessing Custom Params:\")\n",
"print(f\" Linearization Mode: {custom_params.getLinearizationMode()}\")\n",
"print(f\" Degeneracy Mode: {custom_params.getDegeneracyMode()}\")\n",
"print(f\" Rank Tolerance: {custom_params.getTriangulationParameters().rankTolerance}\")"
] ]
}, },
{ {
@ -156,7 +159,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 10,
"metadata": { "metadata": {
"id": "usage_example_code" "id": "usage_example_code"
}, },
@ -171,27 +174,28 @@
"linearizationMode: 2\n", "linearizationMode: 2\n",
"triangulationParameters:\n", "triangulationParameters:\n",
"rankTolerance = 1e-08\n", "rankTolerance = 1e-08\n",
"enableEPI = false\n", "enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n", "landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = 3\n", "dynamicOutlierRejectionThreshold = 1\n",
"\n", "useLOST = 0\n",
"noise model\n",
"\n", "\n",
"result:\n", "result:\n",
"Degenerate\n", "no point, status = 1\n",
"\n", "\n",
"SmartFactorBase, z = \n", "SmartFactorBase, z = \n",
"Factor\n" " keys = { }\n"
] ]
} }
], ],
"source": [ "source": [
"from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n", "from gtsam import SmartProjectionPose3Factor, Cal3_S2\n",
"\n", "\n",
"# Example: Using custom params with a smart factor\n", "# Example: Using custom params with a smart factor\n",
"noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240)\n", "K = Cal3_S2(500, 500, 0, 320, 240)\n",
"\n", "\n",
"smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n", "smart_factor = SmartProjectionPose3Factor(noise, K, custom_params)\n",
"print(\"Smart Factor created with custom params:\")\n", "print(\"Smart Factor created with custom params:\")\n",
"smart_factor.print()" "smart_factor.print()"
] ]

159
gtsam/slam/doc/SmartProjectionPoseFactor.ipynb
View File

@ -54,28 +54,17 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": null,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
"outputs": [ "outputs": [],
{
"ename": "ImportError",
"evalue": "cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n\u001b[0;32m 5\u001b[0m Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
" # SmartProjectionPoseFactor with Cal3_S2 is called SmartProjectionPose3Factor\n",
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n",
" SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n", " SmartProjectionParams, SmartProjectionPose3Factor,\n",
" Cal3_S2)\n", " Cal3_S2)\n",
"from gtsam import symbol_shorthand\n", "from gtsam import symbol_shorthand\n",
"import graphviz\n", "import graphviz\n",
@ -104,7 +93,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 5,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
@ -118,39 +107,45 @@
" SmartProjectionFactor\n", " SmartProjectionFactor\n",
"linearizationMode: 0\n", "linearizationMode: 0\n",
"triangulationParameters:\n", "triangulationParameters:\n",
"rankTolerance = 1e-09\n", "rankTolerance = 1\n",
"enableEPI = false\n", "enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n", "landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n", "dynamicOutlierRejectionThreshold = -1\n",
"\n", "useLOST = 0\n",
"noise model\n",
"\n", "\n",
"result:\n", "result:\n",
"Degenerate\n", "no point, status = 1\n",
"\n", "\n",
"SmartFactorBase, z = \n", "SmartFactorBase, z = \n",
"measurement 0, px = \n", "measurement 0, px = \n",
"[150; 505]\n", "150\n",
"\n", "505\n",
"noise model = diagonal sigmas [1; 1];\n", "noise model = unit (2) \n",
"measurement 1, px = \n", "measurement 1, px = \n",
"[470; 495]\n", "470\n",
"\n", "495\n",
"noise model = diagonal sigmas [1; 1];\n", "noise model = unit (2) \n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"measurement 2, px = \n", "measurement 2, px = \n",
"[480; 150]\n", "480\n",
"\n", "150\n",
"noise model = diagonal sigmas [1; 1];\n", "noise model = unit (2) \n",
"Factor on x0 x1 x2\n" " keys = { x0 x1 x2 }\n"
] ]
} }
], ],
"source": [ "source": [
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams() # Use default params\n", "smart_params = SmartProjectionParams() # Use default params\n",
"K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", "K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
"\n", "\n",
"# Factor type includes the Calibration type\n", "smart_factor = SmartProjectionPose3Factor(smart_noise, K, smart_params)\n",
"smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n",
"\n", "\n",
"# Add measurements and keys (Pose keys)\n", "# Add measurements and keys (Pose keys)\n",
"smart_factor.add(Point2(150, 505), X(0))\n", "smart_factor.add(Point2(150, 505), X(0))\n",
@ -181,7 +176,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 16,
"metadata": { "metadata": {
"id": "eval_example_code" "id": "eval_example_code"
}, },
@ -190,19 +185,19 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Triangulated point result:\n", "Triangulated point result: Status.VALID\n",
"Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", "Triangulated point: [0.28416823 1.95555615 5.67688675]\n",
"\n", "\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" "Total reprojection error (0.5 * sum(err^2/sigma^2)): 120243.1626\n"
] ]
} }
], ],
"source": [ "source": [
"# Create Values containing Pose3 objects\n", "# Create Values containing Pose3 objects\n",
"values = Values()\n", "values = Values()\n",
"pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n", "pose0 = Pose3(Rot3.Ypr(0.1, 0, 0), Point3(-1, 0, 0.5))\n",
"pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n", "pose1 = Pose3(Rot3.Ypr(0.0, -0.1, 0), Point3(1, 0, 0.5))\n",
"pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n", "pose2 = Pose3(Rot3.Ypr(0, 0.0, 0.2), Point3(0, 1, 0.5))\n",
"\n", "\n",
"values.insert(X(0), pose0)\n", "values.insert(X(0), pose0)\n",
"values.insert(X(1), pose1)\n", "values.insert(X(1), pose1)\n",
@ -210,9 +205,10 @@
"\n", "\n",
"# Triangulate first to see the implicit point\n", "# Triangulate first to see the implicit point\n",
"point_result = smart_factor.point(values)\n", "point_result = smart_factor.point(values)\n",
"print(f\"Triangulated point result:\\n{point_result}\")\n", "print(f\"Triangulated point result: {point_result.status}\")\n",
"\n", "\n",
"if point_result.valid():\n", "if point_result.valid():\n",
" print(f\"Triangulated point: {point_result.get()}\")\n",
" # Calculate error\n", " # Calculate error\n",
" total_error = smart_factor.error(values)\n", " total_error = smart_factor.error(values)\n",
" print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
@ -240,7 +236,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": null,
"metadata": { "metadata": {
"id": "linearize_example_code" "id": "linearize_example_code"
}, },
@ -251,61 +247,29 @@
"text": [ "text": [
"\n", "\n",
"Linearized Factor (showing HessianFactor structure):\n", "Linearized Factor (showing HessianFactor structure):\n",
"RegularHessianFactor(6): density=100% keys={ x0 x1 x2 }\n",
"Augmented information matrix: (dimensions: 6, 6, 6) : \n",
"{\n",
"\t[ 1900, 4864, -698, -5014, -4686, -552; ]\n",
"\t[ 4864, 1.337e+04, 1473, -9.522e+03, -1.623e+04, 447; ]\n",
"\t[ -698, 1473, 4803, 3085, -3660, -1.118e+04; ]\n",
"\t[ -5014, -9.522e+03, 3085, 1318, 2669, -1843; ]\n",
"\t[ -4686, -1.623e+04, -3660, 2669, 4137, -1067; ]\n",
"\t[ -552, 447, -1.118e+04, -1843, -1067, 4035; ]\n",
"\t[ 1318, 2669, -1843, 768, -2084, 849; ]\n",
"\t[ 2669, 4137, -1067, -2084, 6054, -1412; ]\n",
"\t[ -1843, -1067, 4035, 849, -1412, 4392; ]\n",
"\t[ 6273, -1.411e+04, -2161, 11515, 16951, 996; ]\n",
"\t[ -141, -1903, -1.299e+04, 230, -2148, 7017; ]\n",
"\t[ -104, 5192, -1500, -373, -620, -7830; ]\n",
"\t[ -830, 6917, -785, -395, -1398, -708; ]\n",
"\t[ -969, 3198, -1569, 587, 1292, -3268; ]\n",
"\t[ 268, -1.286e+04, 1865, -122, -2205, 2048; ]\n",
"\t[ -708, -1569, -7830, 23563, 2137, 1132; ]\n",
"\t[ -321, 3767, -302, 2137, 15054, -1172; ]\n",
"\t[ -1196, 5036, 1563, 1132, -1172, 8384; ]\n",
"}\n",
"Augmented Diagonal Block [0,0]:\n",
"[ 1900, 4864, -698; ]\n",
"[ 4864, 1.337e+04, 1473; ]\n",
"[ -698, 1473, 4803; ]\n",
"\n", "\n",
"Augmented Diagonal Block [1,1]:\n", " keys: x0(6) x1(6) x2(6) \n",
"[ 768, -2084, 849; ]\n", "Augmented information matrix: [\n",
"[ -2084, 6054, -1412; ]\n", "\t202102, 6373.47, -59741.9, 4386.91, -35145.2, 11091.1, -115337, -44197.1, 12233.6, -8181.39, 19268.4, -7521.18, -81683.2, -724.877, 5044.41, -484.005, 14404.6, -5767.48, 89097.4;\n",
"[ 849, -1412, 4392; ]\n", "\t6373.47, 81115.2, -30292.7, 14717.5, -2404.93, -3343.21, 63410.4, 20584.6, -5335.01, 3787.66, -10602.2, 4111.71, -60743.7, -88128.4, 38992.5, -18242.3, 11072.3, -3393.47, 105458;\n",
"\n", "\t-59741.9, -30292.7, 27634, -6415.45, 10844.3, -1981.98, 10553.8, 5348.27, -1607.72, 998.024, -1760.06, 696.401, 44568.2, 31147.8, -15125.4, 6542.51, -7986, 2832.46, -62376.9;\n",
"Augmented Diagonal Block [2,2]:\n", "\t4386.91, 14717.5, -6415.45, 2722.09, -996.496, -424.653, 9577.1, 3000.7, -765.229, 551.36, -1601.54, 620.327, -12253.7, -15890.6, 7106.5, -3294.65, 2225.84, -703.855, 20429.2;\n",
"[ 23563, 2137, 1132; ]\n", "\t-35145.2, -2404.93, 10844.3, -996.496, 6132.48, -1869.56, 18982.5, 7333.62, -2035.73, 1357.9, -3171.11, 1238.23, 15136.7, 1537.88, -1499.49, 376.242, -2675.09, 1054.42, -17138.8;\n",
"[ 2137, 15054, -1172; ]\n", "\t11091.1, -3343.21, -1981.98, -424.653, -1869.56, 777.22, -9389.67, -3428.6, 932.466, -633.633, 1569.05, -611.241, -1827.86, 3981.42, -1495.55, 805.333, 305.895, -169.934, 204.626;\n",
"[ 1132, -1172, 8384; ]\n", "\t-115337, 63410.4, 10553.8, 9577.1, 18982.5, -9389.67, 210063, -32012.2, 19847.7, -6615.86, -35357.8, 12992.1, -83063.6, 2877.07, 3675.5, 245.696, 14633.2, -5901.9, 91375.5;\n",
"\n", "\t-44197.1, 20584.6, 5348.27, 3000.7, 7333.62, -3428.6, -32012.2, 79820.6, -31086.7, 15340.5, 5564.44, -1509.55, 71381, -87978.3, 31244.3, -17668.1, -12223.3, 5946.04, -40235;\n",
"Off-Diagonal Block [0,1]:\n", "\t12233.6, -5335.01, -1607.72, -765.229, -2035.73, 932.466, 19847.7, -31086.7, 12383.1, -5991.81, -3406.72, 1051.42, -29836.9, 33051, -11561.9, 6625.01, 5124.61, -2447.34, 18485.3;\n",
"[ -5014, -4686, -552; ]\n", "\t-8181.39, 3787.66, 998.024, 551.36, 1357.9, -633.633, -6615.86, 15340.5, -5991.81, 2949.34, 1147.27, -319.227, 13846.5, -16832.1, 5966.68, -3379.52, -2372.04, 1151.02, -7909.48;\n",
"[ -9.522e+03, -1.623e+04, 447; ]\n", "\t19268.4, -10602.2, -1760.06, -1601.54, -3171.11, 1569.05, -35357.8, 5564.44, -3406.72, 1147.27, 5951.86, -2185.74, 14119.3, -690.008, -543.913, -82.7916, -2486.55, 1005.27, -15442.2;\n",
"[ 3085, -3660, -1.118e+04; ]\n", "\t-7521.18, 4111.71, 696.401, 620.327, 1238.23, -611.241, 12992.1, -1509.55, 1051.42, -319.227, -2185.74, 806.502, -4768.82, -371.498, 426.945, -95.4627, 842.323, -333.351, 5486.34;\n",
"\n", "\t-81683.2, -60743.7, 44568.2, -12253.7, 15136.7, -1827.86, -83063.6, 71381, -29836.9, 13846.5, 14119.3, -4768.82, 149690, -5708.64, -6412.92, -549.716, -26368.6, 10641.3, -162324;\n",
"Off-Diagonal Block [0,2]:\n", "\t-724.877, -88128.4, 31147.8, -15890.6, 1537.88, 3981.42, 2877.07, -87978.3, 33051, -16832.1, -690.008, -371.498, -5708.64, 160163, -64108.9, 32675.7, 347.645, -2041.12, -63443.5;\n",
"[ 11515, 16951, 996; ]\n", "\t5044.41, 38992.5, -15125.4, 7106.5, -1499.49, -1495.55, 3675.5, 31244.3, -11561.9, 5966.68, -543.913, 426.945, -6412.92, -64108.9, 26167.1, -13114.9, 1394.39, 202.071, 34971.1;\n",
"[ 230, -2148, 7017; ]\n", "\t-484.005, -18242.3, 6542.51, -3294.65, 376.242, 805.333, 245.696, -17668.1, 6625.01, -3379.52, -82.7916, -95.4627, -549.716, 32675.7, -13114.9, 6668.86, -37.4944, -372.943, -13620.5;\n",
"[ -373, -620, -7830; ]\n", "\t14404.6, 11072.3, -7986, 2225.84, -2675.09, 305.895, 14633.2, -12223.3, 5124.61, -2372.04, -2486.55, 842.323, -26368.6, 347.645, 1394.39, -37.4944, 4647.64, -1867.77, 28880.4;\n",
"\n", "\t-5767.48, -3393.47, 2832.46, -703.855, 1054.42, -169.934, -5901.9, 5946.04, -2447.34, 1151.02, 1005.27, -333.351, 10641.3, -2041.12, 202.071, -372.943, -1867.77, 773.191, -10827.4;\n",
"Off-Diagonal Block [1,2]:\n", "\t89097.4, 105458, -62376.9, 20429.2, -17138.8, 204.626, 91375.5, -40235, 18485.3, -7909.48, -15442.2, 5486.34, -162324, -63443.5, 34971.1, -13620.5, 28880.4, -10827.4, 240486\n",
"[ 6273, -830, -969; ]\n", "]\n"
"[ -1.411e+04, 6917, 3198; ]\n",
"[ -2161, -785, -1569; ]\n",
"\n",
"Error vector:\n",
"[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n",
"Constant error term: 103876\n"
] ]
} }
], ],
@ -319,8 +283,7 @@
"if linear_factor:\n", "if linear_factor:\n",
" print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n", " print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n",
" # Cast to HessianFactor to print its details\n", " # Cast to HessianFactor to print its details\n",
" # Note: The factor dimension is Pose3::dimension (6)\n", " hessian_factor = gtsam.HessianFactor(linear_factor)\n",
" hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n",
" if hessian_factor:\n", " if hessian_factor:\n",
" hessian_factor.print()\n", " hessian_factor.print()\n",
" else:\n", " else:\n",

74
gtsam/slam/doc/StereoFactor.ipynb
View File

@ -52,7 +52,7 @@
}, },
"outputs": [], "outputs": [],
"source": [ "source": [
"%pip install --quiet gtsam-develop" "%pip install --quiet gtsam"
] ]
}, },
{ {
@ -66,6 +66,8 @@
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
"from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n", "from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n",
"# Need StereoCamera for backprojection/triangulation\n",
"from gtsam import StereoCamera \n",
"# The Python wrapper often creates specific instantiations\n", "# The Python wrapper often creates specific instantiations\n",
"from gtsam import GenericStereoFactor3D\n", "from gtsam import GenericStereoFactor3D\n",
"from gtsam import symbol_shorthand\n", "from gtsam import symbol_shorthand\n",
@ -101,7 +103,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": 2,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
@ -170,20 +172,23 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 4, "execution_count": 3,
"metadata": { "metadata": {
"id": "eval_example_code" "id": "eval_example_code"
}, },
"outputs": [ "outputs": [
{ {
"ename": "AttributeError", "name": "stdout",
"evalue": "'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'", "output_type": "stream",
"output_type": "error", "text": [
"traceback": [ "Expected landmark point (no offset): [ 1.54225239 -2.27112649 2.95849821]\n",
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", "\n",
"\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", "Error (no offset) at expected landmark: 48664.883462255115 (Should be near zero)\n",
"Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 4\u001b[0m pose \u001b[38;5;241m=\u001b[39m Pose3(Rot3\u001b[38;5;241m.\u001b[39mRodrigues(\u001b[38;5;241m0.1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m0.2\u001b[39m, \u001b[38;5;241m0.3\u001b[39m), Point3(\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m0.5\u001b[39m))\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Triangulate a point that *should* project to measured_stereo\u001b[39;00m\n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m expected_point_camera \u001b[38;5;241m=\u001b[39m \u001b[43mK_stereo\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mbackproject\u001b[49m(measured_stereo)\n\u001b[0;32m 8\u001b[0m landmark \u001b[38;5;241m=\u001b[39m pose\u001b[38;5;241m.\u001b[39mtransformFrom(expected_point_camera)\n\u001b[0;32m 9\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mExpected landmark point: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mlandmark\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", "\n",
"\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'" "Expected landmark point (offset): [ 2.89128008 -3.54882535 1.19789333]\n",
"Error (with offset) at recomputed landmark: 1783675.2295780657 (Should be near zero)\n",
"\n",
"Error (no offset) at noisy landmark: 54320.22670263611\n"
] ]
} }
], ],
@ -192,33 +197,44 @@
"\n", "\n",
"# Example values\n", "# Example values\n",
"pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n", "pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n",
"# Triangulate a point that *should* project to measured_stereo\n",
"# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n",
"expected_point_camera = K_stereo.backproject(measured_stereo)\n",
"landmark = pose.transformFrom(expected_point_camera)\n",
"print(f\"Expected landmark point: {landmark}\")\n",
"\n",
"values.insert(pose_key, pose)\n", "values.insert(pose_key, pose)\n",
"\n",
"# --- Evaluate factor without offset --- \n",
"# Create a StereoCamera object at the current pose\n",
"camera_no_offset = StereoCamera(pose, K_stereo)\n",
"# Triangulate (backproject) the measurement to get the point in the camera frame\n",
"# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n",
"expected_point_camera = camera_no_offset.backproject(measured_stereo) # Point in camera frame\n",
"# Transform the point from the camera frame to the world frame\n",
"landmark = pose.transformFrom(expected_point_camera) # Point in world frame\n",
"print(f\"Expected landmark point (no offset): {landmark}\")\n",
"\n",
"values.insert(landmark_key, landmark)\n", "values.insert(landmark_key, landmark)\n",
"\n",
"# Evaluate factor without offset\n",
"error_no_offset = factor_no_offset.error(values)\n", "error_no_offset = factor_no_offset.error(values)\n",
"print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be zero)\")\n", "print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be near zero)\")\n",
"\n", "\n",
"# Evaluate factor with offset\n", "# --- Evaluate factor with offset --- \n",
"# Need to recompute landmark based on offset pose\n", "# Calculate the actual sensor pose in the world\n",
"pose_with_offset = pose * body_P_sensor # This is world_P_sensor\n", "pose_with_offset = pose.compose(body_P_sensor) # world_P_sensor = world_P_body * body_P_sensor\n",
"expected_point_offset_cam = K_stereo.backproject(measured_stereo)\n", "# Create a StereoCamera object at the sensor pose\n",
"landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam)\n", "camera_with_offset = StereoCamera(pose_with_offset, K_stereo)\n",
"# Triangulate the measurement from the sensor's perspective\n",
"expected_point_offset_cam = camera_with_offset.backproject(measured_stereo) # Point in sensor frame\n",
"# Transform the point from the sensor frame to the world frame\n",
"landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam) # Point in world frame\n",
"print(f\"\\nExpected landmark point (offset): {landmark_offset}\")\n",
"\n",
"# Update the landmark value in Values for the offset factor calculation\n",
"values.update(landmark_key, landmark_offset)\n", "values.update(landmark_key, landmark_offset)\n",
"error_with_offset = factor_with_offset.error(values)\n", "error_with_offset = factor_with_offset.error(values)\n",
"print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be zero)\")\n", "print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be near zero)\")\n",
"\n", "\n",
"# Evaluate with noisy landmark\n", "# --- Evaluate with noisy landmark (using the no-offset factor for simplicity) ---\n",
"noisy_landmark = landmark + Point3(0.1, -0.05, 0.1)\n", "# Use the original landmark calculated for the no-offset case as the 'ground truth'\n",
"noisy_landmark = landmark + Point3(0.1, -0.05, 0.1) \n",
"values.update(landmark_key, noisy_landmark)\n", "values.update(landmark_key, noisy_landmark)\n",
"error_no_offset_noisy = factor_no_offset.error(values)\n", "error_no_offset_noisy = factor_no_offset.error(values)\n",
"print(f\"Error (no offset) at noisy landmark: {error_no_offset_noisy}\")" "print(f\"\\nError (no offset) at noisy landmark: {error_no_offset_noisy}\")"
] ]
} }
], ],