diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index 291cb0a48..51746ce34 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -66,13 +66,19 @@ "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", - " SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n", - " PinholeCameraCal3_S2, Cal3_S2)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "C = symbol_shorthand.C # Key for Camera object" + "from gtsam import (\n", + " Values,\n", + " Point2,\n", + " Point3,\n", + " Pose3,\n", + " Rot3,\n", + " NonlinearFactorGraph,\n", + " SmartProjectionParams,\n", + " SmartProjectionFactorPinholeCameraCal3_S2,\n", + " PinholeCameraCal3_S2,\n", + " Cal3_S2,\n", + ")\n", + "from gtsam.symbol_shorthand import C" ] }, { @@ -102,14 +108,37 @@ }, "outputs": [ { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "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", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'" + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 3 measurements.\n", + "SmartFactor: SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1\n", + "enableEPI = 0\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "useLOST = 0\n", + "noise model\n", + "\n", + "result:\n", + "no point, status = 1\n", + "\n", + "SmartFactorBase, z = \n", + "measurement 0, px = \n", + "150\n", + "505\n", + "noise model = unit (2) \n", + "measurement 1, px = \n", + "470\n", + "495\n", + "noise model = unit (2) \n", + "measurement 2, px = \n", + "480\n", + "150\n", + "noise model = unit (2) \n", + " keys = { c0 c1 c2 }\n" ] } ], @@ -149,7 +178,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 4, "metadata": { "id": "eval_example_code" }, @@ -159,7 +188,7 @@ "output_type": "stream", "text": [ "Triangulated point result:\n", - "Status.DEGENERATE\n", + "Status.BEHIND_CAMERA\n", "\n", "Triangulation failed, error calculation depends on degeneracyMode.\n" ] @@ -188,110 +217,11 @@ "else:\n", " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")" ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_header_md" - }, - "source": [ - "## Linearization" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_desc_md" - }, - "source": [ - "The `.linearize(values)` method creates a linear factor (e.g., `RegularHessianFactor`) connecting the camera variables, effectively marginalizing the implicit point." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": { - "id": "linearize_example_code" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "Linearized Factor (showing HessianFactor structure):\n", - "RegularHessianFactor(9): density=100% keys={ C0 C1 C2 }\n", - "Augmented information matrix: (dimensions: 9, 9, 9) : \n", - "{\n", - "\t[ 4.383e+04, 2.596e+04, -1.466e+04, -6.732e+03, 1.357e+04, -4.327e+03, -2.004e+04, -1.143e+03, -3.459e+03; ]\n", - "\t[ 2.596e+04, 6.879e+04, 5.416e+03, 3.589e+03, -1.075e+04, 2.158e+04, 3.107e+04, -8.167e+03, -1.237e+04; ]\n", - "\t[ -1.466e+04, 5.416e+03, 2.336e+04, -1.026e+03, -3.572e+03, -1.124e+04, -1.497e+04, -5.631e+03, 4.149e+03; ]\n", - "\t[ -6.732e+03, 3.589e+03, -1.026e+03, 7.027e+03, 1.434e+03, 3.742e+03, 2.527e+03, 2.454e+03, -6.619e+03; ]\n", - "\t[ 1.357e+04, -1.075e+04, -3.572e+03, 1.434e+03, 1.511e+04, -8.935e+02, -1.484e+04, 3.811e+03, -1.993e+03; ]\n", - "\t[ -4.327e+03, 2.158e+04, -1.124e+04, 3.742e+03, -8.935e+02, 2.587e+04, 2.085e+04, -1.193e+04, -5.818e+03; ]\n", - "\t[ -2.004e+04, 3.107e+04, -1.497e+04, 2.527e+03, -1.484e+04, 2.085e+04, 3.128e+04, -5.349e+03, -6.557e+03; ]\n", - "\t[ -1.143e+03, -8.167e+03, -5.631e+03, 2.454e+03, 3.811e+03, -1.193e+04, -5.349e+03, 1.537e+04, -1.987e+03; ]\n", - "\t[ -3.459e+03, -1.237e+04, 4.149e+03, -6.619e+03, -1.993e+03, -5.818e+03, -6.557e+03, -1.987e+03, 1.043e+04; ]\n", - "}\n", - "Augmented Diagonal Block [0,0]:\n", - "[ 4.383e+04, 2.596e+04, -1.466e+04; ]\n", - "[ 2.596e+04, 6.879e+04, 5.416e+03; ]\n", - "[ -1.466e+04, 5.416e+03, 2.336e+04; ]\n", - "\n", - "Augmented Diagonal Block [1,1]:\n", - "[ 7.027e+03, 1.434e+03, 3.742e+03; ]\n", - "[ 1.434e+03, 1.511e+04, -8.935e+02; ]\n", - "[ 3.742e+03, -8.935e+02, 2.587e+04; ]\n", - "\n", - "Augmented Diagonal Block [2,2]:\n", - "[ 3.128e+04, -5.349e+03, -6.557e+03; ]\n", - "[ -5.349e+03, 1.537e+04, -1.987e+03; ]\n", - "[ -6.557e+03, -1.987e+03, 1.043e+04; ]\n", - "\n", - "Off-Diagonal Block [0,1]:\n", - "[ -6.732e+03, 1.357e+04, -4.327e+03; ]\n", - "[ 3.589e+03, -1.075e+04, 2.158e+04; ]\n", - "[ -1.026e+03, -3.572e+03, -1.124e+04; ]\n", - "\n", - "Off-Diagonal Block [0,2]:\n", - "[ -2.004e+04, -1.143e+03, -3.459e+03; ]\n", - "[ 3.107e+04, -8.167e+03, -1.237e+04; ]\n", - "[ -1.497e+04, -5.631e+03, 4.149e+03; ]\n", - "\n", - "Off-Diagonal Block [1,2]:\n", - "[ 2.527e+03, 2.454e+03, -6.619e+03; ]\n", - "[ -1.484e+04, 3.811e+03, -1.993e+03; ]\n", - "[ 2.085e+04, -1.193e+04, -5.818e+03; ]\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" - ] - } - ], - "source": [ - "graph = NonlinearFactorGraph()\n", - "graph.add(smart_factor)\n", - "\n", - "# Linearize (default mode is HESSIAN)\n", - "linear_factor = smart_factor.linearize(values)\n", - "\n", - "if linear_factor:\n", - " print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n", - " # Cast to HessianFactor to print its details\n", - " hessian_factor = gtsam.RegularHessianFactorCal3_S2.Downcast(linear_factor)\n", - " if hessian_factor:\n", - " hessian_factor.print()\n", - " else:\n", - " print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n", - "else:\n", - " print(\"Linearization failed (likely due to triangulation degeneracy)\")" - ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -305,7 +235,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/doc/SmartProjectionParams.ipynb b/gtsam/slam/doc/SmartProjectionParams.ipynb index 36e840cb8..cdf1d7291 100644 --- a/gtsam/slam/doc/SmartProjectionParams.ipynb +++ b/gtsam/slam/doc/SmartProjectionParams.ipynb @@ -48,7 +48,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": { "id": "pip_code", "tags": [ @@ -57,12 +57,16 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 3, "metadata": { "id": "imports_code" }, @@ -83,7 +87,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 4, "metadata": { "id": "create_example_code" }, @@ -159,7 +163,7 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 5, "metadata": { "id": "usage_example_code" }, @@ -189,13 +193,13 @@ } ], "source": [ - "from gtsam import SmartProjectionPose3Factor, Cal3_S2\n", + "from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n", "\n", "# Example: Using custom params with a smart factor\n", "noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "K = Cal3_S2(500, 500, 0, 320, 240)\n", "\n", - "smart_factor = SmartProjectionPose3Factor(noise, K, custom_params)\n", + "smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n", "print(\"Smart Factor created with custom params:\")\n", "smart_factor.print()" ] @@ -203,7 +207,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -217,7 +221,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index c8d7f3b2e..f48ff9191 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -40,7 +40,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -49,27 +49,34 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": { "id": "imports_code" }, "outputs": [], "source": [ "import gtsam\n", - "import numpy as np\n", - " # SmartProjectionPoseFactor with Cal3_S2 is called SmartProjectionPose3Factor\n", - "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", - " SmartProjectionParams, SmartProjectionPose3Factor,\n", - " Cal3_S2)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "X = symbol_shorthand.X # Key for Pose3 variable" + "from gtsam import (\n", + " Values,\n", + " Point2,\n", + " Point3,\n", + " Pose3,\n", + " Rot3,\n", + " NonlinearFactorGraph,\n", + " SmartProjectionParams,\n", + " SmartProjectionPoseFactorCal3_S2,\n", + " Cal3_S2,\n", + ")\n", + "from gtsam.symbol_shorthand import X # Key for Pose3 variable" ] }, { @@ -93,7 +100,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 3, "metadata": { "id": "create_example_code" }, @@ -125,13 +132,7 @@ "measurement 1, px = \n", "470\n", "495\n", - "noise model = unit (2) \n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ + "noise model = unit (2) \n", "measurement 2, px = \n", "480\n", "150\n", @@ -145,7 +146,7 @@ "smart_params = SmartProjectionParams() # Use default params\n", "K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", "\n", - "smart_factor = SmartProjectionPose3Factor(smart_noise, K, smart_params)\n", + "smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n", "\n", "# Add measurements and keys (Pose keys)\n", "smart_factor.add(Point2(150, 505), X(0))\n", @@ -176,7 +177,7 @@ }, { "cell_type": "code", - "execution_count": 16, + "execution_count": 4, "metadata": { "id": "eval_example_code" }, @@ -236,7 +237,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 5, "metadata": { "id": "linearize_example_code" }, @@ -295,7 +296,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -309,7 +310,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 911f4fd9a..b733d0c6a 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -152,6 +152,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { void serialize() const; }; +// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified} +#include + +#include + +template +virtual class SmartFactorBase : gtsam::NonlinearFactor { + void add(const gtsam::Point2& measured, size_t key); + void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys); + size_t dim() const; + const std::vector& measured() const; + std::vector cameras(const gtsam::Values& values) const; +}; + #include /// Linearization mode: what factor to linearize to @@ -176,8 +193,11 @@ class SmartProjectionParams { void print(const std::string& str = "") const; }; -template }> -class SmartProjectionFactor : gtsam::NonlinearFactor { +template +virtual class SmartProjectionFactor : gtsam::SmartFactorBase { SmartProjectionFactor(); SmartProjectionFactor( @@ -238,7 +258,9 @@ class SmartProjectionFactor : gtsam::NonlinearFactor { }; #include -template +// We are not deriving from SmartProjectionFactor yet - too complicated in wrapper +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, const CALIBRATION* K); @@ -262,9 +284,6 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { gtsam::TriangulationResult point(const gtsam::Values& values) const; }; -typedef gtsam::SmartProjectionPoseFactor - SmartProjectionPose3Factor; - // WIP // #include // typedef gtsam::PinholeCamera PinholeCameraCal3_S2; diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 037065ac5..eea1994f2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -38,10 +38,10 @@ for i=0:length(measurements) projectionFactorSeenBy = []; if options.includeCameraFactors == 1 for j=1:options.numberOfLandmarks - SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01); + SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2(0.01); % Use constructor with default values, but express the pose of the % camera as a 90 degree rotation about the X axis -% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ... +% SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2( ... % 1, ... % rankTol % -1, ... % linThreshold % false, ... % manageDegeneracy