diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index bd17958e2..2bce88bdd 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -12,6 +12,7 @@ namespace gtsam { #include #include #include +#include #include #include #include @@ -94,6 +95,7 @@ class Values { void insert(size_t j, const gtsam::EssentialMatrix& E); void insert(size_t j, const gtsam::FundamentalMatrix& F); void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert(size_t j, const gtsam::OrientedPlane3& plane); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); @@ -138,6 +140,7 @@ class Values { void update(size_t j, const gtsam::EssentialMatrix& E); void update(size_t j, const gtsam::FundamentalMatrix& F); void update(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void update(size_t j, const gtsam::OrientedPlane3& plane); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); @@ -179,6 +182,7 @@ class Values { void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E); void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F); void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert_or_assign(size_t j, const gtsam::OrientedPlane3& plane); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); @@ -216,6 +220,7 @@ class Values { gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix, + gtsam::OrientedPlane3, gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb index 7815b844f..e44d3b5a6 100644 --- a/gtsam/slam/doc/OrientedPlane3Factor.ipynb +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -34,7 +34,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -43,7 +43,11 @@ }, "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" ] }, { @@ -82,7 +86,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 3, "metadata": { "id": "factor_example_code" }, @@ -94,18 +98,11 @@ "OrientedPlane3Factor: \n", "OrientedPlane3Factor Factor (x0, p0)\n", "Measured Plane : 0 0 1 1\n", - "isotropic dim=3 sigma=0.05\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) -> 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) -> 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" + "isotropic dim=3 sigma=0.05\n", + "\n", + "Error at ground truth: 0.0\n", + "\n", + "Error with noisy plane: 0.6469041114912286\n" ] } ], @@ -158,12 +155,12 @@ }, "source": [ "A unary factor that puts a prior on the *direction* (normal vector) of an `OrientedPlane3`. The distance component (`d`) of the measured plane is ignored.\n", - "The error is the 3D difference between the estimated plane's normal and the measured plane's normal." + "The error is the difference between the estimated plane's normal and the measured plane's normal, but as directions only have 2 DOF, the noise model also has to have dimension 2." ] }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 4, "metadata": { "id": "prior_example_code" }, @@ -175,25 +172,17 @@ "OrientedPlane3DirectionPrior: \n", "OrientedPlane3DirectionPrior: Prior Factor on p0\n", "Measured Plane : 0 0 1 0\n", - "isotropic dim=3 sigma=0.02\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." + "isotropic dim=2 sigma=0.02\n", + "\n", + "Error for prior on noisy_plane: 0.2550239722533919\n", + "Error for prior on gt_plane_world: 0.0\n" ] } ], "source": [ "# Measured direction prior (e.g., plane normal is close to world Z axis)\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(2, 0.02)\n", "\n", "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), direction_noise)\n", "prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n", @@ -211,7 +200,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -225,7 +214,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4,