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

Add OrientedPlane3 to Values wrapper

release/4.3a0
Frank Dellaert 2025-04-26 22:36:53 -04:00
parent d245a7afde
commit 502ff2762b
2 changed files with 26 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/nonlinear/values.i
View File

@ -12,6 +12,7 @@ namespace gtsam {
#include <gtsam/geometry/Cal3Unified.h> #include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/FundamentalMatrix.h> #include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
@ -94,6 +95,7 @@ class Values {
void insert(size_t j, const gtsam::EssentialMatrix& E); 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::FundamentalMatrix& F);
void insert(size_t j, const gtsam::SimpleFundamentalMatrix& 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<gtsam::Cal3Bundler>& camera); void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera); void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera); void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@ -138,6 +140,7 @@ class Values {
void update(size_t j, const gtsam::EssentialMatrix& E); 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::FundamentalMatrix& F);
void update(size_t j, const gtsam::SimpleFundamentalMatrix& 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<gtsam::Cal3Bundler>& camera); void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera); void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera); void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& 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::EssentialMatrix& E);
void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F); 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::SimpleFundamentalMatrix& F);
void insert_or_assign(size_t j, const gtsam::OrientedPlane3& plane);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@ -216,6 +220,7 @@ class Values {
gtsam::EssentialMatrix, gtsam::EssentialMatrix,
gtsam::FundamentalMatrix, gtsam::FundamentalMatrix,
gtsam::SimpleFundamentalMatrix, gtsam::SimpleFundamentalMatrix,
gtsam::OrientedPlane3,
gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3f>, gtsam::PinholeCamera<gtsam::Cal3f>,
gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::PinholeCamera<gtsam::Cal3_S2>,

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

@ -34,7 +34,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "pip_code", "id": "pip_code",
"tags": [ "tags": [
@ -43,7 +43,11 @@
}, },
"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"
] ]
}, },
{ {
@ -82,7 +86,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 4, "execution_count": 3,
"metadata": { "metadata": {
"id": "factor_example_code" "id": "factor_example_code"
}, },
@ -94,18 +98,11 @@
"OrientedPlane3Factor: \n", "OrientedPlane3Factor: \n",
"OrientedPlane3Factor Factor (x0, p0)\n", "OrientedPlane3Factor Factor (x0, p0)\n",
"Measured Plane : 0 0 1 1\n", "Measured Plane : 0 0 1 1\n",
"isotropic dim=3 sigma=0.05\n" "isotropic dim=3 sigma=0.05\n",
] "\n",
}, "Error at ground truth: 0.0\n",
{ "\n",
"ename": "TypeError", "Error with noisy plane: 0.6469041114912286\n"
"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"
] ]
} }
], ],
@ -158,12 +155,12 @@
}, },
"source": [ "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", "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", "cell_type": "code",
"execution_count": 6, "execution_count": 4,
"metadata": { "metadata": {
"id": "prior_example_code" "id": "prior_example_code"
}, },
@ -175,25 +172,17 @@
"OrientedPlane3DirectionPrior: \n", "OrientedPlane3DirectionPrior: \n",
"OrientedPlane3DirectionPrior: Prior Factor on p0\n", "OrientedPlane3DirectionPrior: Prior Factor on p0\n",
"Measured Plane : 0 0 1 0\n", "Measured Plane : 0 0 1 0\n",
"isotropic dim=3 sigma=0.02\n" "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"
"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."
] ]
} }
], ],
"source": [ "source": [
"# Measured direction prior (e.g., plane normal is close to world Z axis)\n", "# 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", "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", "\n",
"prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), 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",
@ -211,7 +200,7 @@
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "gtsam", "display_name": "py312",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -225,7 +214,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.13.1" "version": "3.12.6"
} }
}, },
"nbformat": 4, "nbformat": 4,