From c28c9b58d6442170b980feb4000ccd7411bd48d5 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 24 Apr 2025 10:52:25 -0400 Subject: [PATCH 01/17] New SLAM notebooks --- gtsam/slam/doc/BetweenFactor.ipynb | 322 ++++++++++++++ .../slam/doc/EssentialMatrixConstraint.ipynb | 212 +++++++++ gtsam/slam/doc/EssentialMatrixFactor.ipynb | 392 ++++++++++++++++ gtsam/slam/doc/FrobeniusFactor.ipynb | 269 +++++++++++ gtsam/slam/doc/GeneralSFMFactor.ipynb | 223 ++++++++++ gtsam/slam/doc/InitializePose3.ipynb | 256 +++++++++++ gtsam/slam/doc/KarcherMeanFactor.ipynb | 245 ++++++++++ gtsam/slam/doc/OrientedPlane3Factor.ipynb | 230 ++++++++++ gtsam/slam/doc/PlanarProjectionFactor.ipynb | 293 ++++++++++++ gtsam/slam/doc/PoseRotationPrior.ipynb | 215 +++++++++ gtsam/slam/doc/PoseTranslationPrior.ipynb | 214 +++++++++ gtsam/slam/doc/ProjectionFactor.ipynb | 238 ++++++++++ gtsam/slam/doc/ReferenceFrameFactor.ipynb | 219 +++++++++ gtsam/slam/doc/RotateFactor.ipynb | 229 ++++++++++ gtsam/slam/doc/SmartFactorParams.ipynb | 221 +++++++++ gtsam/slam/doc/SmartProjectionFactor.ipynb | 343 ++++++++++++++ .../slam/doc/SmartProjectionPoseFactor.ipynb | 354 +++++++++++++++ gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 418 ++++++++++++++++++ gtsam/slam/doc/StereoFactor.ipynb | 246 +++++++++++ gtsam/slam/doc/TriangulationFactor.ipynb | 283 ++++++++++++ gtsam/slam/doc/dataset.ipynb | 252 +++++++++++ gtsam/slam/doc/lago.ipynb | 242 ++++++++++ 22 files changed, 5916 insertions(+) create mode 100644 gtsam/slam/doc/BetweenFactor.ipynb create mode 100644 gtsam/slam/doc/EssentialMatrixConstraint.ipynb create mode 100644 gtsam/slam/doc/EssentialMatrixFactor.ipynb create mode 100644 gtsam/slam/doc/FrobeniusFactor.ipynb create mode 100644 gtsam/slam/doc/GeneralSFMFactor.ipynb create mode 100644 gtsam/slam/doc/InitializePose3.ipynb create mode 100644 gtsam/slam/doc/KarcherMeanFactor.ipynb create mode 100644 gtsam/slam/doc/OrientedPlane3Factor.ipynb create mode 100644 gtsam/slam/doc/PlanarProjectionFactor.ipynb create mode 100644 gtsam/slam/doc/PoseRotationPrior.ipynb create mode 100644 gtsam/slam/doc/PoseTranslationPrior.ipynb create mode 100644 gtsam/slam/doc/ProjectionFactor.ipynb create mode 100644 gtsam/slam/doc/ReferenceFrameFactor.ipynb create mode 100644 gtsam/slam/doc/RotateFactor.ipynb create mode 100644 gtsam/slam/doc/SmartFactorParams.ipynb create mode 100644 gtsam/slam/doc/SmartProjectionFactor.ipynb create mode 100644 gtsam/slam/doc/SmartProjectionPoseFactor.ipynb create mode 100644 gtsam/slam/doc/SmartProjectionRigFactor.ipynb create mode 100644 gtsam/slam/doc/StereoFactor.ipynb create mode 100644 gtsam/slam/doc/TriangulationFactor.ipynb create mode 100644 gtsam/slam/doc/dataset.ipynb create mode 100644 gtsam/slam/doc/lago.ipynb diff --git a/gtsam/slam/doc/BetweenFactor.ipynb b/gtsam/slam/doc/BetweenFactor.ipynb new file mode 100644 index 000000000..4eef414bb --- /dev/null +++ b/gtsam/slam/doc/BetweenFactor.ipynb @@ -0,0 +1,322 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# BetweenFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`BetweenFactor` is a fundamental factor in GTSAM used to represent measurements of the relative transformation (motion) between two variables of the same type `VALUE`.\n", + "Common examples include:\n", + "* `BetweenFactorPose2`: Represents odometry measurements between 2D robot poses.\n", + "* `BetweenFactorPose3`: Represents odometry or relative pose estimates between 3D poses.\n", + "\n", + "The `VALUE` type must be a Lie Group (e.g., `Pose2`, `Pose3`, `Rot2`, `Rot3`).\n", + "\n", + "The factor encodes a constraint based on the measurement `measured_`, such that the error is calculated based on the difference between the predicted relative transformation and the measured one. Specifically, if the factor connects keys $k_1$ and $k_2$ corresponding to values $X_1$ and $X_2$, and the measurement is $Z$, the factor aims to minimize:\n", + "\n", + "$$ \\text{error}(X_1, X_2) = \\text{Log}(Z^{-1} \\cdot (X_1^{-1} \\cdot X_2)) $$\n", + "\n", + "where `Log` is the logarithmic map for the Lie group `VALUE`, $X_1^{-1} \\cdot X_2$ is the predicted relative transformation `between(X1, X2)`, and $Z^{-1}$ is the inverse of the measurement. The error vector lives in the tangent space of the identity element of the group.\n", + "\n", + "`BetweenConstraint` is a derived class that uses a `noiseModel::Constrained` noise model, effectively enforcing the relative transformation to be exactly the measured value." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import BetweenFactorPose2, BetweenFactorPose3\n", + "from gtsam import Pose2, Pose3, Rot3, Point3\n", + "from gtsam import NonlinearFactorGraph, Values\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a BetweenFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "You create a `BetweenFactor` by specifying:\n", + "1. The keys of the two variables it connects (e.g., `X(0)`, `X(1)`).\n", + "2. The measured relative transformation (e.g., a `Pose2` or `Pose3`).\n", + "3. A noise model describing the uncertainty of the measurement." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BetweenFactorPose2: BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "BetweenFactorPose3: BetweenFactor(x1,x2)\n", + " measured: R: [\n", + "\t0.995004165, -0.0998334166, 0;\n", + "\t0.0998334166, 0.995004165, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.5 0 0\n", + " noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.1; 0.1; 0.1];\n" + ] + } + ], + "source": [ + "# Example for Pose2 (2D SLAM odometry)\n", + "key1 = X(0)\n", + "key2 = X(1)\n", + "measured_pose2 = Pose2(1.0, 0.0, 0.0) # Move 1 meter forward\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", + "\n", + "between_factor_pose2 = BetweenFactorPose2(key1, key2, measured_pose2, odometry_noise)\n", + "between_factor_pose2.print(\"BetweenFactorPose2: \")\n", + "\n", + "# Example for Pose3 (3D SLAM odometry)\n", + "measured_pose3 = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0)) # Move 0.5m forward, yaw 0.1 rad\n", + "odometry_noise_3d = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05, 0.1, 0.1, 0.1]))\n", + "\n", + "between_factor_pose3 = BetweenFactorPose3(X(1), X(2), measured_pose3, odometry_noise_3d)\n", + "between_factor_pose3.print(\"\\nBetweenFactorPose3: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method calculates the error vector based on the current estimates of the variables in the `Values` object." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error vector for BetweenFactorPose2: 0.3750000000000002\n", + "Manual unwhitened error calculation: [0.10247917 0.09747917 0.05 ]\n", + "Factor unwhitened error: [0.1 0.1 0.05]\n", + "Manually whitened error: [0.51239583 0.48739583 0.5 ]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "values.insert(X(0), Pose2(0.0, 0.0, 0.0))\n", + "values.insert(X(1), Pose2(1.1, 0.1, 0.05)) # Slightly off from measurement\n", + "\n", + "# Evaluate the error for the Pose2 factor\n", + "error_vector = between_factor_pose2.error(values)\n", + "print(f\"Error vector for BetweenFactorPose2: {error_vector}\")\n", + "\n", + "# The unwhitened error is Log(Z^-1 * (X1^-1 * X2))\n", + "pose0 = values.atPose2(X(0))\n", + "pose1 = values.atPose2(X(1))\n", + "predicted_relative = pose0.between(pose1)\n", + "error_pose = measured_pose2.inverse() * predicted_relative\n", + "unwhitened_error_expected = Pose2.Logmap(error_pose)\n", + "print(f\"Manual unwhitened error calculation: {unwhitened_error_expected}\")\n", + "print(f\"Factor unwhitened error: {between_factor_pose2.unwhitenedError(values)}\")\n", + "\n", + "# The whitened error (returned by error()) applies the noise model\n", + "# For diagonal noise model, error_vector = unwhitened_error / sigmas\n", + "sigmas = odometry_noise.sigmas()\n", + "whitened_expected = unwhitened_error_expected / sigmas\n", + "print(f\"Manually whitened error: {whitened_expected}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "viz_header_md" + }, + "source": [ + "## Visualization" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "viz_example_code" + }, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor1\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "graph.add(between_factor_pose2)\n", + "graph.add(between_factor_pose3)\n", + "\n", + "dot_string = graph.dot(values)\n", + "graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/EssentialMatrixConstraint.ipynb b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb new file mode 100644 index 000000000..fe54c0515 --- /dev/null +++ b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb @@ -0,0 +1,212 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# EssentialMatrixConstraint" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`EssentialMatrixConstraint` is a binary factor connecting two `Pose3` variables.\n", + "It represents a constraint derived from a measured Essential matrix ($E$) between the two camera views corresponding to the poses.\n", + "The Essential matrix $E$ encapsulates the relative rotation $R$ and translation direction $t$ between two *calibrated* cameras according to the epipolar constraint:\n", + "$$ p_2^T E p_1 = 0 $$\n", + "where $p_1$ and $p_2$ are the homogeneous coordinates of corresponding points in the *normalized (calibrated)* image planes.\n", + "\n", + "This factor takes the measured $E_{12}$ (from pose 1 to pose 2) and compares it to the Essential matrix predicted from the estimated poses $P_1$ and $P_2$.\n", + "The error is computed in the 5-dimensional tangent space of the Essential matrix manifold.\n", + "\n", + "**Note:** This factor requires known camera calibration, as the Essential matrix operates on normalized image coordinates." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import EssentialMatrixConstraint, EssentialMatrix, Pose3, Rot3, Point3, Values\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating an EssentialMatrixConstraint" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "To create the factor, provide:\n", + "1. Keys for the two `Pose3` variables (e.g., `X(1)`, `X(2)`).\n", + "2. The measured `gtsam.EssentialMatrix` ($E_{12}$).\n", + "3. A 5-dimensional noise model (`gtsam.noiseModel`)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "EssentialMatrixConstraint: EssentialMatrixConstraint(x1,x2)\n", + " measured: R:\n", + " [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "d: :1\n", + "0\n", + "0\n", + "isotropic dim=5 sigma=0.01\n" + ] + } + ], + "source": [ + "# Assume we have two poses\n", + "pose1 = Pose3(Rot3.Yaw(0.0), Point3(0, 0, 0))\n", + "pose2 = Pose3(Rot3.Yaw(0.1), Point3(1, 0, 0))\n", + "\n", + "# Calculate the ground truth Essential matrix between them\n", + "gt_E12 = EssentialMatrix.FromPose3(pose1.between(pose2))\n", + "\n", + "# Add some noise (conceptual, E lives on a manifold)\n", + "# In practice, E would be estimated from image correspondences\n", + "measured_E = gt_E12 # Use ground truth for this example\n", + "\n", + "# Define a noise model (5 dimensional!)\n", + "noise_dim = 5\n", + "E_noise = gtsam.noiseModel.Isotropic.Sigma(noise_dim, 0.01)\n", + "\n", + "# Create the factor\n", + "key1 = X(1)\n", + "key2 = X(2)\n", + "factor = EssentialMatrixConstraint(key1, key2, measured_E, E_noise)\n", + "\n", + "factor.print(\"EssentialMatrixConstraint: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method computes the error vector in the 5D tangent space of the Essential matrix manifold. The error represents the difference between the measured E and the E predicted from the current pose estimates in `values`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error vector at ground truth: 0.0\n", + "Error vector at noisy pose: 1.4069198000486227\n" + ] + } + ], + "source": [ + "values = Values()\n", + "# Insert values close to ground truth\n", + "values.insert(key1, pose1)\n", + "values.insert(key2, pose2)\n", + "\n", + "error_vector = factor.error(values)\n", + "print(f\"Error vector at ground truth: {error_vector}\")\n", + "\n", + "# Insert values slightly different\n", + "noisy_pose2 = Pose3(Rot3.Yaw(0.11), Point3(1.05, 0.01, -0.01))\n", + "values.update(key2, noisy_pose2)\n", + "\n", + "error_vector_noisy = factor.error(values)\n", + "print(f\"Error vector at noisy pose: {error_vector_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/EssentialMatrixFactor.ipynb b/gtsam/slam/doc/EssentialMatrixFactor.ipynb new file mode 100644 index 000000000..cac43612c --- /dev/null +++ b/gtsam/slam/doc/EssentialMatrixFactor.ipynb @@ -0,0 +1,392 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# EssentialMatrixFactor Variants" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines several factors related to the Essential matrix ($E$), which encodes the relative rotation and translation direction between two *calibrated* cameras.\n", + "They are primarily used in Structure from Motion (SfM) problems where point correspondences between views are available but the 3D structure and/or camera poses/calibration are unknown.\n", + "\n", + "The core constraint is the epipolar constraint: $p_2^T E p_1 = 0$, where $p_1$ and $p_2$ are corresponding points in *normalized (calibrated)* image coordinates.\n", + "\n", + "Factors defined here:\n", + "* `EssentialMatrixFactor`: Constrains an unknown `EssentialMatrix` variable using a single point correspondence $(p_1, p_2)$.\n", + "* `EssentialMatrixFactor2`: Constrains an `EssentialMatrix` and an unknown inverse depth variable using a point correspondence.\n", + "* `EssentialMatrixFactor3`: Like Factor2, but incorporates an additional fixed extrinsic rotation (useful for camera rigs).\n", + "* `EssentialMatrixFactor4`: Constrains an `EssentialMatrix` and a *shared* `CALIBRATION` variable using a point correspondence given in *pixel* coordinates.\n", + "* `EssentialMatrixFactor5`: Constrains an `EssentialMatrix` and *two* unknown `CALIBRATION` variables (one for each camera) using a point correspondence given in *pixel* coordinates." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (EssentialMatrix, Point2, Point3, Rot3, Unit3, Pose3, Cal3_S2, EssentialMatrixFactor,\n", + " EssentialMatrixFactor2, EssentialMatrixFactor3, EssentialMatrixFactor4Cal3_S2,\n", + " EssentialMatrixFactor5Cal3_S2, Values)\n", + "from gtsam import symbol_shorthand\n", + "\n", + "E = symbol_shorthand.E\n", + "K = symbol_shorthand.K\n", + "D = symbol_shorthand.D # For inverse depth" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `EssentialMatrixFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "This factor involves a single `EssentialMatrix` variable. It takes a pair of corresponding points $(p_A, p_B)$ in *normalized (calibrated)* image coordinates and penalizes deviations from the epipolar constraint $p_B^T E p_A = 0$.\n", + "The error is $p_B^T E p_A$." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor 1: keys = { e0 }\n", + "isotropic dim=1 sigma=0.01\n", + " EssentialMatrixFactor with measurements\n", + " (0.5 0.2 1)' and ( 0.4 0.25 1)'\n", + "\n", + "Error for Factor 1: 12.499999999999995\n" + ] + } + ], + "source": [ + "# Assume normalized coordinates\n", + "pA_calibrated = Point2(0.5, 0.2)\n", + "pB_calibrated = Point2(0.4, 0.25)\n", + "\n", + "# Noise model on the epipolar error (scalar)\n", + "epipolar_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)\n", + "\n", + "# Key for the unknown Essential Matrix\n", + "keyE = E(0)\n", + "\n", + "factor1 = EssentialMatrixFactor(keyE, pA_calibrated, pB_calibrated, epipolar_noise)\n", + "factor1.print(\"Factor 1: \")\n", + "\n", + "# Evaluate error (requires an EssentialMatrix value)\n", + "values = Values()\n", + "# Example: E for identity rotation, translation (1,0,0)\n", + "example_E = EssentialMatrix(Rot3(), Unit3(1, 0, 0))\n", + "values.insert(keyE, example_E)\n", + "error1 = factor1.error(values)\n", + "print(f\"\\nError for Factor 1: {error1}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `EssentialMatrixFactor2`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "This factor involves an `EssentialMatrix` variable and a `double` variable representing the *inverse depth* of the 3D point corresponding to the measurement $p_A$ in the first camera's frame.\n", + "It assumes the measurement $p_B$ is perfect and calculates the reprojection error of the point (reconstructed using $p_A$ and the inverse depth) in the first camera image, after projecting it into the second camera and back.\n", + "It requires point correspondences $(p_A, p_B)$, which can be provided in either calibrated or pixel coordinates (if a calibration object `K` is provided).\n", + "The error is a 2D reprojection error in the first image plane (typically in pixels if K is provided)." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 2: keys = { e0 d0 }\n", + " noise model: unit (2) \n", + " EssentialMatrixFactor2 with measurements\n", + " (480 288 1)' and (464 312)'\n", + "\n", + "Error for Factor 2: 412.82000000000016\n" + ] + } + ], + "source": [ + "# Assume pixel coordinates and known calibration\n", + "K_cal = Cal3_S2(500, 500, 0, 320, 240)\n", + "pA_pixels = Point2(480, 288) # Corresponds to (0.5, 0.2) calibrated\n", + "pB_pixels = Point2(464, 312) # Corresponds to (0.4, 0.25) calibrated\n", + "\n", + "# Noise model on the 2D reprojection error (pixels)\n", + "reprojection_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "\n", + "# Key for inverse depth\n", + "keyD = D(0)\n", + "\n", + "factor2 = EssentialMatrixFactor2(keyE, keyD, pA_pixels, pB_pixels, reprojection_noise)\n", + "factor2.print(\"\\nFactor 2: \")\n", + "\n", + "# Evaluate error (requires E and inverse depth d)\n", + "values.insert(keyD, 0.2) # Assume inverse depth d = 1/Z = 1/5 = 0.2\n", + "error2 = factor2.error(values)\n", + "print(f\"\\nError for Factor 2: {error2}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_header_md" + }, + "source": [ + "## 3. `EssentialMatrixFactor3`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_desc_md" + }, + "source": [ + "This is identical to `EssentialMatrixFactor2` but includes an additional fixed `Rot3` representing the rotation from the 'body' frame (where the Essential matrix is defined) to the 'camera' frame (where the measurements are made).\n", + "`iRc`: Rotation from camera frame to body frame (inverse of body-to-camera).\n", + "The Essential matrix $E_{body}$ is transformed to the camera frame before use: $E_{camera} = R_{cRb} \\cdot E_{body}$." + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": { + "id": "factor3_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 3: keys = { e0 d0 }\n", + " noise model: unit (2) \n", + " EssentialMatrixFactor2 with measurements\n", + " (480 288 1)' and (464 312)'\n", + " EssentialMatrixFactor3 with rotation [\n", + "\t0.99875, -0.0499792, 0;\n", + "\t0.0499792, 0.99875, 0;\n", + "\t0, 0, 1\n", + "]\n", + "\n", + "Error for Factor 3: 413.0638991792357\n" + ] + } + ], + "source": [ + "body_R_cam = Rot3.Yaw(0.05) # Example fixed rotation\n", + "\n", + "factor3 = EssentialMatrixFactor3(keyE, keyD, pA_pixels, pB_pixels, body_R_cam, reprojection_noise)\n", + "factor3.print(\"\\nFactor 3: \")\n", + "\n", + "# Evaluate error (uses same E and d from values)\n", + "error3 = factor3.error(values)\n", + "print(f\"\\nError for Factor 3: {error3}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor4_header_md" + }, + "source": [ + "## 4. `EssentialMatrixFactor4`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor4_desc_md" + }, + "source": [ + "This factor involves an `EssentialMatrix` variable and a single unknown `CALIBRATION` variable (e.g., `Cal3_S2`) that is assumed to be **shared** by both cameras.\n", + "It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n", + "The error is the algebraic epipolar error $ (K^{-1} p_B)^T E (K^{-1} p_A) $.\n", + "\n", + "**Note:** Recovering calibration from 2D correspondences alone is often ill-posed. This factor typically requires strong priors on the calibration." + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": { + "id": "factor4_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 4: keys = { e0 k0 }\n", + "isotropic dim=1 sigma=0.01\n", + " EssentialMatrixFactor4 with measurements\n", + " (480 288)' and (464 312)'\n", + "\n", + "Error for Factor 4: 11.520000000000007\n" + ] + } + ], + "source": [ + "# Key for the unknown shared Calibration\n", + "keyK = K(0)\n", + "\n", + "factor4 = EssentialMatrixFactor4Cal3_S2(keyE, keyK, pA_pixels, pB_pixels, epipolar_noise)\n", + "factor4.print(\"\\nFactor 4: \")\n", + "\n", + "# Evaluate error (requires E and K)\n", + "values.insert(keyK, K_cal) # Use the known K for this example\n", + "error4 = factor4.error(values)\n", + "print(f\"\\nError for Factor 4: {error4}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor5_header_md" + }, + "source": [ + "## 5. `EssentialMatrixFactor5`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor5_desc_md" + }, + "source": [ + "Similar to Factor4, but allows for **two different** unknown `CALIBRATION` variables, one for each camera ($K_A$ and $K_B$).\n", + "It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n", + "The error is the algebraic epipolar error $ (K_B^{-1} p_B)^T E (K_A^{-1} p_A) $.\n", + "\n", + "**Note:** Like Factor4, this is often ill-posed without strong priors." + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": { + "id": "factor5_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 5: keys = { e0 k0 k1 }\n", + "isotropic dim=1 sigma=0.01\n", + " EssentialMatrixFactor5 with measurements\n", + " (480 288)' and (464 312)'\n", + "\n", + "Error for Factor 5: 11.520000000000007\n" + ] + } + ], + "source": [ + "# Keys for potentially different calibrations\n", + "keyKA = K(0) # Can reuse keyK if they are actually the same\n", + "keyKB = K(1)\n", + "\n", + "factor5 = EssentialMatrixFactor5Cal3_S2(keyE, keyKA, keyKB, pA_pixels, pB_pixels, epipolar_noise)\n", + "factor5.print(\"\\nFactor 5: \")\n", + "\n", + "# Evaluate error (requires E, KA, KB)\n", + "# values already contains E(0) and K(0)\n", + "values.insert(keyKB, K_cal) # Assume KB is also the same known K\n", + "error5 = factor5.error(values)\n", + "print(f\"\\nError for Factor 5: {error5}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/FrobeniusFactor.ipynb b/gtsam/slam/doc/FrobeniusFactor.ipynb new file mode 100644 index 000000000..fdf317da8 --- /dev/null +++ b/gtsam/slam/doc/FrobeniusFactor.ipynb @@ -0,0 +1,269 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Frobenius Norm Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors that operate directly on the entries of rotation matrices (`Rot3` or generally `SO(n)`) rather than using their Lie algebra representation (log map). They minimize the Frobenius norm of the difference between rotation matrices.\n", + "\n", + "These factors can sometimes be useful in specific optimization contexts, particularly in rotation averaging problems or as alternatives to standard `BetweenFactor` or `PriorFactor` on rotations.\n", + "\n", + "* `FrobeniusPrior`: Penalizes the Frobenius norm difference between a variable rotation `T` and a fixed target matrix `M`. Error is $||T - M||_F^2$.\n", + "* `FrobeniusFactor`: Penalizes the Frobenius norm difference between two variable rotations `T1` and `T2`. Error is $||T1 - T2||_F^2$.\n", + "* `FrobeniusBetweenFactor`: Penalizes the Frobenius norm difference between the predicted rotation `T2` and the expected rotation `T1 * T12_measured`. Error is $||T1 \\cdot T_{12} - T2||_F^2$.\n", + "**Note:** The noise models for these factors operate on the vectorized rotation matrix (e.g., 9 elements for `Rot3`). The helper function `ConvertNoiseModel` attempts to convert standard rotation noise models (like those for `BetweenFactor`) into an appropriate isotropic noise model for the Frobenius factor. It expects the input noise model to be isotropic." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3, Pose3, Values\n", + "from gtsam import FrobeniusPriorRot3, FrobeniusFactorRot3, FrobeniusBetweenFactorRot3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "R = symbol_shorthand.R # Using 'R' for Rot3 keys" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fprior_header_md" + }, + "source": [ + "## 1. `FrobeniusPrior`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fprior_desc_md" + }, + "source": [ + "Constrains a `Rot3` variable `R(0)` to be close to a target matrix `M` in the Frobenius norm sense." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "fprior_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "FrobeniusPriorRot3: FrobeniusPriorFactor on 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", + "\n", + "FrobeniusPrior error (vectorized matrix diff): (9,)\n", + "[ 0.00054931 -0.00997917 0. -0.00997917 -0.00054931 0.\n", + " 0. 0. 0. ]\n" + ] + } + ], + "source": [ + "target_matrix = Rot3.Yaw(0.1).matrix() # Target matrix (must be 3x3)\n", + "key = R(0)\n", + "\n", + "# Create a standard isotropic noise model for rotation (3 dimensional)\n", + "rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)\n", + "\n", + "# Convert it for the Frobenius factor (9 dimensional)\n", + "frobenius_noise_model_prior = gtsam.noiseModel.Isotropic.Sigma(9, 0.01) # Or use ConvertNoiseModel\n", + "\n", + "prior_fro = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model_prior)\n", + "prior_fro.print(\"FrobeniusPriorRot3: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n", + "error_prior = prior_fro.error(values)\n", + "print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior.shape}\\n{error_prior}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ffactor_header_md" + }, + "source": [ + "## 1. `FrobeniusFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ffactor_desc_md" + }, + "source": [ + "Constrains two `Rot3` variables `R(0)` and `R(1)` to be close to each other in the Frobenius norm sense." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "ffactor_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "FrobeniusFactorRot3: keys = { r0 r1 }\n", + " noise model: unit (9) \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." + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "key1 = R(0)\n", + "key2 = R(1)\n", + "# Use same noise model dimension (9)\n", + "frobenius_noise_model_between = gtsam.noiseModel.Isotropic.Sigma(9, 0.02)\n", + "\n", + "factor_fro = FrobeniusFactorRot3(key1, key2, frobenius_noise_model_between)\n", + "factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n", + "\n", + "# Evaluate error\n", + "values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n", + "error_factor = factor_fro.error(values)\n", + "print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor.shape}\\n{error_factor}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fbetween_header_md" + }, + "source": [ + "## 3. `FrobeniusBetweenFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fbetween_desc_md" + }, + "source": [ + "Acts like `BetweenFactor` but minimizes $||R_1 \\cdot R_{12} - R_2||_F^2$ instead of using the Log map error." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "fbetween_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor(R0,R1)\n", + " T12: R: [\n", + "\t0.999875, -0.0149991, 0;\n", + "\t0.0149991, 0.999875, 0;\n", + "\t0, 0, 1\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", + "\n", + "FrobeniusBetweenFactor error: (9,)\n", + "[-0. 0. 0. 0. -0. 0. 0. 0. 0.]\n" + ] + } + ], + "source": [ + "measured_R12 = Rot3.Yaw(0.005)\n", + "# Use same noise model dimension (9)\n", + "frobenius_noise_model_b = gtsam.noiseModel.Isotropic.Sigma(9, 0.005)\n", + "\n", + "between_fro = FrobeniusBetweenFactorRot3(key1, key2, measured_R12, frobenius_noise_model_b)\n", + "between_fro.print(\"\\nFrobeniusBetweenFactorRot3: \")\n", + "\n", + "# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n", + "error_between = between_fro.error(values)\n", + "print(f\"\\nFrobeniusBetweenFactor error: {error_between.shape}\\n{error_between}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.9" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/GeneralSFMFactor.ipynb b/gtsam/slam/doc/GeneralSFMFactor.ipynb new file mode 100644 index 000000000..366254d1a --- /dev/null +++ b/gtsam/slam/doc/GeneralSFMFactor.ipynb @@ -0,0 +1,223 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# GeneralSFMFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors for Structure from Motion (SfM) problems where camera calibration might be unknown or optimized alongside pose and structure.\n", + "\n", + "`GeneralSFMFactor`:\n", + "- A binary factor connecting a `CAMERA` variable and a `LANDMARK` variable.\n", + "- Represents the reprojection error of the `LANDMARK` into the `CAMERA` view, compared to a 2D `measured_` pixel coordinate.\n", + "- The `CAMERA` type encapsulates both pose and calibration (e.g., `PinholeCamera`).\n", + "- Error: `camera.project(landmark) - measured`\n", + "\n", + "`GeneralSFMFactor2`:\n", + "- A ternary factor connecting a `Pose3` variable, a `Point3` landmark variable, and a `CALIBRATION` variable.\n", + "- This explicitly separates the camera pose and calibration into different variables.\n", + "- Error: `PinholeCamera(pose, calibration).project(landmark) - measured`\n", + "\n", + "These factors are core components for visual SLAM or SfM systems where calibration is refined or initially unknown." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (GeneralSFMFactorCal3_S2, GeneralSFMFactor2Cal3_S2,\n", + " PinholeCameraCal3_S2, Pose3, Point3, Point2, Cal3_S2, Values)\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L\n", + "K = symbol_shorthand.K\n", + "C = symbol_shorthand.C # For Camera variable" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `GeneralSFMFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "Connects a combined Camera variable (pose+calibration) and a Landmark.\n", + "Requires the `Values` object to contain instances of the specific `CAMERA` type (e.g., `PinholeCameraCal3_S2`)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "GeneralSFMFactor: keys = { c0 l0 }\n", + " noise model: unit (2) \n", + "GeneralSFMFactor: .z[\n", + "\t320;\n", + "\t240\n", + "]\n", + "\n", + "Error for GeneralSFMFactor: 0.0\n" + ] + } + ], + "source": [ + "measured_pt = Point2(320, 240) # Measurement in pixels\n", + "sfm_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "camera_key = C(0)\n", + "landmark_key = L(0)\n", + "\n", + "# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactorCal3_S2\n", + "factor1 = GeneralSFMFactorCal3_S2(measured_pt, sfm_noise, camera_key, landmark_key)\n", + "factor1.print(\"GeneralSFMFactor: \")\n", + "\n", + "# Evaluate error - requires a Camera object in Values\n", + "values = Values()\n", + "camera_pose = Pose3() # Identity pose\n", + "calibration = Cal3_S2(500, 500, 0, 320, 240) # fx, fy, s, u0, v0\n", + "camera = PinholeCameraCal3_S2(camera_pose, calibration)\n", + "landmark = Point3(0, 0, 5) # Point 5m in front of camera\n", + "\n", + "values.insert(camera_key, camera)\n", + "values.insert(landmark_key, landmark)\n", + "\n", + "error1 = factor1.error(values)\n", + "print(f\"\\nError for GeneralSFMFactor: {error1}\") # Should be [0, 0] if landmark projects to measured_pt" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `GeneralSFMFactor2`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "Connects separate Pose3, Point3 (Landmark), and Calibration variables." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "GeneralSFMFactor2: keys = { x0 l0 k0 }\n", + " noise model: unit (2) \n", + "GeneralSFMFactor2: .z[\n", + "\t320;\n", + "\t240\n", + "]\n", + "\n", + "Error for GeneralSFMFactor2: 0.0\n" + ] + } + ], + "source": [ + "pose_key = X(0)\n", + "calib_key = K(0)\n", + "# landmark_key = L(0) # Re-use from above\n", + "\n", + "# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactor2Cal3_S2\n", + "factor2 = GeneralSFMFactor2Cal3_S2(measured_pt, sfm_noise, pose_key, landmark_key, calib_key)\n", + "factor2.print(\"GeneralSFMFactor2: \")\n", + "\n", + "# Evaluate error - requires Pose3, Point3, Cal3_S2 objects in Values\n", + "values2 = Values()\n", + "values2.insert(pose_key, camera_pose)\n", + "values2.insert(landmark_key, landmark)\n", + "values2.insert(calib_key, calibration)\n", + "\n", + "error2 = factor2.error(values2)\n", + "print(f\"\\nError for GeneralSFMFactor2: {error2}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/InitializePose3.ipynb b/gtsam/slam/doc/InitializePose3.ipynb new file mode 100644 index 000000000..2e65b906b --- /dev/null +++ b/gtsam/slam/doc/InitializePose3.ipynb @@ -0,0 +1,256 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# InitializePose3" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `InitializePose3` structure provides static methods for computing an initial estimate for 3D poses (`Pose3`) in a factor graph, particularly useful for Structure from Motion (SfM) or SLAM problems.\n", + "The core idea is to first estimate the orientations (`Rot3`) independently and then use these estimates to initialize a linear system for the translations.\n", + "\n", + "Key static methods:\n", + "* `buildPose3graph(graph)`: Extracts the subgraph containing only `Pose3` `BetweenFactor` and `PriorFactor` constraints from a larger `NonlinearFactorGraph`.\n", + "* `computeOrientationsChordal(pose3Graph)`: Estimates rotations using chordal relaxation on the rotation constraints.\n", + "* `computeOrientationsGradient(pose3Graph, initialGuess)`: Estimates rotations using gradient descent on the manifold.\n", + "* `initializeOrientations(graph)`: Convenience function combining `buildPose3graph` and `computeOrientationsChordal`.\n", + "* `computePoses(initialRot, poseGraph)`: Computes translations given estimated rotations by solving a linear system (performing one Gauss-Newton iteration on poses).\n", + "* `initialize(graph)`: Performs the full initialization pipeline (extract graph, estimate rotations via chordal, compute translations).\n", + "* `initialize(graph, givenGuess, useGradient)`: Full pipeline allowing specification of an initial guess and choosing between chordal or gradient descent for rotations." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import NonlinearFactorGraph, Values, Pose3, Rot3, Point3, PriorFactorPose3, BetweenFactorPose3\n", + "from gtsam import InitializePose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_header_md" + }, + "source": [ + "## Example Initialization Pipeline" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_desc_md" + }, + "source": [ + "We'll create a simple 3D pose graph and use the `InitializePose3.initialize` method to get an initial estimate." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "example_pipeline_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph:\n", + "size: 4\n", + "\n", + "Factor 0: PriorFactor on x0\n", + " prior mean: R: [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + " noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.05; 0.05; 0.05];\n", + "\n", + "Factor 1: BetweenFactor(x0,x1)\n", + " measured: R: [\n", + "\t0.877582562, -0.479425539, 0;\n", + "\t0.479425539, 0.877582562, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 1 0.1 0\n", + " noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n", + "\n", + "Factor 2: BetweenFactor(x1,x2)\n", + " measured: R: [\n", + "\t0.921060994, -0.389418342, 0;\n", + "\t0.389418342, 0.921060994, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.9 -0.1 0\n", + " noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n", + "\n", + "Factor 3: BetweenFactor(x2,x0)\n", + " measured: R: [\n", + "\t0.621609968, 0.78332691, 0;\n", + "\t-0.78332691, 0.621609968, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: -1.8 0.05 0\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.1; 0.4; 0.4; 0.4];\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "\n", + "Initial Estimate (using InitializePose3.initialize):\n", + "\n", + "Values with 3 values:\n", + "Value x0: (class gtsam::Pose3)\n", + "R: [\n", + "\t0.995004165, -0.0998334166, 0;\n", + "\t0.0998334166, 0.995004165, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 -7.63278329e-15 0\n", + "\n", + "Value x1: (class gtsam::Pose3)\n", + "R: [\n", + "\t0.825335615, -0.564642473, 0;\n", + "\t0.564642473, 0.825335615, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.956742586 0.343109526 0\n", + "\n", + "Value x2: (class gtsam::Pose3)\n", + "R: [\n", + "\t0.540302306, -0.841470985, 0;\n", + "\t0.841470985, 0.540302306, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 1.62773065 0.912529884 0\n", + "\n" + ] + } + ], + "source": [ + "# 1. Create a NonlinearFactorGraph with Pose3 factors\n", + "graph = NonlinearFactorGraph()\n", + "\n", + "# Add a prior on the first pose\n", + "prior_mean = Pose3(Rot3.Yaw(0.1), Point3(0.1, 0, 0))\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3 + [0.05]*3))\n", + "graph.add(PriorFactorPose3(X(0), prior_mean, prior_noise))\n", + "\n", + "# Add odometry factors\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05]*3 + [0.2]*3))\n", + "odometry1 = Pose3(Rot3.Yaw(0.5), Point3(1.0, 0.1, 0))\n", + "odometry2 = Pose3(Rot3.Yaw(0.4), Point3(0.9, -0.1, 0))\n", + "graph.add(BetweenFactorPose3(X(0), X(1), odometry1, odometry_noise))\n", + "graph.add(BetweenFactorPose3(X(1), X(2), odometry2, odometry_noise))\n", + "\n", + "# Add a loop closure factor (less certain)\n", + "loop_mean = Pose3(Rot3.Yaw(-0.9), Point3(-1.8, 0.05, 0))\n", + "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1]*3 + [0.4]*3))\n", + "graph.add(BetweenFactorPose3(X(2), X(0), loop_mean, loop_noise))\n", + "\n", + "graph.print(\"Original Factor Graph:\\n\")\n", + "\n", + "# 2. Perform initialization using the default chordal relaxation method\n", + "initial_estimate = InitializePose3.initialize(graph)\n", + "\n", + "print(\"\\nInitial Estimate (using InitializePose3.initialize):\\n\")\n", + "initial_estimate.print()\n", + "\n", + "# 3. (Optional) Perform initialization step-by-step\n", + "# pose3graph = InitializePose3.buildPose3graph(graph)\n", + "# initial_orientations = InitializePose3.initializeOrientations(graph)\n", + "# initial_estimate_manual = InitializePose3.computePoses(initial_orientations, pose3graph)\n", + "# print(\"\\nInitial Estimate (Manual Steps):\\n\")\n", + "# initial_estimate_manual.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_header_md" + }, + "source": [ + "## Notes" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_desc_md" + }, + "source": [ + "- The quality of the initial estimate depends heavily on the quality and connectivity of the pose graph factors.\n", + "- Chordal relaxation (`computeOrientationsChordal`) is generally faster and often sufficient.\n", + "- Gradient descent (`computeOrientationsGradient`) might provide slightly more accurate orientations but is slower and requires a good initial guess.\n", + "- The final `computePoses` step performs only a *single* Gauss-Newton iteration, assuming the rotations are fixed. The resulting estimate is meant as an initialization for a full nonlinear optimization (e.g., using `GaussNewtonOptimizer` or `LevenbergMarquardtOptimizer`)." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/KarcherMeanFactor.ipynb b/gtsam/slam/doc/KarcherMeanFactor.ipynb new file mode 100644 index 000000000..a1ca7a51e --- /dev/null +++ b/gtsam/slam/doc/KarcherMeanFactor.ipynb @@ -0,0 +1,245 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Karcher Mean Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", + "The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n", + "$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n", + "\n", + "Functions/Classes:\n", + "* `FindKarcherMean(rotations)`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", + "* `KarcherMeanFactor`: A factor that enforces a constraint related to the Karcher mean. It does *not* constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the *sum of tangent space updates* applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3, FindKarcherMean, KarcherMeanFactorRot3, NonlinearFactorGraph, Values\n", + "from gtsam import symbol_shorthand\n", + "\n", + "R = symbol_shorthand.R" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "find_header_md" + }, + "source": [ + "## 1. `FindKarcherMean`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "find_desc_md" + }, + "source": [ + "Computes the Karcher mean of a list of rotations." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "find_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Input Rotations (Yaw angles):\n", + " 0.100\n", + " 0.150\n", + " 0.050\n", + " 0.120\n", + "\n", + "Computed Karcher Mean (Yaw angle): 0.105\n" + ] + } + ], + "source": [ + "# Create a list of Rot3 objects\n", + "rotations = gtsam.Rot3Vector()\n", + "rotations.append(Rot3.Yaw(0.1))\n", + "rotations.append(Rot3.Yaw(0.15))\n", + "rotations.append(Rot3.Yaw(0.05))\n", + "rotations.append(Rot3.Yaw(0.12))\n", + "\n", + "# Compute the Karcher mean\n", + "karcher_mean = FindKarcherMean(rotations)\n", + "\n", + "print(\"Input Rotations (Yaw angles):\")\n", + "for r in rotations: print(f\" {r.yaw():.3f}\")\n", + "\n", + "print(f\"\\nComputed Karcher Mean (Yaw angle): {karcher_mean.yaw():.3f}\")\n", + "# Note: For yaw rotations, the Karcher mean yaw is close to the arithmetic mean (0.105)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_header_md" + }, + "source": [ + "## 2. `KarcherMeanFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_desc_md" + }, + "source": [ + "Creates a factor that constrains the rotational average of a set of `Rot3` variables.\n", + "It acts as a soft gauge constraint. When linearized, it yields a Jacobian factor where each block corresponding to a variable is $\\sqrt{\\beta} I_{3x3}$, and the error vector is zero. The `beta` parameter (optional, defaults to 1) controls the strength (precision) of the constraint." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "id": "factor_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n", + "\n", + "Linearized Factor (JacobianFactor):\n", + " A[r0] = [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " A[r1] = [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " A[r2] = [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " b = [ 0 0 0 ]\n", + " No noise model\n" + ] + }, + { + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.JacobianFactor' object has no attribute 'find'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[9], line 20\u001b[0m\n\u001b[0;32m 18\u001b[0m sqrt_beta \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39msqrt(beta)\n\u001b[0;32m 19\u001b[0m expected_jacobian \u001b[38;5;241m=\u001b[39m sqrt_beta \u001b[38;5;241m*\u001b[39m np\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m3\u001b[39m)\n\u001b[1;32m---> 20\u001b[0m A0 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(\u001b[43mlinearized_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mfind\u001b[49m(R(\u001b[38;5;241m0\u001b[39m)))\n\u001b[0;32m 21\u001b[0m A1 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m1\u001b[39m)))\n\u001b[0;32m 22\u001b[0m A2 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m2\u001b[39m)))\n", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.JacobianFactor' object has no attribute 'find'" + ] + } + ], + "source": [ + "keys = [R(0), R(1), R(2)]\n", + "beta = 100.0 # Strength of the constraint\n", + "\n", + "k_factor = KarcherMeanFactorRot3(keys)\n", + "k_factor.print(\"KarcherMeanFactorRot3: \")\n", + "\n", + "# Linearization example\n", + "values = Values()\n", + "values.insert(R(0), Rot3.Yaw(0.1))\n", + "values.insert(R(1), Rot3.Yaw(0.2))\n", + "values.insert(R(2), Rot3.Yaw(0.3))\n", + "\n", + "linearized_factor = k_factor.linearize(values)\n", + "print(\"\\nLinearized Factor (JacobianFactor):\")\n", + "linearized_factor.print()\n", + "\n", + "# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n", + "sqrt_beta = np.sqrt(beta)\n", + "expected_jacobian = sqrt_beta * np.eye(3)\n", + "A0 = linearized_factor.getA(linearized_factor.find(R(0)))\n", + "A1 = linearized_factor.getA(linearized_factor.find(R(1)))\n", + "A2 = linearized_factor.getA(linearized_factor.find(R(2)))\n", + "b = linearized_factor.getb()\n", + "\n", + "print(f\"\\nJacobian for R(0):\\n{A0}\")\n", + "print(f\"Jacobian for R(1):\\n{A1}\")\n", + "print(f\"Jacobian for R(2):\\n{A2}\")\n", + "print(f\"Error vector b:\\n{b}\")\n", + "print(f\"sqrt(beta): {sqrt_beta}\")\n", + "assert np.allclose(A0, expected_jacobian)\n", + "assert np.allclose(A1, expected_jacobian)\n", + "assert np.allclose(A2, expected_jacobian)\n", + "assert np.allclose(b, np.zeros(3))" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb new file mode 100644 index 000000000..e140dbd71 --- /dev/null +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -0,0 +1,230 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# OrientedPlane3 Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors for working with planar landmarks represented by `gtsam.OrientedPlane3`.\n", + "`OrientedPlane3` represents a plane using normalized coefficients $(n_x, n_y, n_z, d)$, where $(n_x, n_y, n_z)$ is the unit normal vector and $d$ is the distance from the origin along the normal.\n", + "\n", + "Factors defined:\n", + "* `OrientedPlane3Factor`: A binary factor connecting a `Pose3` and an `OrientedPlane3`. It measures the difference between the plane parameters as observed from the given pose and a measured plane.\n", + "* `OrientedPlane3DirectionPrior`: A unary factor on an `OrientedPlane3`. It penalizes the difference between the plane's normal direction and a measured direction (represented by the normal of a measured `OrientedPlane3`). **Note:** The factor error is 3D, but only constrains 2 degrees of freedom (direction). Consider using a more specific direction factor if only direction is measured." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "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": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n", + "from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "P = symbol_shorthand.P # For Plane" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_header_md" + }, + "source": [ + "## 1. `OrientedPlane3Factor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_desc_md" + }, + "source": [ + "Connects a `Pose3` (camera/robot pose) and an `OrientedPlane3` (landmark). The measurement is the plane as observed *from the sensor frame*.\n", + "The error is calculated by transforming the global plane landmark into the sensor frame defined by the pose, and then computing the difference (`localCoordinates`) with the measured plane." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "factor_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "OrientedPlane3Factor: OrientedPlane3Factor Factor with keys: x0 P0\n", + "\n", + "Error model: diagonal sigmas [0.05; 0.05; 0.05];\n", + "\n", + "Error at ground truth: [-0. -0. -0.]\n", + "\n", + "Error with noisy plane: [ -0.0150837 -0.00503354 -49.83283361]\n" + ] + } + ], + "source": [ + "# Ground truth plane (e.g., z=1 in world frame)\n", + "gt_plane_world = OrientedPlane3(0, 0, 1, 1)\n", + "\n", + "# Ground truth pose\n", + "gt_pose = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0))\n", + "\n", + "# Measurement: transform the world plane into the camera frame\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", + "measured_plane_coeffs = gt_plane_world.coeffs()\n", + "plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n", + "\n", + "pose_key = X(0)\n", + "plane_key = P(0)\n", + "\n", + "plane_factor = OrientedPlane3Factor(measured_plane_coeffs, plane_noise, pose_key, plane_key)\n", + "plane_factor.print(\"OrientedPlane3Factor: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose)\n", + "values.insert(plane_key, gt_plane_world)\n", + "error1 = plane_factor.error(values)\n", + "print(f\"\\nError at ground truth: {error1}\")\n", + "\n", + "# Evaluate with slightly different plane estimate\n", + "noisy_plane = OrientedPlane3(0.01, 0.01, 0.99, 1.05)\n", + "values.update(plane_key, noisy_plane)\n", + "error2 = plane_factor.error(values)\n", + "print(f\"\\nError with noisy plane: {error2}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "prior_header_md" + }, + "source": [ + "## 2. `OrientedPlane3DirectionPrior`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "prior_desc_md" + }, + "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." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "prior_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "OrientedPlane3DirectionPrior: Factor OrientedPlane3DirectionPrior on P0\n", + "Noise model: diagonal sigmas [0.02; 0.02; 0.02];\n", + "\n", + "Error for prior on noisy_plane: [-0.5 -0.5 0.5]\n", + "Error for prior on gt_plane_world: [0. 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", + "\n", + "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.coeffs(), direction_noise)\n", + "prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n", + "\n", + "# Evaluate error using the 'noisy_plane' from the previous step\n", + "error_prior = prior_factor.error(values) # values still contains plane_key -> noisy_plane\n", + "print(f\"\\nError for prior on noisy_plane: {error_prior}\")\n", + "\n", + "# Evaluate error for ground truth plane\n", + "values.update(plane_key, gt_plane_world)\n", + "error_prior_gt = prior_factor.error(values)\n", + "print(f\"Error for prior on gt_plane_world: {error_prior_gt}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb new file mode 100644 index 000000000..ce7c37a28 --- /dev/null +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -0,0 +1,293 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# PlanarProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `PlanarProjectionFactor` variants provide camera projection constraints specifically designed for scenarios where the robot or camera moves primarily on a 2D plane (e.g., ground robots with cameras).\n", + "They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n", + "* The robot's 2D pose (`Pose2` `wTb`: world-to-body) in the ground plane.\n", + "* The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n", + "* The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n", + "* The 3D landmark position in the world frame.\n", + "\n", + "The core projection logic involves converting the `Pose2` `wTb` to a `Pose3` assuming z=0 and yaw=theta, composing it with `bTc` to get the world-to-camera pose `wTc`, and then using a standard `PinholeCamera` model to project the landmark.\n", + "\n", + "Variants:\n", + "* `PlanarProjectionFactor1`: Unknown is robot `Pose2` (`wTb`). Landmark, `bTc`, and calibration are fixed. Useful for localization.\n", + "* `PlanarProjectionFactor2`: Unknowns are robot `Pose2` (`wTb`) and `Point3` landmark. `bTc` and calibration are fixed. Useful for planar SLAM.\n", + "* `PlanarProjectionFactor3`: Unknowns are robot `Pose2` (`wTb`), camera offset `Pose3` (`bTc`), and `Cal3DS2` calibration. Landmark is fixed. Useful for calibration." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n", + " PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L\n", + "C = symbol_shorthand.C # Calibration\n", + "O = symbol_shorthand.O # Offset" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `PlanarProjectionFactor1` (Localization)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "Used when the landmark, camera offset (`bTc`), and calibration (`calib`) are known, and we want to estimate the robot's `Pose2` (`wTb`)." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Ground Truth Pose2: (1, 0, 0.785398)\n", + "\n", + "Calculated Measurement: [ 909.25565099 1841.1002863 ]\n", + "Factor 1: keys = { x0 }\n", + "isotropic dim=2 sigma=1.5\n", + "\n", + "Error at ground truth: 0.0\n", + "Error at noisy pose: 3317.647263749095\n" + ] + } + ], + "source": [ + "# Known parameters\n", + "landmark_pt = Point3(2.0, 0.5, 0.5)\n", + "body_T_cam = Pose3(Rot3.Yaw(-np.pi/2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", + "calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)\n", + "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", + "\n", + "# Assume ground truth pose and calculate expected measurement\n", + "gt_pose2 = Pose2(1.0, 0.0, np.pi/4)\n", + "gt_world_T_cam = Pose3(gt_pose2) * body_T_cam\n", + "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n", + "measured_pt2 = gt_camera.project(landmark_pt)\n", + "print(f\"Ground Truth Pose2: {gt_pose2}\")\n", + "print(f\"Calculated Measurement: {measured_pt2}\")\n", + "\n", + "# Create the factor\n", + "pose_key = X(0)\n", + "factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor1.print(\"Factor 1: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose2) # Error should be zero here\n", + "error1_gt = factor1.error(values)\n", + "print(f\"\\nError at ground truth: {error1_gt}\")\n", + "\n", + "noisy_pose2 = Pose2(1.05, 0.02, np.pi/4 + 0.05)\n", + "values.update(pose_key, noisy_pose2)\n", + "error1_noisy = factor1.error(values)\n", + "print(f\"Error at noisy pose: {error1_noisy}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `PlanarProjectionFactor2` (Planar SLAM)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "Used when the camera offset (`bTc`) and calibration (`calib`) are known, but both the robot `Pose2` (`wTb`) and the `Point3` landmark position are unknown." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor 2: keys = { l0 x0 }\n", + "isotropic dim=2 sigma=1.5\n" + ] + }, + { + "ename": "RuntimeError", + "evalue": "Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 10\u001b[0m\n\u001b[0;32m 8\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose2)\n\u001b[0;32m 9\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(landmark_key, landmark_pt) \u001b[38;5;66;03m# Error should be zero\u001b[39;00m\n\u001b[1;32m---> 10\u001b[0m error2_gt \u001b[38;5;241m=\u001b[39m \u001b[43mfactor2\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror2_gt\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 13\u001b[0m noisy_landmark \u001b[38;5;241m=\u001b[39m Point3(\u001b[38;5;241m2.1\u001b[39m, \u001b[38;5;241m0.45\u001b[39m, \u001b[38;5;241m0.55\u001b[39m)\n", + "\u001b[1;31mRuntimeError\u001b[0m: Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix" + ] + } + ], + "source": [ + "landmark_key = L(0)\n", + "\n", + "factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor2.print(\"Factor 2: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose2)\n", + "values.insert(landmark_key, landmark_pt) # Error should be zero\n", + "error2_gt = factor2.error(values)\n", + "print(f\"\\nError at ground truth: {error2_gt}\")\n", + "\n", + "noisy_landmark = Point3(2.1, 0.45, 0.55)\n", + "values.update(landmark_key, noisy_landmark)\n", + "error2_noisy = factor2.error(values)\n", + "print(f\"Error with noisy landmark: {error2_noisy}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_header_md" + }, + "source": [ + "## 3. `PlanarProjectionFactor3` (Calibration)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_desc_md" + }, + "source": [ + "Used when the landmark position is known, but the robot `Pose2` (`wTb`), the camera offset `Pose3` (`bTc`), and the `Cal3DS2` calibration are unknown." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "factor3_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", + "Noise model: diagonal sigmas [1.5; 1.5];\n", + "\n", + "Error at ground truth: [-0. -0.]\n", + "Error with noisy calibration: [8.38867847 0.63659684]\n" + ] + } + ], + "source": [ + "offset_key = O(0)\n", + "calib_key = C(0)\n", + "\n", + "factor3 = PlanarProjectionFactor3(pose_key, offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", + "factor3.print(\"Factor 3: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose2)\n", + "values.insert(offset_key, body_T_cam)\n", + "values.insert(calib_key, calib) # Error should be zero\n", + "error3_gt = factor3.error(values)\n", + "print(f\"\\nError at ground truth: {error3_gt}\")\n", + "\n", + "noisy_calib = Cal3DS2(fx=510, fy=495, s=0, u0=322, v0=241, k1=0, k2=0, p1=0, p2=0)\n", + "values.update(calib_key, noisy_calib)\n", + "error3_noisy = factor3.error(values)\n", + "print(f\"Error with noisy calibration: {error3_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/PoseRotationPrior.ipynb b/gtsam/slam/doc/PoseRotationPrior.ipynb new file mode 100644 index 000000000..cb7d09a75 --- /dev/null +++ b/gtsam/slam/doc/PoseRotationPrior.ipynb @@ -0,0 +1,215 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# PoseRotationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`PoseRotationPrior` is a unary factor that applies a prior constraint only to the **rotation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n", + "It ignores the translation component of the pose variable during error calculation.\n", + "The error is calculated as the difference between the rotation component of the pose variable and the measured prior rotation, expressed in the tangent space of the rotation group.\n", + "\n", + "Error: $ \\text{Log}(\\text{measured}^{-1} \\cdot \\text{pose.rotation}()) $\n", + "\n", + "This is useful when you have information about the absolute orientation of a pose but little or no information about its translation." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "imports_code" + }, + "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": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPriorPose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a PoseRotationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Provide the key of the pose variable, the measured prior rotation (`Rot3` for `Pose3`, `Rot2` for `Pose2`), and a noise model defined on the rotation manifold's dimension (e.g., 3 for `Rot3`)." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "PoseRotationPrior: PoseRotationPriorFactor on x0\n", + "Noise model: diagonal sigmas [0.05; 0.05; 0.05];\n", + "Measured Rotation R: [\n", + "\t0.707107, -0.707107, 0;\n", + "\t0.707107, 0.707107, 0;\n", + "\t0, 0, 1\n", + "]\n", + "\n" + ] + } + ], + "source": [ + "pose_key = X(0)\n", + "measured_rotation = Rot3.Yaw(np.pi / 4) # Prior belief about orientation\n", + "\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", + "\n", + "# Factor type includes the Pose type, e.g. PoseRotationPriorPose3\n", + "factor = PoseRotationPriorPose3(pose_key, measured_rotation, rotation_noise)\n", + "factor.print(\"PoseRotationPrior: \")\n", + "\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", + "factor_from_pose = PoseRotationPriorPose3(pose_key, full_pose_prior, rotation_noise)\n", + "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error depends only on the rotation part of the `Pose3` value in the `Values` object." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error with correct rotation: [0. 0. 0.] (Should be near zero)\n", + "Error with incorrect rotation: [-0. -0. 2.00000004] (Should be non-zero)\n", + "Error with different translation: [-0. -0. 2.00000004] (Should be same as error2)\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Pose with correct rotation but different translation\n", + "pose_val1 = Pose3(measured_rotation, Point3(1, 2, 3))\n", + "values.insert(pose_key, pose_val1)\n", + "error1 = factor.error(values)\n", + "print(f\"Error with correct rotation: {error1} (Should be near zero)\")\n", + "\n", + "# Pose with incorrect rotation\n", + "pose_val2 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(1, 2, 3))\n", + "values.update(pose_key, pose_val2)\n", + "error2 = factor.error(values)\n", + "print(f\"Error with incorrect rotation: {error2} (Should be non-zero)\")\n", + "\n", + "# Check that translation change doesn't affect error\n", + "pose_val3 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(100, 200, 300))\n", + "values.update(pose_key, pose_val3)\n", + "error3 = factor.error(values)\n", + "print(f\"Error with different translation: {error3} (Should be same as error2)\")\n", + "assert np.allclose(error2, error3)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/PoseTranslationPrior.ipynb b/gtsam/slam/doc/PoseTranslationPrior.ipynb new file mode 100644 index 000000000..047bf5e59 --- /dev/null +++ b/gtsam/slam/doc/PoseTranslationPrior.ipynb @@ -0,0 +1,214 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# PoseTranslationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`PoseTranslationPrior` is a unary factor that applies a prior constraint only to the **translation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\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", + "\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::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)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "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": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a PoseTranslationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Provide the key of the pose variable, the measured prior translation (`Point3` for `Pose3`, `Point2` for `Pose2`), and a noise model defined on the translation space dimension (e.g., 3 for `Point3`)." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "PoseTranslationPrior: Factor PoseTranslationPrior on x0\n", + "Noise model: diagonal sigmas [0.5; 0.5; 0.5];\n", + "Measured Translation10 20 5\n" + ] + } + ], + "source": [ + "pose_key = X(0)\n", + "measured_translation = Point3(10.0, 20.0, 5.0) # Prior belief about position\n", + "\n", + "# Noise model on translation (3 dimensions for Point3)\n", + "translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev\n", + "\n", + "# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3\n", + "factor = PoseTranslationPriorPose3(pose_key, measured_translation, translation_noise)\n", + "factor.print(\"PoseTranslationPrior: \")\n", + "\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", + "factor_from_pose = PoseTranslationPriorPose3(pose_key, full_pose_prior, translation_noise)\n", + "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error depends only on the translation part of the `Pose3` value in the `Values` object." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error with correct translation: [0. 0. 0.] (Should be near zero)\n", + "Error with incorrect translation: [ 0.39999999 -0.19999999 0.19999999] (Should be non-zero)\n", + "Error with different rotation: [ 0.39999999 -0.19999999 0.19999999] (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" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Pose with correct translation but different rotation\n", + "pose_val1 = Pose3(Rot3.Roll(0.5), measured_translation)\n", + "values.insert(pose_key, pose_val1)\n", + "error1 = factor.error(values)\n", + "print(f\"Error with correct translation: {error1} (Should be near zero)\")\n", + "\n", + "# Pose with incorrect translation\n", + "pose_val2 = Pose3(Rot3.Roll(0.5), Point3(10.2, 19.9, 5.1))\n", + "values.update(pose_key, pose_val2)\n", + "error2 = factor.error(values)\n", + "print(f\"Error with incorrect translation: {error2} (Should be non-zero)\")\n", + "\n", + "# Check that rotation change doesn't affect unwhitened error\n", + "pose_val3 = Pose3(Rot3.Yaw(1.0), Point3(10.2, 19.9, 5.1))\n", + "values.update(pose_key, pose_val3)\n", + "error3 = factor.error(values)\n", + "unwhitened2 = factor.unwhitenedError(values)\n", + "print(f\"Error with different rotation: {error3} (Should reflect Jacobian change)\")\n", + "print(f\"Unwhitened error with different rotation: {unwhitened2} (Should be [0.2, -0.1, 0.1])\")\n", + "# assert np.allclose(error2, error3) # Whitened error WILL change due to Jacobian\n", + "assert np.allclose(unwhitened2, np.array([0.2, -0.1, 0.1]))" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/ProjectionFactor.ipynb b/gtsam/slam/doc/ProjectionFactor.ipynb new file mode 100644 index 000000000..50bc5d71d --- /dev/null +++ b/gtsam/slam/doc/ProjectionFactor.ipynb @@ -0,0 +1,238 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# GenericProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`GenericProjectionFactor` is a versatile factor for monocular camera measurements.\n", + "It models the reprojection error between the predicted projection of a 3D `LANDMARK` (usually `Point3`) onto the image plane of a camera defined by `POSE` (usually `Pose3`) and `CALIBRATION` (e.g., `Cal3_S2`, `Cal3Bundler`, `Cal3DS2`), and a measured 2D pixel coordinate `measured_`.\n", + "\n", + "Key features:\n", + "- **Templated:** Works with different pose, landmark, and calibration types.\n", + "- **Fixed Calibration:** Assumes the `CALIBRATION` object (`K_`) is known and fixed (passed as a shared pointer).\n", + "- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform between the pose variable's frame (body) and the camera's sensor frame.\n", + "- **Cheirality Handling:** Can be configured to throw an exception or return a large error if the landmark projects behind the camera.\n", + "\n", + "The error is the 2D vector difference:\n", + "$$ \\text{error}(P, L) = \\text{project}(P \\cdot S, L) - z $$\n", + "where $P$ is the pose variable, $L$ is the landmark variable, $S$ is the optional `body_P_sensor` transform, `project` is the camera projection function including calibration, and $z$ is the `measured_` point." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Point3, Point2, Rot3, Cal3_S2, Values\n", + "# The Python wrapper often creates specific instantiations\n", + "from gtsam import GenericProjectionFactorCal3_S2\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a GenericProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing:\n", + "1. The 2D measurement (`Point2`).\n", + "2. The noise model (typically 2D isotropic).\n", + "3. The key for the pose variable.\n", + "4. The key for the landmark variable.\n", + "5. A `shared_ptr` to the fixed calibration object.\n", + "6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n", + "7. (Optional) Cheirality handling flags." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor with offset: GenericProjectionFactor, z = [\n", + "\t330;\n", + "\t250\n", + "]\n", + " sensor pose in body frame: R: [\n", + "\t6.12323e-17, 6.12323e-17, 1;\n", + "\t-1, 3.7494e-33, 6.12323e-17;\n", + "\t-0, -1, 6.12323e-17\n", + "]\n", + "t: 0.1 0 0.2\n", + " keys = { x0 l1 }\n", + " noise model: unit (2) \n", + "\n", + "Factor without offset: GenericProjectionFactor, z = [\n", + "\t330;\n", + "\t250\n", + "]\n", + " keys = { x0 l1 }\n", + " noise model: unit (2) \n" + ] + } + ], + "source": [ + "measured_pt2 = Point2(330, 250)\n", + "pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # 1 pixel std dev\n", + "pose_key = X(0)\n", + "landmark_key = L(1)\n", + "\n", + "# Shared pointer to calibration\n", + "K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n", + "\n", + "# Optional sensor pose offset\n", + "body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n", + "\n", + "# Create factor with sensor offset\n", + "factor_with_offset = GenericProjectionFactorCal3_S2(\n", + " measured_pt2, pixel_noise, pose_key, landmark_key, K, body_P_sensor)\n", + "factor_with_offset.print(\"Factor with offset: \")\n", + "\n", + "# Create factor without sensor offset (implicitly identity)\n", + "factor_no_offset = GenericProjectionFactorCal3_S2(\n", + " measured_pt2, pixel_noise, pose_key, landmark_key, K)\n", + "factor_no_offset.print(\"\\nFactor without offset: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error is the difference between the predicted projection and the measurement." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error (no offset): 1175689.2145311693\n", + "Error (with offset): 50751.576015003826\n", + "Manual error calculation (no offset): [1370.63962025 687.55033305]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Example values\n", + "pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n", + "landmark = Point3(4.0, 2.0, 3.0)\n", + "\n", + "values.insert(pose_key, pose)\n", + "values.insert(landmark_key, landmark)\n", + "\n", + "# Evaluate factor without offset\n", + "error_no_offset = factor_no_offset.error(values)\n", + "print(f\"Error (no offset): {error_no_offset}\")\n", + "\n", + "# Evaluate factor with offset\n", + "error_with_offset = factor_with_offset.error(values)\n", + "print(f\"Error (with offset): {error_with_offset}\")\n", + "\n", + "# Manually verify projection (no offset case)\n", + "cam_no_offset = gtsam.PinholeCameraCal3_S2(pose, K)\n", + "predicted_no_offset = cam_no_offset.project(landmark)\n", + "manual_error = predicted_no_offset - measured_pt2\n", + "print(f\"Manual error calculation (no offset): {manual_error}\")\n", + "assert np.allclose(factor_no_offset.unwhitenedError(values), manual_error)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb new file mode 100644 index 000000000..5ea646d19 --- /dev/null +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -0,0 +1,219 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# ReferenceFrameFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`ReferenceFrameFactor` is a ternary factor used to relate landmark observations made in two different reference frames (e.g., from two different robots or two distinct SLAM sessions).\n", + "It connects:\n", + "1. A landmark (`POINT`) expressed in a 'global' or common reference frame (`globalKey`).\n", + "2. A transform (`TRANSFORM`) representing the pose of a 'local' frame relative to the 'global' frame (`transKey`). Typically `TRANSFORM = global_T_local`.\n", + "3. The *same* landmark (`POINT`) expressed in the 'local' reference frame (`localKey`).\n", + "\n", + "The factor enforces the constraint that the globally expressed landmark, when transformed by the `global_T_local` transform, should match the locally expressed landmark.\n", + "The transformation logic depends on the specific `POINT` and `TRANSFORM` types and is handled by the `transform_point` helper function (which usually calls `Transform::transformFrom`).\n", + "\n", + "Error: $ \\text{Log}(\\text{local}^{-1} \\cdot \\text{transform\\_point}(\\text{trans}, \\text{global})) $\n", + "\n", + "This factor is crucial for multi-robot map merging or combining results from different SLAM sessions where common landmarks have been observed." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'ReferenceFrameFactorPoint3Pose3' 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 5\u001b[0m\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 Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# The Python wrapper creates specific instantiations\u001b[39;00m\n\u001b[1;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 ReferenceFrameFactorPoint3Pose3\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 8\u001b[0m L \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mL \u001b[38;5;66;03m# Global landmark\u001b[39;00m\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n", + "# The Python wrapper creates specific instantiations\n", + "from gtsam import ReferenceFrameFactorPoint3Pose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "L = symbol_shorthand.L # Global landmark\n", + "T = symbol_shorthand.T # Transform global_T_local\n", + "l = symbol_shorthand.l # Local landmark" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a ReferenceFrameFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing the keys for the global landmark, the transform, the local landmark, and a noise model.\n", + "The noise model dimensionality should match the dimension of the `POINT` type." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ReferenceFrameFactor: : ReferenceFrameFactor(Global: L0, Transform: T0, Local: l0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.1];\n" + ] + } + ], + "source": [ + "global_landmark_key = L(0)\n", + "transform_key = T(0)\n", + "local_landmark_key = l(0)\n", + "\n", + "# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)\n", + "noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev\n", + "\n", + "# Factor type includes Point and Transform types\n", + "factor = ReferenceFrameFactorPoint3Pose3(global_landmark_key,\n", + " transform_key,\n", + " local_landmark_key,\n", + " noise)\n", + "factor.print(\"ReferenceFrameFactor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error represents how much the transformed global landmark deviates from the local landmark estimate." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Expected local landmark: [ 2. -4. 1.]\n", + "\n", + "Error at ground truth: [-0. 0. 0.] (Should be zero)\n", + "Error with noisy local landmark: [-1. 1. -0.5]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Example values\n", + "global_landmark = Point3(5.0, 2.0, 1.0)\n", + "global_T_local = Pose3(Rot3.Yaw(np.pi/2), Point3(1, 0, 0))\n", + "expected_local_landmark = global_T_local.transformTo(global_landmark)\n", + "print(f\"Expected local landmark: {expected_local_landmark}\")\n", + "\n", + "values.insert(global_landmark_key, global_landmark)\n", + "values.insert(transform_key, global_T_local)\n", + "values.insert(local_landmark_key, expected_local_landmark)\n", + "\n", + "error_gt = factor.error(values)\n", + "print(f\"\\nError at ground truth: {error_gt} (Should be zero)\")\n", + "\n", + "# Introduce error in the local landmark estimate\n", + "noisy_local_landmark = expected_local_landmark + Point3(0.1, -0.1, 0.05)\n", + "values.update(local_landmark_key, noisy_local_landmark)\n", + "error_noisy = factor.error(values)\n", + "print(f\"Error with noisy local landmark: {error_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/RotateFactor.ipynb b/gtsam/slam/doc/RotateFactor.ipynb new file mode 100644 index 000000000..a62e986ad --- /dev/null +++ b/gtsam/slam/doc/RotateFactor.ipynb @@ -0,0 +1,229 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Rotate Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors that constrain an unknown rotation (`Rot3`) based on how it transforms either full rotations or directions.\n", + "\n", + "* `RotateFactor`: Relates two *incremental* rotations measured in different frames using an unknown rotation relating the frames. If $Z = R \\cdot P \\cdot R^T$, where $P = e^{[p]}$ and $Z = e^{[z]}$ are measured incremental rotations (expressed as angular velocity vectors $p$ and $z$), this factor constrains the unknown rotation $R$ such that $p = R z$. The error is $Rz - p$.\n", + "* `RotateDirectionsFactor`: Relates two *directions* (unit vectors, `Unit3`) measured in different frames using an unknown rotation $R$ relating the frames. If $p_{world} = R \\cdot z_{body}$, this factor constrains $R$. The error is the angular difference between the predicted $R z_{body}$ and the measured $p_{world}$." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "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": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3, Point3, Unit3, Values\n", + "from gtsam import RotateFactor, RotateDirectionsFactor\n", + "from gtsam import symbol_shorthand\n", + "\n", + "R = symbol_shorthand.R # For Rotation key" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `RotateFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "Constraints an unknown `Rot3` based on corresponding incremental rotation measurements $P$ (predicted/world) and $Z$ (measured/body)." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "RotateFactor: Factor RotateFactor on R0\n", + "Noise model: diagonal sigmas [0.001; 0.001; 0.001];\n", + "RotateFactor:]\n", + "p: [0.01 0.02 0.03]\n", + "z: [ 0.03 -0.01 0.02]\n", + "\n", + "Error at ground truth R: [-0. -0. 0.]\n", + "Error at noisy R: [ 0.039987 0.00999887 -0.00998887]\n" + ] + } + ], + "source": [ + "# Assume P and Z are small incremental rotations\n", + "P = Rot3.Expmap(np.array([0.01, 0.02, 0.03]))\n", + "Z = Rot3.Expmap(np.array([0.03, -0.01, 0.02]))\n", + "rot_key = R(0)\n", + "noise1 = gtsam.noiseModel.Isotropic.Sigma(3, 0.001)\n", + "\n", + "factor1 = RotateFactor(rot_key, P, Z, noise1)\n", + "factor1.print(\"RotateFactor: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "# Ground truth R would satisfy P = R*Z\n", + "# R = P * Z.inverse()\n", + "gt_R = P * (Z.inverse())\n", + "values.insert(rot_key, gt_R)\n", + "error1_gt = factor1.error(values)\n", + "print(f\"\\nError at ground truth R: {error1_gt}\")\n", + "\n", + "noisy_R = gt_R * Rot3.Expmap(np.array([0.001, -0.001, 0.001]))\n", + "values.update(rot_key, noisy_R)\n", + "error1_noisy = factor1.error(values)\n", + "print(f\"Error at noisy R: {error1_noisy}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `RotateDirectionsFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "Constraints an unknown `Rot3` based on corresponding direction measurements $p_{world}$ (predicted/world) and $z_{body}$ (measured/body)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "RotateDirectionsFactor: Factor RotateDirectionsFactor on R0\n", + "Noise model: diagonal sigmas [0.01; 0.01];\n", + "RotateDirectionsFactor:\n", + "pUnit3: (1, 0, 0)\n", + "zUnit3: (0, 1, 0)\n", + "\n", + "Check: gt_R * z_body = [ 1.0000000e+00 -6.1232340e-17 0.0000000e+00]\n", + "Error at ground truth R: [-0. 0.]\n", + "Error at noisy R: [-0.99995 -0.99995]\n" + ] + } + ], + "source": [ + "p_world = Unit3(Point3(1, 0, 0)) # Direction in world frame\n", + "z_body = Unit3(Point3(0, 1, 0)) # Corresponding direction in body frame\n", + "noise2 = gtsam.noiseModel.Isotropic.Sigma(2, 0.01) # Noise on 2D tangent space\n", + "\n", + "factor2 = RotateDirectionsFactor(rot_key, p_world, z_body, noise2)\n", + "factor2.print(\"RotateDirectionsFactor: \")\n", + "\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", + "gt_R_dir = Rot3.Yaw(-np.pi/2)\n", + "print(f\"\\nCheck: gt_R * z_body = {gt_R_dir * z_body.point3()}\")\n", + "\n", + "# Evaluate error\n", + "values_dir = Values()\n", + "values_dir.insert(rot_key, gt_R_dir)\n", + "error2_gt = factor2.error(values_dir)\n", + "print(f\"Error at ground truth R: {error2_gt}\")\n", + "\n", + "noisy_R_dir = gt_R_dir * Rot3.Expmap(np.array([0.01, 0, 0.01]))\n", + "values_dir.update(rot_key, noisy_R_dir)\n", + "error2_noisy = factor2.error(values_dir)\n", + "print(f\"Error at noisy R: {error2_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartFactorParams.ipynb b/gtsam/slam/doc/SmartFactorParams.ipynb new file mode 100644 index 000000000..b19218538 --- /dev/null +++ b/gtsam/slam/doc/SmartFactorParams.ipynb @@ -0,0 +1,221 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionParams" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionParams` is a structure used to configure the behavior of \"smart\" factors like `SmartProjectionFactor`, `SmartProjectionPoseFactor`, and `SmartStereoFactor`.\n", + "These factors implicitly manage the triangulation of a 3D point observed by multiple cameras.\n", + "\n", + "Parameters include:\n", + "- **`linearizationMode`**: Determines the type of linear factor produced when `.linearize()` is called.\n", + " - `HESSIAN` (Default): Creates a `RegularHessianFactor` by marginalizing out the point.\n", + " - `IMPLICIT_SCHUR`: Creates a `RegularImplicitSchurFactor`.\n", + " - `JACOBIAN_Q`: Creates a `JacobianFactorQ`.\n", + " - `JACOBIAN_SVD`: Creates a `JacobianFactorSVD`.\n", + "- **`degeneracyMode`**: How to handle cases where triangulation fails or is ill-conditioned.\n", + " - `IGNORE_DEGENERACY` (Default): Factor might become ill-conditioned.\n", + " - `ZERO_ON_DEGENERACY`: Return a zero-information factor (Hessian/Jacobian) if triangulation fails.\n", + " - `HANDLE_INFINITY`: Treat the point as being at infinity (uses `Unit3` representation).\n", + "- **`triangulation`**: A `gtsam.TriangulationParameters` struct containing parameters for the `triangulateSafe` function:\n", + " - `rankTolerance`: Rank tolerance threshold for SVD during triangulation.\n", + " - `enableEPI`: Use Essential matrix check for epipolar inconsistency (only for Point3 landmarks).\n", + " - `landmarkDistanceThreshold`: If point distance is greater than this, use point-at-infinity.\n", + " - `dynamicOutlierRejectionThreshold`: If non-zero, dynamically rejects measurements based on reprojection error (threshold in sigmas).\n", + "- **`retriangulationThreshold`**: If the change in camera poses between linearizations exceeds this threshold (Frobenius norm difference), the point is re-triangulated.\n", + "- **`throwCheirality` / `verboseCheirality`**: Flags inherited from projection factors to control exception handling when a point is behind a camera." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import SmartProjectionParams, LinearizationMode, DegeneracyMode" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating and Modifying Params" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Default Parameters:\n" + ] + }, + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001", + "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 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", + "\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=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001" + ] + } + ], + "source": [ + "# Default parameters\n", + "default_params = SmartProjectionParams()\n", + "print(\"Default Parameters:\")\n", + "default_params.print()\n", + "\n", + "# Custom parameters\n", + "custom_params = SmartProjectionParams(\n", + " linearizationMode = LinearizationMode.JACOBIAN_Q,\n", + " degeneracyMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n", + " throwCheirality = False,\n", + " verboseCheirality = True,\n", + " retriangulationTh = 1e-3\n", + ")\n", + "# Modify triangulation parameters directly\n", + "custom_params.triangulation.rankTolerance = 1e-8\n", + "custom_params.triangulation.enableEPI = False\n", + "custom_params.triangulation.dynamicOutlierRejectionThreshold = 3.0 # Reject points with reproj error > 3*sigma\n", + "\n", + "print(\"\\nCustom Parameters:\")\n", + "custom_params.print()\n", + "\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}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_header_md" + }, + "source": [ + "## Usage" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_desc_md" + }, + "source": [ + "These `SmartProjectionParams` objects are passed to the constructors of smart factors, like `SmartProjectionPoseFactor`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "usage_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart Factor created with custom params:\n", + "SmartProjectionPoseFactor, z = \n", + " SmartProjectionFactor\n", + "linearizationMode: 2\n", + "triangulationParameters:\n", + "rankTolerance = 1e-08\n", + "enableEPI = false\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = 3\n", + "\n", + "\n", + "result:\n", + "Degenerate\n", + "\n", + "SmartFactorBase, z = \n", + "Factor\n" + ] + } + ], + "source": [ + "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 = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240)\n", + "\n", + "smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n", + "print(\"Smart Factor created with custom params:\")\n", + "smart_factor.print()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb new file mode 100644 index 000000000..afb0daa1e --- /dev/null +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -0,0 +1,343 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionFactor` is a \"smart\" factor designed for Structure from Motion (SfM) or visual SLAM problems where **both camera poses and calibration are being optimized**.\n", + "It implicitly represents a 3D point landmark that has been observed by multiple cameras.\n", + "\n", + "Key characteristics:\n", + "- **Implicit Point:** The 3D point is not an explicit variable in the factor graph. Instead, the factor internally triangulates the point based on the current camera estimates whenever needed (e.g., during linearization or error calculation).\n", + "- **Marginalization:** When linearized (e.g., to a Hessian factor), it effectively marginalizes out the 3D point, creating a dense factor connecting only the cameras that observed the point.\n", + "- **`CAMERA` Template:** This template parameter must represent a camera model that includes *both* pose and calibration (e.g., `PinholeCameraCal3_S2`, `PinholeCameraBundler`).\n", + "- **`Values` Requirement:** When using this factor, the `Values` object passed to methods like `error` or `linearize` must contain `CAMERA` objects (not separate `Pose3` and `Calib` objects) associated with the factor's keys.\n", + "- **Configuration:** Its behavior (linearization method, handling of degenerate triangulations) is controlled by `SmartProjectionParams`.\n", + "\n", + "**Use Case:** Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses.\n", + "**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "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": [ + "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" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating and Adding Measurements" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "1. Create the factor with a noise model and parameters.\n", + "2. Add measurements (2D points) and the corresponding camera keys one by one or in batches." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 3 measurements.\n", + "SmartFactor: SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1e-09\n", + "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" + ] + } + ], + "source": [ + "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "smart_params = SmartProjectionParams() # Use default params (HESSIAN, IGNORE_DEGENERACY)\n", + "\n", + "# Factor type includes the Camera type, e.g., SmartProjectionFactorPinholeCameraCal3_S2\n", + "smart_factor = SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n", + "\n", + "# Add measurements and keys\n", + "smart_factor.add(Point2(150, 505), C(0))\n", + "smart_factor.add(Point2(470, 495), C(1))\n", + "smart_factor.add(Point2(480, 150), C(2))\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n", + "smart_factor.print(\"SmartFactor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method implicitly triangulates the point based on the `CAMERA` objects in `values` and computes the sum of squared reprojection errors." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulated point result:\n", + "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" + ] + } + ], + "source": [ + "# Create Values containing CAMERA objects\n", + "values = Values()\n", + "K = Cal3_S2(500, 500, 0, 320, 240)\n", + "pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n", + "pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n", + "pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n", + "\n", + "values.insert(C(0), PinholeCameraCal3_S2(pose0, K))\n", + "values.insert(C(1), PinholeCameraCal3_S2(pose1, K))\n", + "values.insert(C(2), PinholeCameraCal3_S2(pose2, K))\n", + "\n", + "# Triangulate first to see the implicit point\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulated point result:\\n{point_result}\")\n", + "\n", + "if point_result.valid():\n", + " # Calculate error\n", + " total_error = smart_factor.error(values)\n", + " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "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", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb new file mode 100644 index 000000000..9f56fc8c9 --- /dev/null +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -0,0 +1,354 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionPoseFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionPoseFactor` is a \"smart\" factor optimized for visual SLAM problems where the **camera calibration is known and fixed**, and only the camera **poses** are being optimized.\n", + "It inherits from `SmartProjectionFactor` but simplifies the setup by taking a single shared `CALIBRATION` object (e.g., `Cal3_S2`, `Cal3Bundler`) assumed to be used by all cameras observing the landmark.\n", + "\n", + "Key characteristics:\n", + "- **Implicit Point:** Like its base class, the 3D point is not an explicit variable.\n", + "- **Fixed Calibration:** Assumes a single, known calibration `K` for all measurements.\n", + "- **Pose Variables:** The factor connects `Pose3` variables directly.\n", + "- **`Values` Requirement:** Requires `Pose3` objects in the `Values` container for its keys.\n", + "- **Configuration:** Behavior is controlled by `SmartProjectionParams`.\n", + "\n", + "**Use Case:** Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant.\n", + "**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "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": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", + " SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n", + " Cal3_S2)\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X # Key for Pose3 variable" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating and Adding Measurements" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "1. Create the factor with a noise model, the shared calibration `K`, and parameters.\n", + "2. Add measurements (2D points) and the corresponding pose keys." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 3 measurements.\n", + "SmartFactorPose: SmartProjectionPoseFactor, z = \n", + " SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1e-09\n", + "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 x0 x1 x2\n" + ] + } + ], + "source": [ + "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "smart_params = SmartProjectionParams() # Use default params\n", + "K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", + "\n", + "# Factor type includes the Calibration type\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", + "smart_factor.add(Point2(470, 495), X(1))\n", + "smart_factor.add(Point2(480, 150), X(2))\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n", + "smart_factor.print(\"SmartFactorPose: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method uses the `Pose3` objects from `values` and the fixed `K` to triangulate the point and compute the error." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulated point result:\n", + "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" + ] + } + ], + "source": [ + "# Create Values containing Pose3 objects\n", + "values = Values()\n", + "pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n", + "pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n", + "pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n", + "\n", + "values.insert(X(0), pose0)\n", + "values.insert(X(1), pose1)\n", + "values.insert(X(2), pose2)\n", + "\n", + "# Triangulate first to see the implicit point\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulated point result:\\n{point_result}\")\n", + "\n", + "if point_result.valid():\n", + " # Calculate error\n", + " total_error = smart_factor.error(values)\n", + " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "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": [ + "Linearization works similarly to `SmartProjectionFactor`, producing a linear factor (default: Hessian) connecting the `Pose3` variables." + ] + }, + { + "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(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", + "Augmented Diagonal Block [1,1]:\n", + "[ 768, -2084, 849; ]\n", + "[ -2084, 6054, -1412; ]\n", + "[ 849, -1412, 4392; ]\n", + "\n", + "Augmented Diagonal Block [2,2]:\n", + "[ 23563, 2137, 1132; ]\n", + "[ 2137, 15054, -1172; ]\n", + "[ 1132, -1172, 8384; ]\n", + "\n", + "Off-Diagonal Block [0,1]:\n", + "[ -5014, -4686, -552; ]\n", + "[ -9.522e+03, -1.623e+04, 447; ]\n", + "[ 3085, -3660, -1.118e+04; ]\n", + "\n", + "Off-Diagonal Block [0,2]:\n", + "[ 11515, 16951, 996; ]\n", + "[ 230, -2148, 7017; ]\n", + "[ -373, -620, -7830; ]\n", + "\n", + "Off-Diagonal Block [1,2]:\n", + "[ 6273, -830, -969; ]\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" + ] + } + ], + "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", + " # Note: The factor dimension is Pose3::dimension (6)\n", + " hessian_factor = gtsam.RegularHessianFactorPose3.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", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb new file mode 100644 index 000000000..ad3009001 --- /dev/null +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -0,0 +1,418 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionRigFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionRigFactor` is a generalization of `SmartProjectionPoseFactor` designed for multi-camera systems (rigs).\n", + "Like other smart factors, it implicitly represents a 3D point landmark observed by multiple cameras.\n", + "\n", + "Key differences/features:\n", + "- **Multi-Camera Rig:** Assumes a fixed rig configuration, where multiple cameras (`CAMERA` instances, which include fixed intrinsics and fixed extrinsics *relative to the rig's body frame*) are defined.\n", + "- **Pose Variables:** Connects `Pose3` variables representing the pose of the **rig's body frame** in the world.\n", + "- **Multiple Observations per Pose:** Allows multiple measurements associated with the *same* body pose key, but originating from different cameras within the rig.\n", + "- **Camera Indexing:** Each measurement must be associated with both a body pose key and a `cameraId` (index) specifying which camera in the rig took the measurement.\n", + "- **Fixed Calibration/Extrinsics:** The intrinsics and relative extrinsics of the cameras within the rig are assumed fixed.\n", + "- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholeCamera`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts.\n", + "- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n", + "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", + "\n", + "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_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, SmartProjectionRigFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m CameraSetPinholeCameraCal3_S2, 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 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", + " SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n", + " CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X # Key for Pose3 variable (Body Pose)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating the Rig and Factor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "1. Define the camera rig configuration: Create a `CameraSet` containing the `CAMERA` objects (with fixed intrinsics and rig-relative extrinsics).\n", + "2. Create the `SmartProjectionRigFactor` with noise, the rig, and parameters.\n", + "3. Add measurements, specifying the 2D point, the corresponding **body pose key**, and the **camera ID** within the rig." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 4 measurements from 2 unique poses.\n", + "SmartFactorRig: SmartProjectionRigFactor: \n", + " -- Measurement nr 0\n", + "key: x0\n", + "cameraId: 0\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 0 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "-- Measurement nr 1\n", + "key: x0\n", + "cameraId: 1\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 -0.1 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "-- Measurement nr 2\n", + "key: x1\n", + "cameraId: 0\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 0 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "-- Measurement nr 3\n", + "key: x1\n", + "cameraId: 1\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 -0.1 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1e-09\n", + "enableEPI = false\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "\n", + "\n", + "result:\n", + "Degenerate\n", + "\n", + "SmartFactorBase, z = \n", + "measurement 0, px = \n", + "[300; 200]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 1, px = \n", + "[250; 201]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 2, px = \n", + "[310; 210]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 3, px = \n", + "[260; 211]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "Factor on x0 x1\n" + ] + } + ], + "source": [ + "# 1. Define the Camera Rig\n", + "K = Cal3_S2(500, 500, 0, 320, 240)\n", + "# Camera 0: Forward facing, slightly offset\n", + "body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n", + "cam0 = PinholeCameraCal3_S2(body_P_cam0, K)\n", + "# Camera 1: Stereo camera, right of cam0\n", + "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", + "cam1 = PinholeCameraCal3_S2(body_P_cam1, K)\n", + "\n", + "rig_cameras = CameraSetPinholeCameraCal3_S2()\n", + "rig_cameras.append(cam0)\n", + "rig_cameras.append(cam1)\n", + "rig_cameras_ptr = gtsam.make_shared_CameraSetPinholeCameraCal3_S2(rig_cameras)\n", + "\n", + "# 2. Create the Factor\n", + "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "# Ensure parameters are compatible (HESSIAN, ZERO_ON_DEGENERACY)\n", + "smart_params = SmartProjectionParams(linearizationMode=gtsam.LinearizationMode.HESSIAN,\n", + " degeneracyMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", + "\n", + "# Factor type includes the Camera type\n", + "smart_factor = SmartProjectionRigFactorPinholeCameraCal3_S2(smart_noise, rig_cameras_ptr, smart_params)\n", + "\n", + "# 3. Add measurements\n", + "# Observation from Body Pose X(0), Camera 0\n", + "smart_factor.add(Point2(300, 200), X(0), 0)\n", + "# Observation from Body Pose X(0), Camera 1 (stereo pair?)\n", + "smart_factor.add(Point2(250, 201), X(0), 1)\n", + "# Observation from Body Pose X(1), Camera 0\n", + "smart_factor.add(Point2(310, 210), X(1), 0)\n", + "# Observation from Body Pose X(1), Camera 1\n", + "smart_factor.add(Point2(260, 211), X(1), 1)\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n", + "smart_factor.print(\"SmartFactorRig: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method uses the `Pose3` objects (body poses) from `values` and the fixed rig configuration to triangulate the point and compute the error." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulated point result:\n", + "Valid triangulation with point [0.70307883 0.20615766 5.18676602]\n", + "\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 181.1904\n" + ] + } + ], + "source": [ + "# Create Values containing Body Pose3 objects\n", + "values = Values()\n", + "pose0 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))\n", + "pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n", + "values.insert(X(0), pose0)\n", + "values.insert(X(1), pose1)\n", + "\n", + "# Triangulate first to see the implicit point\n", + "# The 'cameras' method internally combines body poses with rig extrinsics\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulated point result:\\n{point_result}\")\n", + "\n", + "if point_result.valid():\n", + " # Calculate error\n", + " total_error = smart_factor.error(values)\n", + " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "else:\n", + " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")\n", + " # Since mode is ZERO_ON_DEGENERACY, error should be 0\n", + " total_error = smart_factor.error(values)\n", + " print(f\"Error when degenerate: {total_error}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_header_md" + }, + "source": [ + "## Linearization" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_desc_md" + }, + "source": [ + "Linearization (currently restricted to HESSIAN mode) produces a `RegularHessianFactor` connecting the unique body pose (`Pose3`) variables involved." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "linearize_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearized Factor (HessianFactor structure):\n", + "RegularHessianFactor(6): density=100% keys={ x0 x1 }\n", + "Augmented information matrix: (dimensions: 6, 6) : \n", + "{\n", + "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", + "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", + "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", + "\t[ -15.09, -101.6, -33.87, 181.2, -46.13, 41.54; ]\n", + "\t[ 3.829, 25.77, 8.589, -46.13, 11.71, -10.54; ]\n", + "\t[ -3.448, -23.21, -7.737, 41.54, -10.54, 9.497; ]\n", + "\t[ 1.257, 8.427, 2.81, -1.257, -8.427, -2.81; ]\n", + "\t[ 8.427, 56.73, 18.91, -8.427, -56.73, -18.91; ]\n", + "\t[ 2.81, 18.91, 6.302, -2.81, -18.91, -6.302; ]\n", + "\t[ -15.09, -101.6, -33.87, 15.09, 101.6, 33.87; ]\n", + "\t[ 3.829, 25.77, 8.589, -3.829, -25.77, -8.589; ]\n", + "\t[ -3.448, -23.21, -7.737, 3.448, 23.21, 7.737; ]\n", + "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", + "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", + "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", + "\t[ 15.09, 101.6, 33.87, -181.2, 46.13, -41.54; ]\n", + "\t[ -3.829, -25.77, -8.589, 46.13, -11.71, 10.54; ]\n", + "\t[ 3.448, 23.21, 7.737, -41.54, 10.54, -9.497; ]\n", + "}\n", + "Augmented Diagonal Block [0,0]:\n", + "[ 1.257, 8.427, 2.81; ]\n", + "[ 8.427, 56.73, 18.91; ]\n", + "[ 2.81, 18.91, 6.302; ]\n", + "\n", + "Augmented Diagonal Block [1,1]:\n", + "[ 1.257, 8.427, 2.81; ]\n", + "[ 8.427, 56.73, 18.91; ]\n", + "[ 2.81, 18.91, 6.302; ]\n", + "\n", + "Off-Diagonal Block [0,1]:\n", + "[ -1.257, -8.427, -2.81; ]\n", + "[ -8.427, -56.73, -18.91; ]\n", + "[ -2.81, -18.91, -6.302; ]\n", + "\n", + "Error vector:\n", + "[-15.087; -101.588; -33.8672; 181.19; -46.1294; 41.5391; 15.087; 101.588; 33.8672; -181.19; 46.1294; -41.5391]\n", + "Constant error term: 181.19\n" + ] + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "graph.add(smart_factor)\n", + "\n", + "# Linearize (HESSIAN mode)\n", + "linear_factor = smart_factor.linearize(values)\n", + "\n", + "if linear_factor:\n", + " print(\"\\nLinearized Factor (HessianFactor structure):\")\n", + " hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n", + " if hessian_factor:\n", + " hessian_factor.print()\n", + " else:\n", + " print(\"Linearized factor is not a HessianFactor\")\n", + "else:\n", + " print(\"Linearization failed (likely due to triangulation degeneracy)\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/StereoFactor.ipynb b/gtsam/slam/doc/StereoFactor.ipynb new file mode 100644 index 000000000..98b597a9f --- /dev/null +++ b/gtsam/slam/doc/StereoFactor.ipynb @@ -0,0 +1,246 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# GenericStereoFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`GenericStereoFactor` is a factor for handling measurements from a **calibrated stereo camera**.\n", + "It relates a 3D `LANDMARK` (usually `Point3`) to a `StereoPoint2` measurement observed by a stereo camera system defined by a `POSE` (usually `Pose3`) and a fixed stereo calibration `Cal3_S2Stereo`.\n", + "\n", + "`StereoPoint2` contains $(u_L, u_R, v)$, the horizontal pixel coordinates in the left ($u_L$) and right ($u_R$) images, and the vertical pixel coordinate ($v$), which is assumed the same for both images in a rectified stereo setup.\n", + "`Cal3_S2Stereo` holds the intrinsic parameters (focal length, principal point) common to both cameras and the stereo baseline (distance between camera centers).\n", + "\n", + "Key features:\n", + "- **Templated:** Works with different pose and landmark types.\n", + "- **Fixed Calibration:** Assumes the `Cal3_S2Stereo` object (`K_`) is known and fixed.\n", + "- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform.\n", + "- **Cheirality Handling:** Can be configured for points behind the camera.\n", + "\n", + "The error is the 3D vector difference:\n", + "$$ \\text{error}(P, L) = \\text{projectStereo}(P \\cdot S, L) - z $$\n", + "where `projectStereo` uses the `StereoCamera` model, $P$ is the pose, $L$ the landmark, $S$ the optional offset, and $z$ is the `measured_` `StereoPoint2`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n", + "# The Python wrapper often creates specific instantiations\n", + "from gtsam import GenericStereoFactor3D\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a GenericStereoFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing:\n", + "1. The measurement (`StereoPoint2`).\n", + "2. The noise model (typically 3D).\n", + "3. The key for the pose variable.\n", + "4. The key for the landmark variable.\n", + "5. A `shared_ptr` to the fixed stereo calibration object (`Cal3_S2Stereo`).\n", + "6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n", + "7. (Optional) Cheirality handling flags." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor with offset: keys = { x0 l1 }\n", + " noise model: unit (3) \n", + "Factor with offset: .z(330, 305, 250)\n", + " sensor pose in body frame: R: [\n", + "\t6.12323e-17, 6.12323e-17, 1;\n", + "\t-1, 3.7494e-33, 6.12323e-17;\n", + "\t-0, -1, 6.12323e-17\n", + "]\n", + "t: 0.1 0 0.2\n", + "\n", + "Factor without offset: keys = { x0 l1 }\n", + " noise model: unit (3) \n", + "\n", + "Factor without offset: .z(330, 305, 250)\n" + ] + } + ], + "source": [ + "measured_stereo = StereoPoint2(330, 305, 250) # uL, uR, v\n", + "stereo_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) # 1 pixel std dev (ul, ur, v)\n", + "pose_key = X(0)\n", + "landmark_key = L(1)\n", + "\n", + "# Shared pointer to stereo calibration\n", + "K_stereo = Cal3_S2Stereo(500.0, 500.0, 0.0, 320.0, 240.0, 0.1) # fx, fy, s, u0, v0, baseline\n", + "\n", + "# Optional sensor pose offset\n", + "body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n", + "\n", + "# Create factor with sensor offset\n", + "factor_with_offset = GenericStereoFactor3D(\n", + " measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo, body_P_sensor=body_P_sensor)\n", + "factor_with_offset.print(\"Factor with offset: \")\n", + "\n", + "# Create factor without sensor offset\n", + "factor_no_offset = GenericStereoFactor3D(\n", + " measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo)\n", + "factor_no_offset.print(\"\\nFactor without offset: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error is the 3D difference between the predicted stereo projection and the measurement." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "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", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Example values\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(landmark_key, landmark)\n", + "\n", + "# Evaluate factor without offset\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", + "\n", + "# Evaluate factor with offset\n", + "# Need to recompute landmark based on offset pose\n", + "pose_with_offset = pose * body_P_sensor # This is world_P_sensor\n", + "expected_point_offset_cam = K_stereo.backproject(measured_stereo)\n", + "landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam)\n", + "values.update(landmark_key, landmark_offset)\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", + "\n", + "# Evaluate with noisy landmark\n", + "noisy_landmark = landmark + Point3(0.1, -0.05, 0.1)\n", + "values.update(landmark_key, noisy_landmark)\n", + "error_no_offset_noisy = factor_no_offset.error(values)\n", + "print(f\"Error (no offset) at noisy landmark: {error_no_offset_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/TriangulationFactor.ipynb b/gtsam/slam/doc/TriangulationFactor.ipynb new file mode 100644 index 000000000..0bf9ea75a --- /dev/null +++ b/gtsam/slam/doc/TriangulationFactor.ipynb @@ -0,0 +1,283 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# TriangulationFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`TriangulationFactor` is a unary factor that constrains a single unknown 3D point (`Point3`) based on a 2D measurement from a **known** camera.\n", + "It's essentially the inverse of a projection factor where the camera pose and calibration are fixed, and only the 3D point is variable.\n", + "\n", + "Key characteristics:\n", + "- **Unary Factor:** Connects only to a `Point3` variable key.\n", + "- **Known Camera:** The `CAMERA` object (e.g., `PinholeCameraCal3_S2`, `StereoCamera`) containing the fixed pose and calibration is provided during factor construction.\n", + "- **Measurement:** Takes a 2D measurement (`Point2` for monocular, `StereoPoint2` for stereo).\n", + "- **Error:** Calculates the reprojection error: $ \\text{error}(L) = \\text{camera.project}(L) - z $\n", + "\n", + "**Use Case:** Useful in triangulation scenarios where multiple camera views with known poses observe an unknown landmark. A `NonlinearFactorGraph` containing only `TriangulationFactor`s (one for each view) can be optimized to find the maximum likelihood estimate of the 3D landmark position." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Point3, Point2, Pose3, Rot3, Cal3_S2, PinholeCameraCal3_S2, Values, NonlinearFactorGraph\n", + "# The Python wrapper often creates specific instantiations\n", + "from gtsam import TriangulationFactorCal3_S2\n", + "from gtsam import symbol_shorthand\n", + "\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a TriangulationFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing:\n", + "1. The known `CAMERA` object.\n", + "2. The 2D measurement.\n", + "3. The noise model for the measurement.\n", + "4. The key for the unknown `Point3` landmark.\n", + "5. (Optional) Cheirality handling flags." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "TriangulationFactor: TriangulationFactor,camera.pose R: [\n", + "\t0.990033, -0.117578, -0.0775207;\n", + "\t0.0993347, 0.97319, -0.207445;\n", + "\t0.0998334, 0.197677, 0.97517\n", + "]\n", + "t: 1 0 0.5\n", + "camera.calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "z[\n", + "\t450.5;\n", + "\t300.2\n", + "]\n", + " keys = { l0 }\n", + " noise model: unit (2) \n" + ] + } + ], + "source": [ + "# Known camera parameters\n", + "K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n", + "pose = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(1, 0, 0.5))\n", + "camera = PinholeCameraCal3_S2(pose, K)\n", + "\n", + "# Measurement observed by this camera\n", + "measured_pt2 = Point2(450.5, 300.2)\n", + "pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "landmark_key = L(0)\n", + "\n", + "# Factor type includes the Camera type\n", + "factor = TriangulationFactorCal3_S2(camera, measured_pt2, pixel_noise, landmark_key)\n", + "factor.print(\"TriangulationFactor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error is the reprojection error given an estimate of the landmark's `Point3` position." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Reprojection error for estimate [3. 0.5 2. ]: 314012.75623020524\n", + "Expected projection: [1225.10768109 467.55726116]\n", + "Manual error calculation: [774.60768109 167.35726116]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Estimate for the landmark position\n", + "landmark_estimate = Point3(3.0, 0.5, 2.0)\n", + "values.insert(landmark_key, landmark_estimate)\n", + "\n", + "error = factor.error(values)\n", + "print(f\"Reprojection error for estimate {landmark_estimate}: {error}\")\n", + "\n", + "# Calculate expected projection\n", + "expected_projection = camera.project(landmark_estimate)\n", + "manual_error = expected_projection - measured_pt2\n", + "print(f\"Expected projection: {expected_projection}\")\n", + "print(f\"Manual error calculation: {manual_error}\")\n", + "assert np.allclose(factor.unwhitenedError(values), manual_error)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_header_md" + }, + "source": [ + "## Usage in Triangulation" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_desc_md" + }, + "source": [ + "Multiple `TriangulationFactor`s, one for each known camera view, can be added to a graph to solve for the landmark position." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "id": "usage_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Optimized Landmark Position:\n", + "Values with 1 values:\n", + "Value l0: (class Eigen::Matrix)\n", + "[\n", + "\t-12446.1;\n", + "\t-55075.8;\n", + "\t2.39319e+06\n", + "]\n", + "\n", + "Final Error: 7855.8598\n" + ] + } + ], + "source": [ + "# Create a second camera and measurement\n", + "pose2 = Pose3(Rot3.Ypr(-0.1, 0.1, -0.1), Point3(-1, 0, 0.5))\n", + "camera2 = PinholeCameraCal3_S2(pose2, K)\n", + "measured_pt2_cam2 = Point2(180.0, 190.0)\n", + "factor2 = TriangulationFactorCal3_S2(camera2, measured_pt2_cam2, pixel_noise, landmark_key)\n", + "\n", + "# Create graph and add factors\n", + "triangulation_graph = NonlinearFactorGraph()\n", + "triangulation_graph.add(factor)\n", + "triangulation_graph.add(factor2)\n", + "\n", + "# Optimize (requires an initial estimate)\n", + "initial_estimate = Values()\n", + "initial_estimate.insert(landmark_key, Point3(2, 0, 5)) # Initial guess\n", + "\n", + "optimizer = gtsam.LevenbergMarquardtOptimizer(triangulation_graph, initial_estimate)\n", + "result = optimizer.optimize()\n", + "\n", + "print(\"\\nOptimized Landmark Position:\")\n", + "result.print()\n", + "print(f\"Final Error: {triangulation_graph.error(result):.4f}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/dataset.ipynb b/gtsam/slam/doc/dataset.ipynb new file mode 100644 index 000000000..2c2130cf3 --- /dev/null +++ b/gtsam/slam/doc/dataset.ipynb @@ -0,0 +1,252 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Dataset Utilities" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `gtsam/slam/dataset.h` header provides utility functions for loading and saving factor graph data, particularly in formats commonly used in the SLAM community like TORO ('.graph') and g2o ('.g2o').\n", + "\n", + "Key functions include:\n", + "* `findExampleDataFile(name)`: Locates example dataset files distributed with GTSAM.\n", + "* `load2D(filename, ...)`: Loads a 2D pose graph (Poses and BetweenFactors) from TORO or g2o format.\n", + "* `load3D(filename)`: Loads a 3D pose graph (currently simpler than `load2D`, assumes specific format).\n", + "* `readG2o(filename, is3D)`: A more general function to read g2o files containing various factor and variable types (2D/3D poses, landmarks, measurements).\n", + "* `writeG2o(graph, initialEstimate, filename)`: Saves a factor graph and values to the g2o format.\n", + "* `parseVariables`/`parseMeasurements`/`parseFactors`: Lower-level parsing functions (less commonly used directly)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import gtsam.utils.plot as gtsam_plot\n", + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "import os\n", + "\n", + "# Ensure the example datasets are available\n", + "# In Colab/pip install, they are usually included.\n", + "# If running locally from build, findExampleDataFile works.\n", + "# If running locally from install without examples, this might fail.\n", + "try:\n", + " # Check for a common example file\n", + " gtsam.findExampleDataFile(\"w100.graph\")\n", + " HAVE_EXAMPLES = True\n", + "except RuntimeError:\n", + " print(\"Warning: Example datasets not found.\")\n", + " print(\"Try running from build directory or installing examples.\")\n", + " HAVE_EXAMPLES = False\n", + "\n", + "# Create dummy files for writing examples if needed\n", + "if not os.path.exists(\"output\"):\n", + " os.makedirs(\"output\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "find_header_md" + }, + "source": [ + "## Finding Example Datasets" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "find_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Found w100.graph at: c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\Data\\w100.graph\n", + "Found dlr.g2o at: None\n" + ] + } + ], + "source": [ + "if HAVE_EXAMPLES:\n", + " try:\n", + " w100_path = gtsam.findExampleDataFile(\"w100.graph\")\n", + " print(f\"Found w100.graph at: {w100_path}\")\n", + " dlr_path = gtsam.findExampleDataFile(\"dlr.g2o\")\n", + " print(f\"Found dlr.g2o at: {dlr_path}\")\n", + " except RuntimeError as e:\n", + " print(f\"Error finding example file: {e}\")\n", + "else:\n", + " print(\"Skipping findExampleDataFile test as examples are not available.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "load2d_header_md" + }, + "source": [ + "## Loading 2D Datasets (`load2D`)" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "load2d_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Loaded c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\Data\\w100.graph:\n", + " Graph size: 300\n", + " Initial estimate keys: 100\n" + ] + } + ], + "source": [ + "if HAVE_EXAMPLES:\n", + " # Load TORO 2D file\n", + " graph_2d, initial_2d = gtsam.load2D(w100_path)\n", + " print(f\"\\nLoaded {w100_path}:\")\n", + " print(f\" Graph size: {graph_2d.size()}\")\n", + " print(f\" Initial estimate keys: {len(initial_2d.keys())}\")\n", + "\n", + " # Plot initial estimate (optional)\n", + " for key in initial_2d.keys():\n", + " gtsam_plot.plot_pose2(0, initial_2d.atPose2(key))\n", + " plt.title(\"Initial Estimate from w100.graph\")\n", + " plt.axis('equal')\n", + " # plt.show() # Uncomment to display plot\n", + " plt.close() # Close plot to prevent display in output\n", + "else:\n", + " print(\"\\nSkipping load2D test.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "g2o_header_md" + }, + "source": [ + "## Loading/Saving G2O Files (`readG2o`, `writeG2o`)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "g2o_desc_md" + }, + "source": [ + "`readG2o` can handle both 2D and 3D datasets and various factor types defined in the g2o format.\n", + "`writeG2o` saves a `NonlinearFactorGraph` and `Values` into a g2o file." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "id": "g2o_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error processing None: readG2o(): incompatible function arguments. The following argument types are supported:\n", + " 1. (filename: str, is3D: bool = False, kernelFunctionType: gtsam.gtsam.KernelFunctionType = ) -> tuple[gtsam.gtsam.NonlinearFactorGraph, gtsam.gtsam.Values]\n", + "\n", + "Invoked with: None; kwargs: is3D=True\n" + ] + } + ], + "source": [ + "if HAVE_EXAMPLES:\n", + " # Load a 3D g2o file\n", + " try:\n", + " graph_3d, initial_3d = gtsam.readG2o(dlr_path, is3D=True)\n", + " print(f\"\\nLoaded {dlr_path} (3D):\")\n", + " print(f\" Graph size: {graph_3d.size()}\")\n", + " print(f\" Initial estimate keys: {len(initial_3d.keys())}\")\n", + " # You could optimize graph_3d with initial_3d here...\n", + " print(\"Optimization skipped for brevity.\")\n", + " optimized_values = initial_3d # Use initial for demo write\n", + "\n", + " # Save the graph and values back to a g2o file\n", + " output_g2o_file = os.path.join(\"output\", \"dlr_rewrite.g2o\")\n", + " gtsam.writeG2o(graph_3d, optimized_values, output_g2o_file)\n", + " print(f\" Saved graph and values to {output_g2o_file}\")\n", + " # Clean up dummy file\n", + " # os.remove(output_g2o_file)\n", + " # os.rmdir(\"output\")\n", + " except (RuntimeError, TypeError) as e:\n", + " print(f\"Error processing {dlr_path}: {e}\")\n", + "else:\n", + " print(\"\\nSkipping readG2o/writeG2o test.\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/lago.ipynb b/gtsam/slam/doc/lago.ipynb new file mode 100644 index 000000000..3bd5de668 --- /dev/null +++ b/gtsam/slam/doc/lago.ipynb @@ -0,0 +1,242 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# LAGO (Linear Approximation for Graph Optimization)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `gtsam::lago` namespace provides functions for initializing `Pose2` estimates in a 2D SLAM factor graph.\n", + "LAGO stands for Linear Approximation for Graph Optimization. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", + "\n", + "The core idea is:\n", + "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", + "2. **Estimate Translations:** Given the estimated orientations, compute the translations by solving another linear system.\n", + "\n", + "Key functions:\n", + "* `initializeOrientations(graph)`: Computes initial estimates for the `Rot2` (orientation) components of the poses in the graph.\n", + "* `initialize(graph)`: Computes initial estimates for the full `Pose2` variables (orientations and translations).\n", + "* `initialize(graph, initialGuess)`: Corrects only the orientation part of a given `initialGuess` using LAGO.\n", + "\n", + "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import gtsam.utils.plot as gtsam_plot\n", + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "from gtsam import NonlinearFactorGraph, Values, Pose2, Point3, PriorFactorPose2, BetweenFactorPose2\n", + "from gtsam import lago\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_header_md" + }, + "source": [ + "## Example Initialization" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_desc_md" + }, + "source": [ + "We'll create a simple 2D pose graph with odometry and a loop closure, then use `lago.initialize`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "example_pipeline_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph:\n", + "size: 6\n", + "\n", + "Factor 0: PriorFactor on x0\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n", + "\n", + "Factor 1: BetweenFactor(x0,x1)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 2: BetweenFactor(x1,x2)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 3: BetweenFactor(x2,x3)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 4: BetweenFactor(x3,x4)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 5: BetweenFactor(x4,x0)\n", + " measured: (2.1, 0.1, 1.62079633)\n", + " noise model: diagonal sigmas [0.25; 0.25; 0.15];\n", + "\n" + ] + }, + { + "ename": "IndexError", + "evalue": "invalid map key", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mIndexError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[3], line 25\u001b[0m\n\u001b[0;32m 22\u001b[0m graph\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOriginal Factor Graph:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;66;03m# 2. Perform LAGO initialization\u001b[39;00m\n\u001b[1;32m---> 25\u001b[0m initial_estimate_lago \u001b[38;5;241m=\u001b[39m \u001b[43mlago\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minitialize\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mInitial Estimate from LAGO:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 28\u001b[0m initial_estimate_lago\u001b[38;5;241m.\u001b[39mprint()\n", + "\u001b[1;31mIndexError\u001b[0m: invalid map key" + ] + } + ], + "source": [ + "# 1. Create a NonlinearFactorGraph with Pose2 factors\n", + "graph = NonlinearFactorGraph()\n", + "\n", + "# Add a prior on the first pose\n", + "prior_mean = Pose2(0.0, 0.0, 0.0)\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))\n", + "graph.add(PriorFactorPose2(X(0), prior_mean, prior_noise))\n", + "\n", + "# Add odometry factors (simulating moving in a square)\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", + "graph.add(BetweenFactorPose2(X(0), X(1), Pose2(2.0, 0.0, 0.0), odometry_noise))\n", + "graph.add(BetweenFactorPose2(X(1), X(2), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(X(2), X(3), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(X(3), X(4), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "\n", + "# Add a loop closure factor\n", + "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))\n", + "# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)\n", + "measured_loop = Pose2(2.1, 0.1, np.pi/2 + 0.05)\n", + "graph.add(BetweenFactorPose2(X(4), X(0), measured_loop, loop_noise))\n", + "\n", + "graph.print(\"Original Factor Graph:\\n\")\n", + "\n", + "# 2. Perform LAGO initialization\n", + "initial_estimate_lago = lago.initialize(graph)\n", + "\n", + "print(\"\\nInitial Estimate from LAGO:\\n\")\n", + "initial_estimate_lago.print()\n", + "\n", + "# 3. Visualize the LAGO estimate (optional)\n", + "plt.figure(1)\n", + "for key in initial_estimate_lago.keys():\n", + " gtsam_plot.plot_pose2(1, initial_estimate_lago.atPose2(key), 0.5)\n", + "plt.title(\"LAGO Initial Estimate\")\n", + "plt.axis('equal')\n", + "# plt.show()\n", + "plt.close() # Close plot to prevent display in output\n", + "\n", + "# 4. Compare orientation initialization\n", + "initial_orientations = lago.initializeOrientations(graph)\n", + "print(\"\\nLAGO Orientations (VectorValues):\")\n", + "initial_orientations.print()\n", + "\n", + "print(\"Orientations from full LAGO Values:\")\n", + "for i in range(5):\n", + " print(f\" X({i}): {initial_estimate_lago.atPose2(X(i)).theta():.4f}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_header_md" + }, + "source": [ + "## Notes" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_desc_md" + }, + "source": [ + "- LAGO provides a good initial guess, especially for orientations, which can significantly help the convergence of nonlinear optimizers.\n", + "- It assumes the graph structure allows for the orientation estimation (typically requires a spanning tree and a prior).\n", + "- The translation estimates are computed based on the fixed, estimated orientations." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} From e4cf0a2cf90101e92ec4eea8ec79b29c98a010ee Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 24 Apr 2025 10:55:19 -0400 Subject: [PATCH 02/17] pip install gtsam-develop --- gtsam/slam/doc/BetweenFactor.ipynb | 2 +- gtsam/slam/doc/EssentialMatrixConstraint.ipynb | 2 +- gtsam/slam/doc/EssentialMatrixFactor.ipynb | 2 +- gtsam/slam/doc/FrobeniusFactor.ipynb | 2 +- gtsam/slam/doc/GeneralSFMFactor.ipynb | 2 +- gtsam/slam/doc/InitializePose3.ipynb | 2 +- gtsam/slam/doc/KarcherMeanFactor.ipynb | 2 +- gtsam/slam/doc/OrientedPlane3Factor.ipynb | 2 +- gtsam/slam/doc/PlanarProjectionFactor.ipynb | 2 +- gtsam/slam/doc/PoseRotationPrior.ipynb | 2 +- gtsam/slam/doc/PoseTranslationPrior.ipynb | 2 +- gtsam/slam/doc/ProjectionFactor.ipynb | 2 +- gtsam/slam/doc/ReferenceFrameFactor.ipynb | 2 +- gtsam/slam/doc/RotateFactor.ipynb | 2 +- gtsam/slam/doc/SmartFactorParams.ipynb | 2 +- gtsam/slam/doc/SmartProjectionFactor.ipynb | 2 +- gtsam/slam/doc/SmartProjectionPoseFactor.ipynb | 2 +- gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 2 +- gtsam/slam/doc/StereoFactor.ipynb | 2 +- gtsam/slam/doc/TriangulationFactor.ipynb | 2 +- gtsam/slam/doc/dataset.ipynb | 2 +- gtsam/slam/doc/lago.ipynb | 2 +- 22 files changed, 22 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/doc/BetweenFactor.ipynb b/gtsam/slam/doc/BetweenFactor.ipynb index 4eef414bb..da2d98e63 100644 --- a/gtsam/slam/doc/BetweenFactor.ipynb +++ b/gtsam/slam/doc/BetweenFactor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/EssentialMatrixConstraint.ipynb b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb index fe54c0515..b8d23505b 100644 --- a/gtsam/slam/doc/EssentialMatrixConstraint.ipynb +++ b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb @@ -47,7 +47,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/EssentialMatrixFactor.ipynb b/gtsam/slam/doc/EssentialMatrixFactor.ipynb index cac43612c..9291b2ec0 100644 --- a/gtsam/slam/doc/EssentialMatrixFactor.ipynb +++ b/gtsam/slam/doc/EssentialMatrixFactor.ipynb @@ -48,7 +48,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/FrobeniusFactor.ipynb b/gtsam/slam/doc/FrobeniusFactor.ipynb index fdf317da8..cba959dc6 100644 --- a/gtsam/slam/doc/FrobeniusFactor.ipynb +++ b/gtsam/slam/doc/FrobeniusFactor.ipynb @@ -45,7 +45,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/GeneralSFMFactor.ipynb b/gtsam/slam/doc/GeneralSFMFactor.ipynb index 366254d1a..3fdfb6408 100644 --- a/gtsam/slam/doc/GeneralSFMFactor.ipynb +++ b/gtsam/slam/doc/GeneralSFMFactor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/InitializePose3.ipynb b/gtsam/slam/doc/InitializePose3.ipynb index 2e65b906b..b132697b2 100644 --- a/gtsam/slam/doc/InitializePose3.ipynb +++ b/gtsam/slam/doc/InitializePose3.ipynb @@ -48,7 +48,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/KarcherMeanFactor.ipynb b/gtsam/slam/doc/KarcherMeanFactor.ipynb index a1ca7a51e..973987286 100644 --- a/gtsam/slam/doc/KarcherMeanFactor.ipynb +++ b/gtsam/slam/doc/KarcherMeanFactor.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb index e140dbd71..1600674b2 100644 --- a/gtsam/slam/doc/OrientedPlane3Factor.ipynb +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -43,7 +43,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb index ce7c37a28..55d76252d 100644 --- a/gtsam/slam/doc/PlanarProjectionFactor.ipynb +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -50,7 +50,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/PoseRotationPrior.ipynb b/gtsam/slam/doc/PoseRotationPrior.ipynb index cb7d09a75..444658711 100644 --- a/gtsam/slam/doc/PoseRotationPrior.ipynb +++ b/gtsam/slam/doc/PoseRotationPrior.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/PoseTranslationPrior.ipynb b/gtsam/slam/doc/PoseTranslationPrior.ipynb index 047bf5e59..a71c61c02 100644 --- a/gtsam/slam/doc/PoseTranslationPrior.ipynb +++ b/gtsam/slam/doc/PoseTranslationPrior.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/ProjectionFactor.ipynb b/gtsam/slam/doc/ProjectionFactor.ipynb index 50bc5d71d..43d7e0f07 100644 --- a/gtsam/slam/doc/ProjectionFactor.ipynb +++ b/gtsam/slam/doc/ProjectionFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb index 5ea646d19..13ca2adef 100644 --- a/gtsam/slam/doc/ReferenceFrameFactor.ipynb +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/RotateFactor.ipynb b/gtsam/slam/doc/RotateFactor.ipynb index a62e986ad..edb658d05 100644 --- a/gtsam/slam/doc/RotateFactor.ipynb +++ b/gtsam/slam/doc/RotateFactor.ipynb @@ -41,7 +41,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartFactorParams.ipynb b/gtsam/slam/doc/SmartFactorParams.ipynb index b19218538..8ff32bdc1 100644 --- a/gtsam/slam/doc/SmartFactorParams.ipynb +++ b/gtsam/slam/doc/SmartFactorParams.ipynb @@ -57,7 +57,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index afb0daa1e..8dd7606ff 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index 9f56fc8c9..be34d6b44 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index ad3009001..09d23b3ca 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/StereoFactor.ipynb b/gtsam/slam/doc/StereoFactor.ipynb index 98b597a9f..08c1b6ab4 100644 --- a/gtsam/slam/doc/StereoFactor.ipynb +++ b/gtsam/slam/doc/StereoFactor.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/TriangulationFactor.ipynb b/gtsam/slam/doc/TriangulationFactor.ipynb index 0bf9ea75a..df324ad95 100644 --- a/gtsam/slam/doc/TriangulationFactor.ipynb +++ b/gtsam/slam/doc/TriangulationFactor.ipynb @@ -47,7 +47,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/dataset.ipynb b/gtsam/slam/doc/dataset.ipynb index 2c2130cf3..cb9c40741 100644 --- a/gtsam/slam/doc/dataset.ipynb +++ b/gtsam/slam/doc/dataset.ipynb @@ -46,7 +46,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/lago.ipynb b/gtsam/slam/doc/lago.ipynb index 3bd5de668..aa6d0e98b 100644 --- a/gtsam/slam/doc/lago.ipynb +++ b/gtsam/slam/doc/lago.ipynb @@ -50,7 +50,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { From e226cdde9f8c12c48556170cb5c9ce9dd3fb0b89 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 24 Apr 2025 10:59:02 -0400 Subject: [PATCH 03/17] yml and summary --- gtsam/slam/slam.md | 54 ++++++++++++++++++++++++++++++++++++++++++++++ myst.yml | 3 +++ 2 files changed, 57 insertions(+) create mode 100644 gtsam/slam/slam.md diff --git a/gtsam/slam/slam.md b/gtsam/slam/slam.md new file mode 100644 index 000000000..4ae7875c7 --- /dev/null +++ b/gtsam/slam/slam.md @@ -0,0 +1,54 @@ +# SLAM Factors and Algorithms + +The `slam` module provides a collection of factors, constraints, utilities, and initialization algorithms commonly used in Simultaneous Localization and Mapping (SLAM) and Structure from Motion (SfM) applications. It builds upon the core GTSAM inference engine (`gtsam/inference`) and geometric types (`gtsam/geometry`). + +## Core Factors + +These are fundamental factor types often used as building blocks in SLAM. + +- [BetweenFactor](doc/BetweenFactor.ipynb) : Represents relative measurements between two poses or other Lie group variables (e.g., odometry). +- [PoseRotationPrior](doc/PoseRotationPrior.ipynb) : A prior factor acting only on the rotation component of a pose variable. +- [PoseTranslationPrior](doc/PoseTranslationPrior.ipynb) : A prior factor acting only on the translation component of a pose variable. + +## Visual SLAM/SfM Factors + +Factors specifically designed for visual data (camera measurements). + +- [GenericProjectionFactor](doc/GenericProjectionFactor.ipynb) : Standard monocular projection factor relating a 3D landmark, camera pose, and fixed calibration to a 2D measurement. +- [StereoFactor](doc/StereoFactor.ipynb) : Standard stereo projection factor relating a 3D landmark, camera pose, and fixed stereo calibration to a `StereoPoint2` measurement. +- [PlanarProjectionFactor](doc/PlanarProjectionFactor.ipynb) : Projection factors specialized for robots moving on a 2D plane. +- [GeneralSFMFactor](doc/GeneralSFMFactor.ipynb) : Projection factors used when camera calibration is unknown or optimized alongside poses and landmarks. +- [EssentialMatrixFactor](doc/EssentialMatrixFactor.ipynb) : Factors constraining poses or calibration based on the Essential matrix derived from calibrated cameras. +- [EssentialMatrixConstraint](doc/EssentialMatrixConstraint.ipynb) : Factor constraining the relative pose between two cameras based on a measured Essential matrix. +- [TriangulationFactor](doc/TriangulationFactor.ipynb) : Factor constraining a 3D point based on a measurement from a single known camera view, useful for triangulation. + +## Smart Factors + +Factors that implicitly manage landmark variables, marginalizing them out during optimization. + +- [SmartFactorParams](doc/SmartFactorParams.ipynb) : Configuration parameters controlling the behavior of smart factors (linearization, degeneracy handling, etc.). +- [SmartProjectionFactor](doc/SmartProjectionFactor.ipynb) : Smart factor for monocular measurements where both camera pose and calibration are optimized. +- [SmartProjectionPoseFactor](doc/SmartProjectionPoseFactor.ipynb) : Smart factor for monocular measurements where camera calibration is fixed, optimizing only poses. +- [SmartProjectionRigFactor](doc/SmartProjectionRigFactor.ipynb) : Smart factor for calibrated multi-camera rigs, optimizing only the rig's body pose. +- [SmartFactorBase](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartFactorBase.h) : Abstract base class for smart factors (internal use). + +## Other Geometric Factors & Constraints + +Factors representing various geometric relationships or constraints. + +- [OrientedPlane3Factor](doc/OrientedPlane3Factor.ipynb) : Factors for estimating and constraining 3D planar landmarks (`OrientedPlane3`). +- [RotateFactor](doc/RotateFactor.ipynb) : Factors constraining an unknown rotation based on how it transforms measured rotations or directions. +- [KarcherMeanFactor](doc/KarcherMeanFactor.ipynb) : Factor for constraining the Karcher mean (geometric average) of a set of rotations or other manifold values. +- [FrobeniusFactor](doc/FrobeniusFactor.ipynb) : Factors operating directly on rotation matrix entries using the Frobenius norm, an alternative to Lie algebra-based factors. +- [ReferenceFrameFactor](doc/ReferenceFrameFactor.ipynb) : Factor relating the same landmark observed in two different coordinate frames via an unknown transformation, useful for map merging. +- [BoundingConstraint](doc/BoundingConstraint.ipynb) : Abstract base class for creating inequality constraints (e.g., keeping a variable within certain bounds). Requires C++ derivation. +- [AntiFactor](doc/AntiFactor.ipynb) : A factor designed to negate the effect of another factor, useful for dynamically removing constraints. + +## Initialization & Utilities + +Helper functions and classes for SLAM tasks. + +- [lago](doc/lago.ipynb) : Linear Approximation for Graph Optimization (LAGO) for initializing `Pose2` graphs. +- [InitializePose3](doc/InitializePose3.ipynb) : Methods for initializing `Pose3` graphs by first solving for rotations, then translations. +- [dataset](doc/dataset.ipynb) : Utility functions for loading/saving common SLAM dataset formats (g2o, TORO). +- [expressions](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/expressions.h) : Pre-defined Expression trees for common SLAM factor types (internal use for Expression-based factors). \ No newline at end of file diff --git a/myst.yml b/myst.yml index 5fbe34010..f0b62c88f 100644 --- a/myst.yml +++ b/myst.yml @@ -25,6 +25,9 @@ project: - file: ./gtsam/navigation/navigation.md children: - pattern: ./gtsam/navigation/doc/* + - file: ./gtsam/slam/slam.md + children: + - pattern: ./gtsam/slam/doc/* - file: ./doc/examples.md children: - pattern: ./python/gtsam/examples/*.ipynb From 8fbe3c9c8cdd4e52133cee0fab40cefe81de3a48 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 17:24:43 -0400 Subject: [PATCH 04/17] Fixed KarcherMean issue --- gtsam/linear/linear.i | 2 +- gtsam/slam/KarcherMeanFactor.h | 10 ++- gtsam/slam/doc/KarcherMeanFactor.ipynb | 91 +++++++++++--------------- gtsam/slam/slam.i | 5 +- 4 files changed, 49 insertions(+), 59 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index de55a5bb4..01a28511c 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -355,7 +355,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { gtsam::Vector error_vector(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const; - //Standard Interface + // Standard Interface gtsam::Matrix getA() const; gtsam::Vector getb() const; size_t rows() const; diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 6b25dd261..d81d15faa 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -27,13 +27,17 @@ namespace gtsam { /** * Optimize for the Karcher mean, minimizing the geodesic distance to each of - * the given rotations, by constructing a factor graph out of simple + * the given Lie groups elements, by constructing a factor graph out of simple * PriorFactors. */ template -T FindKarcherMean(const std::vector> &rotations); +typename std::enable_if::IsLieGroup, T>::type +FindKarcherMean(const std::vector> &elements); -template T FindKarcherMean(std::initializer_list &&rotations); +/// FindKarcherMean version from initializer list +template +typename std::enable_if::IsLieGroup, T>::type +FindKarcherMean(std::initializer_list &&elements); /** * The KarcherMeanFactor creates a constraint on all SO(n) variables with diff --git a/gtsam/slam/doc/KarcherMeanFactor.ipynb b/gtsam/slam/doc/KarcherMeanFactor.ipynb index 973987286..64cd85506 100644 --- a/gtsam/slam/doc/KarcherMeanFactor.ipynb +++ b/gtsam/slam/doc/KarcherMeanFactor.ipynb @@ -15,12 +15,12 @@ "id": "desc_md" }, "source": [ - "This header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", + "The [KarcherMeanFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/KarcherMeanFactor.h) header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", "The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n", "$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n", "\n", "Functions/Classes:\n", - "* `FindKarcherMean(rotations)`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", + "* `FindKarcherMean`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", "* `KarcherMeanFactor`: A factor that enforces a constraint related to the Karcher mean. It does *not* constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the *sum of tangent space updates* applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables." ] }, @@ -35,7 +35,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -44,7 +44,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" ] }, { @@ -57,10 +61,8 @@ "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import Rot3, FindKarcherMean, KarcherMeanFactorRot3, NonlinearFactorGraph, Values\n", - "from gtsam import symbol_shorthand\n", - "\n", - "R = symbol_shorthand.R" + "from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n", + "from gtsam.symbol_shorthand import R" ] }, { @@ -111,7 +113,7 @@ "rotations.append(Rot3.Yaw(0.12))\n", "\n", "# Compute the Karcher mean\n", - "karcher_mean = FindKarcherMean(rotations)\n", + "karcher_mean = FindKarcherMeanRot3(rotations)\n", "\n", "print(\"Input Rotations (Yaw angles):\")\n", "for r in rotations: print(f\" {r.yaw():.3f}\")\n", @@ -141,7 +143,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 6, "metadata": { "id": "factor_example_code" }, @@ -151,36 +153,22 @@ "output_type": "stream", "text": [ "KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n", + "sqrt(beta): 10.0\n", "\n", - "Linearized Factor (JacobianFactor):\n", - " A[r0] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " A[r1] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " A[r2] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " b = [ 0 0 0 ]\n", - " No noise model\n" - ] - }, - { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.JacobianFactor' object has no attribute 'find'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[9], line 20\u001b[0m\n\u001b[0;32m 18\u001b[0m sqrt_beta \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39msqrt(beta)\n\u001b[0;32m 19\u001b[0m expected_jacobian \u001b[38;5;241m=\u001b[39m sqrt_beta \u001b[38;5;241m*\u001b[39m np\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m3\u001b[39m)\n\u001b[1;32m---> 20\u001b[0m A0 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(\u001b[43mlinearized_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mfind\u001b[49m(R(\u001b[38;5;241m0\u001b[39m)))\n\u001b[0;32m 21\u001b[0m A1 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m1\u001b[39m)))\n\u001b[0;32m 22\u001b[0m A2 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m2\u001b[39m)))\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.JacobianFactor' object has no attribute 'find'" + "Jacobian for R(0):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Jacobian for R(1):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Jacobian for R(2):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Error vector b:\n", + "[0. 0. 0.]\n" ] } ], @@ -188,7 +176,7 @@ "keys = [R(0), R(1), R(2)]\n", "beta = 100.0 # Strength of the constraint\n", "\n", - "k_factor = KarcherMeanFactorRot3(keys)\n", + "k_factor = KarcherMeanFactorRot3(keys, 3, beta)\n", "k_factor.print(\"KarcherMeanFactorRot3: \")\n", "\n", "# Linearization example\n", @@ -198,32 +186,27 @@ "values.insert(R(2), Rot3.Yaw(0.3))\n", "\n", "linearized_factor = k_factor.linearize(values)\n", - "print(\"\\nLinearized Factor (JacobianFactor):\")\n", - "linearized_factor.print()\n", "\n", "# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n", "sqrt_beta = np.sqrt(beta)\n", - "expected_jacobian = sqrt_beta * np.eye(3)\n", - "A0 = linearized_factor.getA(linearized_factor.find(R(0)))\n", - "A1 = linearized_factor.getA(linearized_factor.find(R(1)))\n", - "A2 = linearized_factor.getA(linearized_factor.find(R(2)))\n", + "A = linearized_factor.getA()\n", + "assert A.shape == (3, 9), f\"Unexpected shape for A: {A.shape}\"\n", + "A0 = A[:, :3]\n", + "A1 = A[:, 3:6]\n", + "A2 = A[:, 6:9]\n", "b = linearized_factor.getb()\n", "\n", + "print(f\"sqrt(beta): {sqrt_beta}\")\n", "print(f\"\\nJacobian for R(0):\\n{A0}\")\n", "print(f\"Jacobian for R(1):\\n{A1}\")\n", "print(f\"Jacobian for R(2):\\n{A2}\")\n", - "print(f\"Error vector b:\\n{b}\")\n", - "print(f\"sqrt(beta): {sqrt_beta}\")\n", - "assert np.allclose(A0, expected_jacobian)\n", - "assert np.allclose(A1, expected_jacobian)\n", - "assert np.allclose(A2, expected_jacobian)\n", - "assert np.allclose(b, np.zeros(3))" + "print(f\"Error vector b:\\n{b}\")" ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -237,7 +220,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 d8a2e49bf..50f2f0071 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -396,9 +396,12 @@ template virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); + KarcherMeanFactor(const gtsam::KeyVector& keys, int d, double beta); }; -gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); +template +T FindKarcherMean(const std::vector& elements); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 3e13c448024ca5303b3f768d18ce38f8b22c9df0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 17:52:46 -0400 Subject: [PATCH 05/17] Fix LAGO docs, add test --- gtsam/slam/doc/lago.ipynb | 183 ++++++++++++++++++-------------- gtsam/slam/slam.i | 1 + python/gtsam/tests/test_lago.py | 28 ++++- 3 files changed, 131 insertions(+), 81 deletions(-) diff --git a/gtsam/slam/doc/lago.ipynb b/gtsam/slam/doc/lago.ipynb index aa6d0e98b..00f4c2b60 100644 --- a/gtsam/slam/doc/lago.ipynb +++ b/gtsam/slam/doc/lago.ipynb @@ -16,10 +16,10 @@ }, "source": [ "The `gtsam::lago` namespace provides functions for initializing `Pose2` estimates in a 2D SLAM factor graph.\n", - "LAGO stands for Linear Approximation for Graph Optimization. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", + "LAGO stands for **Linear Approximation for Graph Optimization**. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", "\n", "The core idea is:\n", - "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", + "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", "2. **Estimate Translations:** Given the estimated orientations, compute the translations by solving another linear system.\n", "\n", "Key functions:\n", @@ -27,7 +27,9 @@ "* `initialize(graph)`: Computes initial estimates for the full `Pose2` variables (orientations and translations).\n", "* `initialize(graph, initialGuess)`: Corrects only the orientation part of a given `initialGuess` using LAGO.\n", "\n", - "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose." + "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose.\n", + "\n", + "**Important Note**: LAGO expects integer keys numbered from 0 to n-1, with n the number of poses." ] }, { @@ -41,7 +43,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -50,7 +52,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" ] }, { @@ -65,11 +71,8 @@ "import gtsam.utils.plot as gtsam_plot\n", "import matplotlib.pyplot as plt\n", "import numpy as np\n", - "from gtsam import NonlinearFactorGraph, Values, Pose2, Point3, PriorFactorPose2, BetweenFactorPose2\n", - "from gtsam import lago\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X" + "from gtsam import NonlinearFactorGraph, Pose2, PriorFactorPose2, BetweenFactorPose2\n", + "from gtsam import lago" ] }, { @@ -92,7 +95,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 8, "metadata": { "id": "example_pipeline_code" }, @@ -101,51 +104,26 @@ "name": "stdout", "output_type": "stream", "text": [ - "Original Factor Graph:\n", - "size: 6\n", "\n", - "Factor 0: PriorFactor on x0\n", - " prior mean: (0, 0, 0)\n", - " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n", + "Initial Estimate from LAGO:\n", "\n", - "Factor 1: BetweenFactor(x0,x1)\n", - " measured: (2, 0, 0)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Values with 5 values:\n", + "Value 0: (gtsam::Pose2)\n", + "(-7.47244713e-17, -6.32592437e-17, -0.00193783525)\n", "\n", - "Factor 2: BetweenFactor(x1,x2)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Value 1: (gtsam::Pose2)\n", + "(1.70434147, -0.00881225307, 0.034656973)\n", "\n", - "Factor 3: BetweenFactor(x2,x3)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ + "Value 2: (gtsam::Pose2)\n", + "(3.40930145, 0.0555625509, 1.64569894)\n", "\n", - "Factor 4: BetweenFactor(x3,x4)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Value 3: (gtsam::Pose2)\n", + "(2.9638596, 2.05327873, 3.10897006)\n", "\n", - "Factor 5: BetweenFactor(x4,x0)\n", - " measured: (2.1, 0.1, 1.62079633)\n", - " noise model: diagonal sigmas [0.25; 0.25; 0.15];\n", + "Value 4: (gtsam::Pose2)\n", + "(0.669190885, 2.11357777, -1.71695927)\n", "\n" ] - }, - { - "ename": "IndexError", - "evalue": "invalid map key", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mIndexError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 25\u001b[0m\n\u001b[0;32m 22\u001b[0m graph\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOriginal Factor Graph:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;66;03m# 2. Perform LAGO initialization\u001b[39;00m\n\u001b[1;32m---> 25\u001b[0m initial_estimate_lago \u001b[38;5;241m=\u001b[39m \u001b[43mlago\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minitialize\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mInitial Estimate from LAGO:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 28\u001b[0m initial_estimate_lago\u001b[38;5;241m.\u001b[39mprint()\n", - "\u001b[1;31mIndexError\u001b[0m: invalid map key" - ] } ], "source": [ @@ -155,72 +133,117 @@ "# Add a prior on the first pose\n", "prior_mean = Pose2(0.0, 0.0, 0.0)\n", "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))\n", - "graph.add(PriorFactorPose2(X(0), prior_mean, prior_noise))\n", + "graph.add(PriorFactorPose2(0, prior_mean, prior_noise))\n", "\n", "# Add odometry factors (simulating moving in a square)\n", "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", - "graph.add(BetweenFactorPose2(X(0), X(1), Pose2(2.0, 0.0, 0.0), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(1), X(2), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(2), X(3), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(3), X(4), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise))\n", + "graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", "\n", "# Add a loop closure factor\n", "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))\n", "# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)\n", "measured_loop = Pose2(2.1, 0.1, np.pi/2 + 0.05)\n", - "graph.add(BetweenFactorPose2(X(4), X(0), measured_loop, loop_noise))\n", - "\n", - "graph.print(\"Original Factor Graph:\\n\")\n", + "graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise))\n", "\n", "# 2. Perform LAGO initialization\n", "initial_estimate_lago = lago.initialize(graph)\n", "\n", "print(\"\\nInitial Estimate from LAGO:\\n\")\n", - "initial_estimate_lago.print()\n", - "\n", - "# 3. Visualize the LAGO estimate (optional)\n", + "initial_estimate_lago.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The block below visualizes the initial estimate computed by the LAGO algorithm. It uses the `gtsam_plot.plot_pose2` function to plot the poses in 2D space. Each pose is represented with its position and orientation, providing an intuitive way to inspect the initialization results. The visualization helps verify the correctness of the initial guess before proceeding with further optimization." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ "plt.figure(1)\n", "for key in initial_estimate_lago.keys():\n", " gtsam_plot.plot_pose2(1, initial_estimate_lago.atPose2(key), 0.5)\n", "plt.title(\"LAGO Initial Estimate\")\n", "plt.axis('equal')\n", - "# plt.show()\n", - "plt.close() # Close plot to prevent display in output\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, let's look at `lago.initializeOrientations` to compute the initial orientation estimatesThis solves a linear system, and the solution is represented as a `VectorValues` object, which stores the estimated angles for each pose as a 1-d vector.\n", "\n", - "# 4. Compare orientation initialization\n", + "We compare these orientation estimates with the orientations extracted from the full LAGO initialization (`lago.initialize`)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "LAGO Orientations (VectorValues):\n", + "VectorValues: 6 elements\n", + " 0: -1.11022302e-16\n", + " 1: -0.008\n", + " 2: 1.55479633\n", + " 3: 3.11759265\n", + " 4: 4.68038898\n", + " 99999999: 0\n", + "Orientations from full LAGO Values:\n", + " 0: -0.0019\n", + " 1: 0.0347\n", + " 2: 1.6457\n", + " 3: 3.1090\n", + " 4: -1.7170\n" + ] + } + ], + "source": [ "initial_orientations = lago.initializeOrientations(graph)\n", "print(\"\\nLAGO Orientations (VectorValues):\")\n", "initial_orientations.print()\n", "\n", "print(\"Orientations from full LAGO Values:\")\n", "for i in range(5):\n", - " print(f\" X({i}): {initial_estimate_lago.atPose2(X(i)).theta():.4f}\")" + " print(f\" {i}: {initial_estimate_lago.atPose2(i).theta():.4f}\")" ] }, { "cell_type": "markdown", - "metadata": { - "id": "notes_header_md" - }, + "metadata": {}, "source": [ - "## Notes" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "notes_desc_md" - }, - "source": [ - "- LAGO provides a good initial guess, especially for orientations, which can significantly help the convergence of nonlinear optimizers.\n", - "- It assumes the graph structure allows for the orientation estimation (typically requires a spanning tree and a prior).\n", - "- The translation estimates are computed based on the fixed, estimated orientations." + "These are not as accurate (the last one is actually fine, it's $2\\pi$ off) but will still be good enough as an initial estimate." ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -234,7 +257,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 50f2f0071..3fdc31b30 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -467,6 +467,7 @@ typedef gtsam::TriangulationFactor> namespace lago { gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); + gtsam::VectorValues initializeOrientations(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); } } // namespace gtsam diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py index 8ed5dd943..e538d8984 100644 --- a/python/gtsam/tests/test_lago.py +++ b/python/gtsam/tests/test_lago.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Point3, Pose2, PriorFactorPose2, Values +from gtsam import BetweenFactorPose2, Point3, Pose2, PriorFactorPose2, Values class TestLago(unittest.TestCase): @@ -33,6 +33,32 @@ class TestLago(unittest.TestCase): estimateLago: Values = gtsam.lago.initialize(graph) assert isinstance(estimateLago, Values) + def test_initialize2(self) -> None: + """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" + # 1. Create a NonlinearFactorGraph with Pose2 factors + graph = gtsam.NonlinearFactorGraph() + + # Add a prior on the first pose + prior_mean = Pose2(0.0, 0.0, 0.0) + prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + graph.add(PriorFactorPose2(0, prior_mean, prior_noise)) + + # Add odometry factors (simulating moving in a square) + odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise)) + graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + + # Add a loop closure factor + loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15])) + # Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2) + measured_loop = Pose2(2.1, 0.1, np.pi / 2 + 0.05) + graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise)) + + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) + if __name__ == "__main__": unittest.main() From fa23a888f957c82e012d168b2295de7f80a4736e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 18:02:40 -0400 Subject: [PATCH 06/17] Fixed PlanarProjectionFactor --- gtsam/slam/PlanarProjectionFactor.h | 2 +- gtsam/slam/doc/PlanarProjectionFactor.ipynb | 89 +++++++++------------ 2 files changed, 41 insertions(+), 50 deletions(-) diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index d53cd0ae1..dc7e53677 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -201,7 +201,7 @@ namespace gtsam { const Cal3DS2& calib, const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), - NoiseModelFactorN(model, landmarkKey, poseKey), + NoiseModelFactorN(model, poseKey, landmarkKey), bTc_(bTc), calib_(calib) {} diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb index 55d76252d..3100fbb53 100644 --- a/gtsam/slam/doc/PlanarProjectionFactor.ipynb +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -15,9 +15,9 @@ "id": "desc_md" }, "source": [ - "The `PlanarProjectionFactor` variants provide camera projection constraints specifically designed for scenarios where the robot or camera moves primarily on a 2D plane (e.g., ground robots with cameras).\n", + "The `PlanarProjectionFactor` variants provide camera projection factors specifically designed for scenarios where **the robot or camera moves primarily on a 2D plane** (e.g., ground robots with cameras).\n", "They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n", - "* The robot's 2D pose (`Pose2` `wTb`: world-to-body) in the ground plane.\n", + "* The robot's 2D pose (`Pose2` `wTb`: body in world frame) in the ground plane.\n", "* The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n", "* The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n", "* The 3D landmark position in the world frame.\n", @@ -41,7 +41,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -50,12 +50,16 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, @@ -65,13 +69,7 @@ "import numpy as np\n", "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n", " PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L\n", - "C = symbol_shorthand.C # Calibration\n", - "O = symbol_shorthand.O # Offset" + "from gtsam.symbol_shorthand import X, L, C, O" ] }, { @@ -110,19 +108,19 @@ "isotropic dim=2 sigma=1.5\n", "\n", "Error at ground truth: 0.0\n", - "Error at noisy pose: 3317.647263749095\n" + "Error at noisy pose: 3317.6472637491106\n" ] } ], "source": [ "# Known parameters\n", "landmark_pt = Point3(2.0, 0.5, 0.5)\n", - "body_T_cam = Pose3(Rot3.Yaw(-np.pi/2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", + "body_T_cam = Pose3(Rot3.Yaw(-np.pi / 2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", "calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)\n", - "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", + "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", "\n", "# Assume ground truth pose and calculate expected measurement\n", - "gt_pose2 = Pose2(1.0, 0.0, np.pi/4)\n", + "gt_pose2 = Pose2(1.0, 0.0, np.pi / 4)\n", "gt_world_T_cam = Pose3(gt_pose2) * body_T_cam\n", "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n", "measured_pt2 = gt_camera.project(landmark_pt)\n", @@ -130,18 +128,19 @@ "print(f\"Calculated Measurement: {measured_pt2}\")\n", "\n", "# Create the factor\n", - "pose_key = X(0)\n", - "factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor1 = PlanarProjectionFactor1(\n", + " X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n", + ")\n", "factor1.print(\"Factor 1: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2) # Error should be zero here\n", + "values.insert(X(0), gt_pose2) # Error should be zero here\n", "error1_gt = factor1.error(values)\n", "print(f\"\\nError at ground truth: {error1_gt}\")\n", "\n", - "noisy_pose2 = Pose2(1.05, 0.02, np.pi/4 + 0.05)\n", - "values.update(pose_key, noisy_pose2)\n", + "noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n", + "values.update(X(0), noisy_pose2)\n", "error1_noisy = factor1.error(values)\n", "print(f\"Error at noisy pose: {error1_noisy}\")" ] @@ -175,37 +174,29 @@ "name": "stdout", "output_type": "stream", "text": [ - "Factor 2: keys = { l0 x0 }\n", - "isotropic dim=2 sigma=1.5\n" - ] - }, - { - "ename": "RuntimeError", - "evalue": "Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 10\u001b[0m\n\u001b[0;32m 8\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose2)\n\u001b[0;32m 9\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(landmark_key, landmark_pt) \u001b[38;5;66;03m# Error should be zero\u001b[39;00m\n\u001b[1;32m---> 10\u001b[0m error2_gt \u001b[38;5;241m=\u001b[39m \u001b[43mfactor2\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror2_gt\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 13\u001b[0m noisy_landmark \u001b[38;5;241m=\u001b[39m Point3(\u001b[38;5;241m2.1\u001b[39m, \u001b[38;5;241m0.45\u001b[39m, \u001b[38;5;241m0.55\u001b[39m)\n", - "\u001b[1;31mRuntimeError\u001b[0m: Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix" + "Factor 2: keys = { x0 l0 }\n", + "isotropic dim=2 sigma=1.5\n", + "\n", + "Error at ground truth: 0.0\n", + "Error with noisy landmark: 8066.192649473802\n" ] } ], "source": [ - "landmark_key = L(0)\n", - "\n", - "factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor2 = PlanarProjectionFactor2(\n", + " X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n", + ")\n", "factor2.print(\"Factor 2: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2)\n", - "values.insert(landmark_key, landmark_pt) # Error should be zero\n", + "values.insert(X(0), gt_pose2)\n", + "values.insert(L(0), landmark_pt) # Error should be zero\n", "error2_gt = factor2.error(values)\n", "print(f\"\\nError at ground truth: {error2_gt}\")\n", "\n", "noisy_landmark = Point3(2.1, 0.45, 0.55)\n", - "values.update(landmark_key, noisy_landmark)\n", + "values.update(L(0), noisy_landmark)\n", "error2_noisy = factor2.error(values)\n", "print(f\"Error with noisy landmark: {error2_noisy}\")" ] @@ -230,7 +221,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, "metadata": { "id": "factor3_example_code" }, @@ -239,11 +230,11 @@ "name": "stdout", "output_type": "stream", "text": [ - "Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", - "Noise model: diagonal sigmas [1.5; 1.5];\n", + "Factor 3: keys = { x0 o0 c0 }\n", + "isotropic dim=2 sigma=1.5\n", "\n", - "Error at ground truth: [-0. -0.]\n", - "Error with noisy calibration: [8.38867847 0.63659684]\n" + "Error at ground truth: 0.0\n", + "Error with noisy calibration: 92.30212176019934\n" ] } ], @@ -251,12 +242,12 @@ "offset_key = O(0)\n", "calib_key = C(0)\n", "\n", - "factor3 = PlanarProjectionFactor3(pose_key, offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", + "factor3 = PlanarProjectionFactor3(X(0), offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", "factor3.print(\"Factor 3: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2)\n", + "values.insert(X(0), gt_pose2)\n", "values.insert(offset_key, body_T_cam)\n", "values.insert(calib_key, calib) # Error should be zero\n", "error3_gt = factor3.error(values)\n", @@ -271,7 +262,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -285,7 +276,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, From 25a2deb952de1c8afa4e4a9727306005ef753dad Mon Sep 17 00:00:00 2001 From: p-zach Date: Sat, 26 Apr 2025 20:14:48 -0400 Subject: [PATCH 07/17] Wrap OrientedPlane3 --- gtsam/geometry/geometry.i | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 3520ab140..b5f6f1f3f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -694,6 +694,45 @@ class Unit3 { bool equals(const gtsam::Unit3& expected, double tol) const; }; +#include +class OrientedPlane3 { + // Standard constructors + OrientedPlane3(); + OrientedPlane3(const gtsam::Unit3& n, double d); + OrientedPlane3(const gtsam::Vector& vec); + OrientedPlane3(double a, double b, double c, double d); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::OrientedPlane3& s, double tol = 1e-9) const; + + gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr) const; + gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr, + Eigen::Ref Hp, + Eigen::Ref Hr) const; + + gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other) const; + gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other, + Eigen::Ref H1, + Eigen::Ref H2) const; + + static size_t Dim(); + size_t dim() const; + + gtsam::OrientedPlane3 retract(const gtsam::Vector3& v) const; + gtsam::OrientedPlane3 retract(const gtsam::Vector3& v, + Eigen::Ref H) const; + + gtsam::Vector3 localCoordinates(const gtsam::OrientedPlane3& s) const; + + gtsam::Vector planeCoefficients() const; + + gtsam::Unit3 normal() const; + gtsam::Unit3 normal(Eigen::Ref H) const; + double distance() const; + double distance(Eigen::Ref H) const; +}; + #include class EssentialMatrix { // Standard Constructors From 43addb5ee91cdcfdd8d8cc97488c25a0311abf3d Mon Sep 17 00:00:00 2001 From: p-zach Date: Sat, 26 Apr 2025 20:15:54 -0400 Subject: [PATCH 08/17] Wrap many SLAM classes --- gtsam/slam/slam.i | 178 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 176 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d8a2e49bf..89c9943ce 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -162,12 +162,79 @@ enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; class SmartProjectionParams { SmartProjectionParams(); + SmartProjectionParams(gtsam::LinearizationMode linMode = gtsam::LinearizationMode::HESSIAN, + gtsam::DegeneracyMode degMode = gtsam::DegeneracyMode::IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false, double retriangulationTh = 1e-5); + void setLinearizationMode(gtsam::LinearizationMode linMode); void setDegeneracyMode(gtsam::DegeneracyMode degMode); void setRankTolerance(double rankTol); void setEnableEPI(bool enableEPI); void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); + + void print(const std::string& str = "") const; +}; + +template }> +class SmartProjectionFactor : gtsam::NonlinearFactor { + SmartProjectionFactor(); + + SmartProjectionFactor( + const gtsam::noiseModel::Base* sharedNoiseModel, + const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); + + void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; + + bool decideIfTriangulate(const gtsam::CameraSet& cameras) const; + gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet& cameras) const; + bool triangulateForLinearize(const gtsam::CameraSet& cameras) const; + + gtsam::HessianFactor createHessianFactor( + const gtsam::CameraSet& cameras, const double lambda = 0.0, + bool diagonalDamping = false) const; + gtsam::JacobianFactor createJacobianQFactor( + const gtsam::CameraSet& cameras, double lambda) const; + gtsam::JacobianFactor createJacobianQFactor( + const gtsam::Values& values, double lambda) const; + gtsam::JacobianFactor createJacobianSVDFactor( + const gtsam::CameraSet& cameras, double lambda) const; + gtsam::HessianFactor linearizeToHessian( + const gtsam::Values& values, double lambda = 0.0) const; + gtsam::JacobianFactor linearizeToJacobian( + const gtsam::Values& values, double lambda = 0.0) const; + + gtsam::GaussianFactor linearizeDamped(const gtsam::CameraSet& cameras, + const double lambda = 0.0) const; + + gtsam::GaussianFactor linearizeDamped(const gtsam::Values& values, + const double lambda = 0.0) const; + + gtsam::GaussianFactor linearize( + const gtsam::Values& values) const; + + bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::CameraSet& cameras) const; + + bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::Values& values) const; + + gtsam::Vector reprojectionErrorAfterTriangulation(const gtsam::Values& values) const; + + double totalReprojectionError(const gtsam::CameraSet& cameras, + gtsam::Point3 externalPoint) const; + + double error(const gtsam::Values& values) const; + + gtsam::TriangulationResult point() const; + + gtsam::TriangulationResult point(const gtsam::Values& values) const; + + bool isValid() const; + bool isDegenerate() const; + bool isPointBehindCamera() const; + bool isOutlier() const; + bool isFarPoint() const; }; #include @@ -198,6 +265,42 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +// WIP +// #include +// typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +// #include +// template +// class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { +// SmartProjectionRigFactor(); + +// SmartProjectionRigFactor( +// const gtsam::noiseModel::Base* sharedNoiseModel, +// const gtsam::CameraSet& cameraRig, +// const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); + +// void add(const CAMERA::Measurement& measured, const gtsam::Key& poseKey, +// const size_t& cameraId = 0); + +// void add(const CAMERA::MeasurementVector& measurements, const gtsam::KeyVector& poseKeys, +// const gtsam::FastVector& cameraIds = gtsam::FastVector()); + +// const gtsam::KeyVector& nonUniqueKeys() const; + +// const gtsam::CameraSet& cameraRig() const; + +// const gtsam::FastVector& cameraIds() const; + +// void print( +// const std::string& s = "", +// const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + +// bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; + +// gtsam::CameraSet cameras(const gtsam::Values& values) const; + +// double error(const gtsam::Values& values) const; +// }; + #include template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { @@ -227,11 +330,72 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor { typedef gtsam::GenericStereoFactor GenericStereoFactor3D; +#include +template +class ReferenceFrameFactor : gtsam::NoiseModelFactor { + ReferenceFrameFactor(gtsam::Key globalKey, gtsam::Key transKey, + gtsam::Key localKey, const gtsam::noiseModel::Base* model); + + gtsam::Vector evaluateError(const LANDMARK& global, const POSE& trans, const LANDMARK& local); + + void print(const std::string& s="", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +}; + +#include +class RotateFactor : gtsam::NoiseModelFactor { + RotateFactor(gtsam::Key key, const gtsam::Rot3& P, const gtsam::Rot3& Z, + const gtsam::noiseModel::Base* model); + + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + gtsam::Vector evaluateError(const gtsam::Rot3& R) const; +}; +class RotateDirectionsFactor : gtsam::NoiseModelFactor { + RotateDirectionsFactor(gtsam::Key key, const gtsam::Unit3& i_p, const gtsam::Unit3& c_z, + const gtsam::noiseModel::Base* model); + + static gtsam::Rot3 Initialize(const gtsam::Unit3& i_p, const gtsam::Unit3& c_z); + + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + gtsam::Vector evaluateError(const gtsam::Rot3& iRc) const; +}; + +#include +class OrientedPlane3Factor : gtsam::NoiseModelFactor { + OrientedPlane3Factor(); + OrientedPlane3Factor(const gtsam::Vector& z, const gtsam::noiseModel::Gaussian* noiseModel, + gtsam::Key poseKey, gtsam::Key landmarkKey); + + void print(const std::string& s = "OrientedPlane3Factor", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + gtsam::Vector evaluateError( + const gtsam::Pose3& pose, const gtsam::OrientedPlane3& plane) const; +}; +class OrientedPlane3DirectionPrior : gtsam::NoiseModelFactor { + OrientedPlane3DirectionPrior(); + OrientedPlane3DirectionPrior(gtsam::Key key, const gtsam::Vector& z, + const gtsam::noiseModel::Gaussian* noiseModel); + + void print(const std::string& s = "OrientedPlane3DirectionPrior", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const; + + gtsam::Vector evaluateError(const gtsam::OrientedPlane3& plane) const; +}; + #include template virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE::Translation& measured, + const gtsam::noiseModel::Base* model); PoseTranslationPrior(size_t key, const POSE& pose_z, - const gtsam::noiseModel::Base* noiseModel); + const gtsam::noiseModel::Base* model); POSE::Translation measured() const; // enabling serialization functionality @@ -244,8 +408,10 @@ typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; #include template virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE::Rotation& rot_z, + const gtsam::noiseModel::Base* model); PoseRotationPrior(size_t key, const POSE& pose_z, - const gtsam::noiseModel::Base* noiseModel); + const gtsam::noiseModel::Base* model); POSE::Rotation measured() const; }; @@ -404,6 +570,14 @@ gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, size_t d); +template +class FrobeniusPrior : gtsam::NoiseModelFactor { + FrobeniusPrior(gtsam::Key j, const gtsam::Matrix& M, + const gtsam::noiseModel::Base* model); + + gtsam::Vector evaluateError(const T& g) const; +}; + template virtual class FrobeniusFactor : gtsam::NoiseModelFactor { FrobeniusFactor(size_t key1, size_t key2); From 57dbbfd9e316694eef57c92714c159c5e34fcab7 Mon Sep 17 00:00:00 2001 From: p-zach Date: Sat, 26 Apr 2025 20:17:53 -0400 Subject: [PATCH 09/17] Fix SLAM notebook wrap problems and typos --- gtsam/slam/doc/FrobeniusFactor.ipynb | 54 +++--- gtsam/slam/doc/OrientedPlane3Factor.ipynb | 71 ++++---- gtsam/slam/doc/PoseRotationPrior.ipynb | 43 ++--- gtsam/slam/doc/PoseTranslationPrior.ipynb | 44 ++--- gtsam/slam/doc/ReferenceFrameFactor.ipynb | 18 ++ gtsam/slam/doc/RotateFactor.ipynb | 54 +++--- gtsam/slam/doc/SmartProjectionFactor.ipynb | 72 +++----- ...rams.ipynb => SmartProjectionParams.ipynb} | 72 ++++---- .../slam/doc/SmartProjectionPoseFactor.ipynb | 159 +++++++----------- gtsam/slam/doc/StereoFactor.ipynb | 74 ++++---- 10 files changed, 298 insertions(+), 363 deletions(-) rename gtsam/slam/doc/{SmartFactorParams.ipynb => SmartProjectionParams.ipynb} (57%) diff --git a/gtsam/slam/doc/FrobeniusFactor.ipynb b/gtsam/slam/doc/FrobeniusFactor.ipynb index cba959dc6..80d30c9ed 100644 --- a/gtsam/slam/doc/FrobeniusFactor.ipynb +++ b/gtsam/slam/doc/FrobeniusFactor.ipynb @@ -50,7 +50,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "imports_code" }, @@ -86,7 +86,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "fprior_example_code" }, @@ -95,12 +95,10 @@ "name": "stdout", "output_type": "stream", "text": [ - "FrobeniusPriorRot3: FrobeniusPriorFactor on 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", + "FrobeniusPriorRot3: keys = { r0 }\n", + " noise model: unit (9) \n", "\n", - "FrobeniusPrior error (vectorized matrix diff): (9,)\n", - "[ 0.00054931 -0.00997917 0. -0.00997917 -0.00054931 0.\n", - " 0. 0. 0. ]\n" + "FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05\n" ] } ], @@ -121,7 +119,7 @@ "values = Values()\n", "values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\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", - "execution_count": 2, + "execution_count": 7, "metadata": { "id": "ffactor_example_code" }, @@ -155,18 +153,9 @@ "text": [ "\n", "FrobeniusFactorRot3: keys = { r0 r1 }\n", - " noise model: unit (9) \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." + " noise model: unit (9) \n", + "\n", + "FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05\n" ] } ], @@ -182,9 +171,10 @@ "factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n", "\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", "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", - "execution_count": 3, + "execution_count": 9, "metadata": { "id": "fbetween_example_code" }, @@ -217,17 +207,15 @@ "output_type": "stream", "text": [ "\n", - "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor(R0,R1)\n", - " T12: R: [\n", - "\t0.999875, -0.0149991, 0;\n", - "\t0.0149991, 0.999875, 0;\n", + "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor(r0,r1)\n", + " T12: [\n", + "\t0.999988, -0.00499998, 0;\n", + "\t0.00499998, 0.999988, 0;\n", "\t0, 0, 1\n", "]\n", + " noise model: unit (9) \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", - "\n", - "FrobeniusBetweenFactor error: (9,)\n", - "[-0. 0. 0. 0. -0. 0. 0. 0. 0.]\n" + "FrobeniusBetweenFactor error: 1.925929944387236e-34\n" ] } ], @@ -241,7 +229,7 @@ "\n", "# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\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", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.11.9" + "version": "3.13.1" } }, "nbformat": 4, diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb index 1600674b2..7815b844f 100644 --- a/gtsam/slam/doc/OrientedPlane3Factor.ipynb +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -48,32 +48,17 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, - "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)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", "from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n", "from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "P = symbol_shorthand.P # For Plane" + "from gtsam.symbol_shorthand import X, P" ] }, { @@ -97,7 +82,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "factor_example_code" }, @@ -106,13 +91,21 @@ "name": "stdout", "output_type": "stream", "text": [ - "OrientedPlane3Factor: OrientedPlane3Factor Factor with keys: x0 P0\n", - "\n", - "Error model: diagonal sigmas [0.05; 0.05; 0.05];\n", - "\n", - "Error at ground truth: [-0. -0. -0.]\n", - "\n", - "Error with noisy plane: [ -0.0150837 -0.00503354 -49.83283361]\n" + "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" ] } ], @@ -126,7 +119,7 @@ "# Measurement: transform the world plane into the camera frame\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", - "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", "\n", "pose_key = X(0)\n", @@ -170,7 +163,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 6, "metadata": { "id": "prior_example_code" }, @@ -179,11 +172,21 @@ "name": "stdout", "output_type": "stream", "text": [ - "OrientedPlane3DirectionPrior: Factor OrientedPlane3DirectionPrior on P0\n", - "Noise model: diagonal sigmas [0.02; 0.02; 0.02];\n", - "\n", - "Error for prior on noisy_plane: [-0.5 -0.5 0.5]\n", - "Error for prior on gt_plane_world: [0. 0. 0.]\n" + "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." ] } ], @@ -192,7 +195,7 @@ "measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n", "direction_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.02)\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", "\n", "# Evaluate error using the 'noisy_plane' from the previous step\n", diff --git a/gtsam/slam/doc/PoseRotationPrior.ipynb b/gtsam/slam/doc/PoseRotationPrior.ipynb index 444658711..097702a8f 100644 --- a/gtsam/slam/doc/PoseRotationPrior.ipynb +++ b/gtsam/slam/doc/PoseRotationPrior.ipynb @@ -49,27 +49,15 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "imports_code" }, - "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)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\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", "\n", "X = symbol_shorthand.X" @@ -95,7 +83,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "create_example_code" }, @@ -104,14 +92,13 @@ "name": "stdout", "output_type": "stream", "text": [ - "PoseRotationPrior: PoseRotationPriorFactor on x0\n", - "Noise model: diagonal sigmas [0.05; 0.05; 0.05];\n", - "Measured Rotation R: [\n", + "PoseRotationPrior: PoseRotationPrior keys = { x0 }\n", + "isotropic dim=3 sigma=0.05\n", + "Measured Rotation [\n", "\t0.707107, -0.707107, 0;\n", "\t0.707107, 0.707107, 0;\n", "\t0, 0, 1\n", - "]\n", - "\n" + "]\n" ] } ], @@ -122,13 +109,13 @@ "# Noise model on rotation (3 dimensions for Rot3)\n", "rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n", "\n", - "# Factor type includes the Pose type, e.g. PoseRotationPriorPose3\n", - "factor = PoseRotationPriorPose3(pose_key, measured_rotation, rotation_noise)\n", + "# Factor type includes the Pose type, e.g. PoseRotationPrior3D\n", + "factor = PoseRotationPrior3D(pose_key, measured_rotation, rotation_noise)\n", "factor.print(\"PoseRotationPrior: \")\n", "\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", - "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" ] }, @@ -152,7 +139,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 3, "metadata": { "id": "eval_example_code" }, @@ -161,9 +148,9 @@ "name": "stdout", "output_type": "stream", "text": [ - "Error with correct rotation: [0. 0. 0.] (Should be near zero)\n", - "Error with incorrect rotation: [-0. -0. 2.00000004] (Should be non-zero)\n", - "Error with different translation: [-0. -0. 2.00000004] (Should be same as error2)\n" + "Error with correct rotation: 0.0 (Should be near zero)\n", + "Error with incorrect rotation: 1.9999999999999951 (Should be non-zero)\n", + "Error with different translation: 1.9999999999999951 (Should be same as error2)\n" ] } ], diff --git a/gtsam/slam/doc/PoseTranslationPrior.ipynb b/gtsam/slam/doc/PoseTranslationPrior.ipynb index a71c61c02..0c4e3a4cb 100644 --- a/gtsam/slam/doc/PoseTranslationPrior.ipynb +++ b/gtsam/slam/doc/PoseTranslationPrior.ipynb @@ -19,8 +19,6 @@ "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", "\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::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)." ] }, @@ -49,27 +47,15 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, - "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)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\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", "\n", "X = symbol_shorthand.X" @@ -95,7 +81,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "create_example_code" }, @@ -104,9 +90,13 @@ "name": "stdout", "output_type": "stream", "text": [ - "PoseTranslationPrior: Factor PoseTranslationPrior on x0\n", - "Noise model: diagonal sigmas [0.5; 0.5; 0.5];\n", - "Measured Translation10 20 5\n" + "PoseTranslationPrior: PoseTranslationPrior keys = { x0 }\n", + "isotropic dim=3 sigma=0.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", "\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", "\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", - "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" ] }, @@ -147,7 +137,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 5, "metadata": { "id": "eval_example_code" }, @@ -156,9 +146,9 @@ "name": "stdout", "output_type": "stream", "text": [ - "Error with correct translation: [0. 0. 0.] (Should be near zero)\n", - "Error with incorrect translation: [ 0.39999999 -0.19999999 0.19999999] (Should be non-zero)\n", - "Error with different rotation: [ 0.39999999 -0.19999999 0.19999999] (Should reflect Jacobian change)\n", + "Error with correct translation: 0.0 (Should be near zero)\n", + "Error with incorrect translation: 0.11999999999999986 (Should be non-zero)\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" ] } diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb index 13ca2adef..b60f9fa4d 100644 --- a/gtsam/slam/doc/ReferenceFrameFactor.ipynb +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -52,6 +52,24 @@ "%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", "execution_count": 1, diff --git a/gtsam/slam/doc/RotateFactor.ipynb b/gtsam/slam/doc/RotateFactor.ipynb index edb658d05..51d51ef3e 100644 --- a/gtsam/slam/doc/RotateFactor.ipynb +++ b/gtsam/slam/doc/RotateFactor.ipynb @@ -50,19 +50,7 @@ "metadata": { "id": "imports_code" }, - "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)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", @@ -93,7 +81,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "factor1_example_code" }, @@ -102,14 +90,14 @@ "name": "stdout", "output_type": "stream", "text": [ - "RotateFactor: Factor RotateFactor on R0\n", - "Noise model: diagonal sigmas [0.001; 0.001; 0.001];\n", + "RotateFactor: keys = { r0 }\n", + "isotropic dim=3 sigma=0.001\n", "RotateFactor:]\n", - "p: [0.01 0.02 0.03]\n", - "z: [ 0.03 -0.01 0.02]\n", + "p: 0.01 0.02 0.03\n", + "z: 0.03 -0.01 0.02\n", "\n", - "Error at ground truth R: [-0. -0. 0.]\n", - "Error at noisy R: [ 0.039987 0.00999887 -0.00998887]\n" + "Error at ground truth R: 700.0\n", + "Error at noisy R: 699.2869223608442\n" ] } ], @@ -158,7 +146,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 8, "metadata": { "id": "factor2_example_code" }, @@ -167,15 +155,23 @@ "name": "stdout", "output_type": "stream", "text": [ - "RotateDirectionsFactor: Factor RotateDirectionsFactor on R0\n", - "Noise model: diagonal sigmas [0.01; 0.01];\n", + "RotateDirectionsFactor: keys = { r0 }\n", + "isotropic dim=2 sigma=0.01\n", "RotateDirectionsFactor:\n", - "pUnit3: (1, 0, 0)\n", - "zUnit3: (0, 1, 0)\n", + "p:1\n", + "0\n", + "0\n", + "z:0\n", + "1\n", + "0\n", "\n", - "Check: gt_R * z_body = [ 1.0000000e+00 -6.1232340e-17 0.0000000e+00]\n", - "Error at ground truth R: [-0. 0.]\n", - "Error at noisy R: [-0.99995 -0.99995]\n" + "Check: gt_R * z_body = \n", + ": 1\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", "# This corresponds to a -90 deg yaw\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", "# Evaluate error\n", "values_dir = Values()\n", diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index 8dd7606ff..291cb0a48 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -40,7 +40,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -49,28 +49,20 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, - "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)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", @@ -104,42 +96,20 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 3, "metadata": { "id": "create_example_code" }, "outputs": [ { - "name": "stdout", - "output_type": "stream", - "text": [ - "Smart factor involves 3 measurements.\n", - "SmartFactor: SmartProjectionFactor\n", - "linearizationMode: 0\n", - "triangulationParameters:\n", - "rankTolerance = 1e-09\n", - "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" + "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'" ] } ], @@ -179,7 +149,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 7, "metadata": { "id": "eval_example_code" }, @@ -189,9 +159,9 @@ "output_type": "stream", "text": [ "Triangulated point result:\n", - "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "Status.DEGENERATE\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", "# Triangulate first to see the implicit point\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", "if point_result.valid():\n", " # Calculate error\n", diff --git a/gtsam/slam/doc/SmartFactorParams.ipynb b/gtsam/slam/doc/SmartProjectionParams.ipynb similarity index 57% rename from gtsam/slam/doc/SmartFactorParams.ipynb rename to gtsam/slam/doc/SmartProjectionParams.ipynb index 8ff32bdc1..36e840cb8 100644 --- a/gtsam/slam/doc/SmartFactorParams.ipynb +++ b/gtsam/slam/doc/SmartProjectionParams.ipynb @@ -83,7 +83,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 9, "metadata": { "id": "create_example_code" }, @@ -92,18 +92,27 @@ "name": "stdout", "output_type": "stream", "text": [ - "Default Parameters:\n" - ] - }, - { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001", - "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 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", - "\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=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001" + "Default Parameters:\n", + "linearizationMode: 0\n", + " degeneracyMode: 0\n", + "rankTolerance = 1\n", + "enableEPI = 0\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "useLOST = 0\n", + "noise model\n", + "\n", + "\n", + "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", "# Custom parameters\n", "custom_params = SmartProjectionParams(\n", - " linearizationMode = LinearizationMode.JACOBIAN_Q,\n", - " degeneracyMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n", + " linMode = LinearizationMode.JACOBIAN_Q,\n", + " degMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n", " throwCheirality = False,\n", " verboseCheirality = True,\n", " retriangulationTh = 1e-3\n", ")\n", "# Modify triangulation parameters directly\n", - "custom_params.triangulation.rankTolerance = 1e-8\n", - "custom_params.triangulation.enableEPI = False\n", - "custom_params.triangulation.dynamicOutlierRejectionThreshold = 3.0 # Reject points with reproj error > 3*sigma\n", + "custom_params.setRankTolerance(1e-8)\n", + "custom_params.setEnableEPI(False)\n", + "custom_params.setDynamicOutlierRejectionThreshold(3.0) # Reject points with reproj error > 3*sigma\n", "\n", "print(\"\\nCustom Parameters:\")\n", - "custom_params.print()\n", - "\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}\")" + "custom_params.print()" ] }, { @@ -156,7 +159,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 10, "metadata": { "id": "usage_example_code" }, @@ -171,27 +174,28 @@ "linearizationMode: 2\n", "triangulationParameters:\n", "rankTolerance = 1e-08\n", - "enableEPI = false\n", + "enableEPI = 0\n", "landmarkDistanceThreshold = -1\n", - "dynamicOutlierRejectionThreshold = 3\n", - "\n", + "dynamicOutlierRejectionThreshold = 1\n", + "useLOST = 0\n", + "noise model\n", "\n", "result:\n", - "Degenerate\n", + "no point, status = 1\n", "\n", "SmartFactorBase, z = \n", - "Factor\n" + " keys = { }\n" ] } ], "source": [ - "from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n", + "from gtsam import SmartProjectionPose3Factor, Cal3_S2\n", "\n", "# Example: Using custom params with a smart factor\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", - "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", "smart_factor.print()" ] diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index be34d6b44..c8d7f3b2e 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -54,28 +54,17 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": { "id": "imports_code" }, - "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)" - ] - } - ], + "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, SmartProjectionPoseFactorCal3_S2,\n", + " SmartProjectionParams, SmartProjectionPose3Factor,\n", " Cal3_S2)\n", "from gtsam import symbol_shorthand\n", "import graphviz\n", @@ -104,7 +93,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 5, "metadata": { "id": "create_example_code" }, @@ -118,39 +107,45 @@ " SmartProjectionFactor\n", "linearizationMode: 0\n", "triangulationParameters:\n", - "rankTolerance = 1e-09\n", - "enableEPI = false\n", + "rankTolerance = 1\n", + "enableEPI = 0\n", "landmarkDistanceThreshold = -1\n", "dynamicOutlierRejectionThreshold = -1\n", - "\n", + "useLOST = 0\n", + "noise model\n", "\n", "result:\n", - "Degenerate\n", + "no point, status = 1\n", "\n", "SmartFactorBase, z = \n", "measurement 0, px = \n", - "[150; 505]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", + "150\n", + "505\n", + "noise model = unit (2) \n", "measurement 1, px = \n", - "[470; 495]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", + "470\n", + "495\n", + "noise model = unit (2) \n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ "measurement 2, px = \n", - "[480; 150]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "Factor on x0 x1 x2\n" + "480\n", + "150\n", + "noise model = unit (2) \n", + " keys = { x0 x1 x2 }\n" ] } ], "source": [ "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\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", - "# Factor type includes the Calibration type\n", - "smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n", + "smart_factor = SmartProjectionPose3Factor(smart_noise, K, smart_params)\n", "\n", "# Add measurements and keys (Pose keys)\n", "smart_factor.add(Point2(150, 505), X(0))\n", @@ -181,7 +176,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 16, "metadata": { "id": "eval_example_code" }, @@ -190,19 +185,19 @@ "name": "stdout", "output_type": "stream", "text": [ - "Triangulated point result:\n", - "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "Triangulated point result: Status.VALID\n", + "Triangulated point: [0.28416823 1.95555615 5.67688675]\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": [ "# Create Values containing Pose3 objects\n", "values = Values()\n", - "pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n", - "pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n", - "pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 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), Point3(1, 0, 0.5))\n", + "pose2 = Pose3(Rot3.Ypr(0, 0.0, 0.2), Point3(0, 1, 0.5))\n", "\n", "values.insert(X(0), pose0)\n", "values.insert(X(1), pose1)\n", @@ -210,9 +205,10 @@ "\n", "# Triangulate first to see the implicit point\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", "if point_result.valid():\n", + " print(f\"Triangulated point: {point_result.get()}\")\n", " # Calculate error\n", " total_error = smart_factor.error(values)\n", " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", @@ -240,7 +236,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "metadata": { "id": "linearize_example_code" }, @@ -251,61 +247,29 @@ "text": [ "\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", - "Augmented Diagonal Block [1,1]:\n", - "[ 768, -2084, 849; ]\n", - "[ -2084, 6054, -1412; ]\n", - "[ 849, -1412, 4392; ]\n", - "\n", - "Augmented Diagonal Block [2,2]:\n", - "[ 23563, 2137, 1132; ]\n", - "[ 2137, 15054, -1172; ]\n", - "[ 1132, -1172, 8384; ]\n", - "\n", - "Off-Diagonal Block [0,1]:\n", - "[ -5014, -4686, -552; ]\n", - "[ -9.522e+03, -1.623e+04, 447; ]\n", - "[ 3085, -3660, -1.118e+04; ]\n", - "\n", - "Off-Diagonal Block [0,2]:\n", - "[ 11515, 16951, 996; ]\n", - "[ 230, -2148, 7017; ]\n", - "[ -373, -620, -7830; ]\n", - "\n", - "Off-Diagonal Block [1,2]:\n", - "[ 6273, -830, -969; ]\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" + " keys: x0(6) x1(6) x2(6) \n", + "Augmented information matrix: [\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "\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", + "]\n" ] } ], @@ -319,8 +283,7 @@ "if linear_factor:\n", " print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n", " # Cast to HessianFactor to print its details\n", - " # Note: The factor dimension is Pose3::dimension (6)\n", - " hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n", + " hessian_factor = gtsam.HessianFactor(linear_factor)\n", " if hessian_factor:\n", " hessian_factor.print()\n", " else:\n", diff --git a/gtsam/slam/doc/StereoFactor.ipynb b/gtsam/slam/doc/StereoFactor.ipynb index 08c1b6ab4..65e3014db 100644 --- a/gtsam/slam/doc/StereoFactor.ipynb +++ b/gtsam/slam/doc/StereoFactor.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "%pip install --quiet gtsam" ] }, { @@ -66,6 +66,8 @@ "import gtsam\n", "import numpy as np\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", "from gtsam import GenericStereoFactor3D\n", "from gtsam import symbol_shorthand\n", @@ -101,7 +103,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": { "id": "create_example_code" }, @@ -170,20 +172,23 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 3, "metadata": { "id": "eval_example_code" }, "outputs": [ { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "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", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'" + "name": "stdout", + "output_type": "stream", + "text": [ + "Expected landmark point (no offset): [ 1.54225239 -2.27112649 2.95849821]\n", + "\n", + "Error (no offset) at expected landmark: 48664.883462255115 (Should be near zero)\n", + "\n", + "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", "# Example values\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", + "\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", - "\n", - "# Evaluate factor without offset\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", - "# Evaluate factor with offset\n", - "# Need to recompute landmark based on offset pose\n", - "pose_with_offset = pose * body_P_sensor # This is world_P_sensor\n", - "expected_point_offset_cam = K_stereo.backproject(measured_stereo)\n", - "landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam)\n", + "# --- Evaluate factor with offset --- \n", + "# Calculate the actual sensor pose in the world\n", + "pose_with_offset = pose.compose(body_P_sensor) # world_P_sensor = world_P_body * body_P_sensor\n", + "# Create a StereoCamera object at the sensor pose\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", "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", - "# Evaluate with noisy landmark\n", - "noisy_landmark = landmark + Point3(0.1, -0.05, 0.1)\n", + "# --- Evaluate with noisy landmark (using the no-offset factor for simplicity) ---\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", "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}\")" ] } ], From 502ff2762b6fd4d0b78601431995d7a4bc2734c8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 22:36:53 -0400 Subject: [PATCH 10/17] Add OrientedPlane3 to Values wrapper --- gtsam/nonlinear/values.i | 5 +++ gtsam/slam/doc/OrientedPlane3Factor.ipynb | 53 +++++++++-------------- 2 files changed, 26 insertions(+), 32 deletions(-) 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, From 513bbb43ed05eec5ee8621494f3faa166164a700 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 22:40:03 -0400 Subject: [PATCH 11/17] Fix FindKarcherMean tests --- python/gtsam/tests/test_KarcherMeanFactor.py | 6 +++--- python/gtsam/tests/test_backwards_compatibility.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index 0bc942341..c7154fd18 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -35,7 +35,7 @@ class TestKarcherMean(GtsamTestCase): """ rotations = [R, R.inverse()] expected = Rot3() - actual = gtsam.FindKarcherMean(rotations) + actual = gtsam.FindKarcherMeanRot3(rotations) self.gtsamAssertEquals(expected, actual) def test_find_karcher_mean_identity(self): @@ -47,7 +47,7 @@ class TestKarcherMean(GtsamTestCase): aRb_list = [a1Rb1, a2Rb2, a3Rb3] aRb_expected = Rot3() - aRb = gtsam.FindKarcherMean(aRb_list) + aRb = gtsam.FindKarcherMeanRot3(aRb_list) self.gtsamAssertEquals(aRb, aRb_expected) def test_factor(self): @@ -69,7 +69,7 @@ class TestKarcherMean(GtsamTestCase): expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)]) + actual = gtsam.FindKarcherMeanRot3([result.atRot3(1), result.atRot3(2)]) self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2))) diff --git a/python/gtsam/tests/test_backwards_compatibility.py b/python/gtsam/tests/test_backwards_compatibility.py index c64be37a7..c6c7eadca 100644 --- a/python/gtsam/tests/test_backwards_compatibility.py +++ b/python/gtsam/tests/test_backwards_compatibility.py @@ -315,7 +315,7 @@ class TestBackwardsCompatibility(GtsamTestCase): rotations = gtsam.Rot3Vector([R, R.inverse()]) expected = Rot3() - actual = gtsam.FindKarcherMean(rotations) + actual = gtsam.FindKarcherMeanRot3(rotations) self.gtsamAssertEquals(expected, actual) def test_find_karcher_mean_identity(self): @@ -327,7 +327,7 @@ class TestBackwardsCompatibility(GtsamTestCase): aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) aRb_expected = Rot3() - aRb = gtsam.FindKarcherMean(aRb_list) + aRb = gtsam.FindKarcherMeanRot3(aRb_list) self.gtsamAssertEquals(aRb, aRb_expected) def test_factor(self): @@ -354,7 +354,7 @@ class TestBackwardsCompatibility(GtsamTestCase): expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = gtsam.FindKarcherMean( + actual = gtsam.FindKarcherMeanRot3( gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)])) self.gtsamAssertEquals(expected, actual) From 47812cd89908f343117d531209f5e253899ecaaf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 23:35:03 -0400 Subject: [PATCH 12/17] Expanded SmartFactor wrapping --- gtsam/slam/doc/SmartProjectionFactor.ipynb | 166 +++++------------- gtsam/slam/doc/SmartProjectionParams.ipynb | 22 ++- .../slam/doc/SmartProjectionPoseFactor.ipynb | 51 +++--- gtsam/slam/slam.i | 31 +++- .../covarianceAnalysisCreateFactorGraph.m | 4 +- 5 files changed, 114 insertions(+), 160 deletions(-) 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 From 603caf18b4101d9f12f567f212e4e38fc8229ab2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 00:17:48 -0400 Subject: [PATCH 13/17] Edited docs only --- gtsam/slam/doc/SmartProjectionFactor.ipynb | 5 ++++- gtsam/slam/doc/SmartProjectionPoseFactor.ipynb | 5 ++++- gtsam/slam/slam.md | 14 +++++++------- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index 51746ce34..ec4d3eae7 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -26,7 +26,10 @@ "- **Configuration:** Its behavior (linearization method, handling of degenerate triangulations) is controlled by `SmartProjectionParams`.\n", "\n", "**Use Case:** Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses.\n", - "**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency." + "**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency.\n", + "\n", + "If you are using the factor, please cite:\n", + "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014." ] }, { diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index f48ff9191..da3559326 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -26,7 +26,10 @@ "- **Configuration:** Behavior is controlled by `SmartProjectionParams`.\n", "\n", "**Use Case:** Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant.\n", - "**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`." + "**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`.\n", + "\n", + "If you are using the factor, please cite:\n", + "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014." ] }, { diff --git a/gtsam/slam/slam.md b/gtsam/slam/slam.md index 4ae7875c7..748743a49 100644 --- a/gtsam/slam/slam.md +++ b/gtsam/slam/slam.md @@ -1,26 +1,24 @@ -# SLAM Factors and Algorithms +# SLAM The `slam` module provides a collection of factors, constraints, utilities, and initialization algorithms commonly used in Simultaneous Localization and Mapping (SLAM) and Structure from Motion (SfM) applications. It builds upon the core GTSAM inference engine (`gtsam/inference`) and geometric types (`gtsam/geometry`). ## Core Factors These are fundamental factor types often used as building blocks in SLAM. - -- [BetweenFactor](doc/BetweenFactor.ipynb) : Represents relative measurements between two poses or other Lie group variables (e.g., odometry). -- [PoseRotationPrior](doc/PoseRotationPrior.ipynb) : A prior factor acting only on the rotation component of a pose variable. -- [PoseTranslationPrior](doc/PoseTranslationPrior.ipynb) : A prior factor acting only on the translation component of a pose variable. +- [PriorFactor](doc/PriorFactor.ipynb) : A prior factor acting only on the rotation component of a pose variable. +- [BetweenFactor](doc/BetweenFactor.ipynb) : Represents relative measurements between two poses or other Lie group variables (e.g., derived from [odometry](https://en.wikipedia.org/wiki/Odometry)). ## Visual SLAM/SfM Factors Factors specifically designed for visual data (camera measurements). - [GenericProjectionFactor](doc/GenericProjectionFactor.ipynb) : Standard monocular projection factor relating a 3D landmark, camera pose, and fixed calibration to a 2D measurement. -- [StereoFactor](doc/StereoFactor.ipynb) : Standard stereo projection factor relating a 3D landmark, camera pose, and fixed stereo calibration to a `StereoPoint2` measurement. -- [PlanarProjectionFactor](doc/PlanarProjectionFactor.ipynb) : Projection factors specialized for robots moving on a 2D plane. - [GeneralSFMFactor](doc/GeneralSFMFactor.ipynb) : Projection factors used when camera calibration is unknown or optimized alongside poses and landmarks. +- [StereoFactor](doc/StereoFactor.ipynb) : Standard stereo projection factor relating a 3D landmark, camera pose, and fixed stereo calibration to a `StereoPoint2` measurement. - [EssentialMatrixFactor](doc/EssentialMatrixFactor.ipynb) : Factors constraining poses or calibration based on the Essential matrix derived from calibrated cameras. - [EssentialMatrixConstraint](doc/EssentialMatrixConstraint.ipynb) : Factor constraining the relative pose between two cameras based on a measured Essential matrix. - [TriangulationFactor](doc/TriangulationFactor.ipynb) : Factor constraining a 3D point based on a measurement from a single known camera view, useful for triangulation. +- [PlanarProjectionFactor](doc/PlanarProjectionFactor.ipynb) : Projection factors specialized for robots moving on a 2D plane. ## Smart Factors @@ -36,6 +34,8 @@ Factors that implicitly manage landmark variables, marginalizing them out during Factors representing various geometric relationships or constraints. +- [PoseRotationPrior](doc/PoseRotationPrior.ipynb) : A prior factor acting only on the rotation component of a pose variable. +- [PoseTranslationPrior](doc/PoseTranslationPrior.ipynb) : A prior factor acting only on the translation component of a pose variable. - [OrientedPlane3Factor](doc/OrientedPlane3Factor.ipynb) : Factors for estimating and constraining 3D planar landmarks (`OrientedPlane3`). - [RotateFactor](doc/RotateFactor.ipynb) : Factors constraining an unknown rotation based on how it transforms measured rotations or directions. - [KarcherMeanFactor](doc/KarcherMeanFactor.ipynb) : Factor for constraining the Karcher mean (geometric average) of a set of rotations or other manifold values. From 2cfd2e00a51fbe9ae2e468e9b483476812c434c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 00:22:36 -0400 Subject: [PATCH 14/17] Wrap Rig version --- gtsam/geometry/SimpleCamera.h | 7 +- gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 171 +++++------------- gtsam/slam/slam.i | 78 ++++---- 3 files changed, 86 insertions(+), 170 deletions(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 794451442..db36bd6ba 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -31,10 +31,15 @@ namespace gtsam { /// Convenient aliases for Pinhole camera classes with different calibrations. /// Also needed as forward declarations in the wrapper. + using PinholePoseCal3_S2 = gtsam::PinholePose; + using PinholePoseCal3Bundler = gtsam::PinholePose; + using PinholePoseCal3DS2 = gtsam::PinholePose; + using PinholePoseCal3Unified = gtsam::PinholePose; + using PinholePoseCal3Fisheye = gtsam::PinholePose; using PinholeCameraCal3_S2 = gtsam::PinholeCamera; using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; - + } // namespace gtsam diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index 09d23b3ca..10a9e4092 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -28,7 +28,10 @@ "- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n", "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", "\n", - "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized." + "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized.\n", + "\n", + "If you are using the factor, please cite:\n", + "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014.\n" ] }, { @@ -42,7 +45,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -51,38 +54,36 @@ }, "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": 9, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_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, SmartProjectionRigFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m CameraSetPinholeCameraCal3_S2, 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 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", - " SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n", - " CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "X = symbol_shorthand.X # Key for Pose3 variable (Body Pose)" + "from gtsam import (\n", + " Values,\n", + " Point2,\n", + " Point3,\n", + " Pose3,\n", + " Rot3,\n", + " NonlinearFactorGraph,\n", + " SmartProjectionParams,\n", + " SmartProjectionRigFactorPinholePoseCal3_S2,\n", + " PinholePoseCal3_S2,\n", + " Cal3_S2,\n", + ")\n", + "from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)" ] }, { @@ -107,99 +108,20 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": { "id": "create_example_code" }, "outputs": [ { - "name": "stdout", - "output_type": "stream", - "text": [ - "Smart factor involves 4 measurements from 2 unique poses.\n", - "SmartFactorRig: SmartProjectionRigFactor: \n", - " -- Measurement nr 0\n", - "key: x0\n", - "cameraId: 0\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 0 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "-- Measurement nr 1\n", - "key: x0\n", - "cameraId: 1\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 -0.1 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "-- Measurement nr 2\n", - "key: x1\n", - "cameraId: 0\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 0 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "-- Measurement nr 3\n", - "key: x1\n", - "cameraId: 1\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 -0.1 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "SmartProjectionFactor\n", - "linearizationMode: 0\n", - "triangulationParameters:\n", - "rankTolerance = 1e-09\n", - "enableEPI = false\n", - "landmarkDistanceThreshold = -1\n", - "dynamicOutlierRejectionThreshold = -1\n", - "\n", - "\n", - "result:\n", - "Degenerate\n", - "\n", - "SmartFactorBase, z = \n", - "measurement 0, px = \n", - "[300; 200]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 1, px = \n", - "[250; 201]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 2, px = \n", - "[310; 210]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 3, px = \n", - "[260; 211]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "Factor on x0 x1\n" + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[10], line 19\u001b[0m\n\u001b[1;32m 15\u001b[0m smart_params \u001b[38;5;241m=\u001b[39m SmartProjectionParams(linMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mLinearizationMode\u001b[38;5;241m.\u001b[39mHESSIAN,\n\u001b[1;32m 16\u001b[0m degMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mDegeneracyMode\u001b[38;5;241m.\u001b[39mZERO_ON_DEGENERACY)\n\u001b[1;32m 18\u001b[0m \u001b[38;5;66;03m# Factor type includes the Camera type\u001b[39;00m\n\u001b[0;32m---> 19\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionRigFactorPinholePoseCal3_S2\u001b[49m\u001b[43m(\u001b[49m\u001b[43msmart_noise\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrig_cameras\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43msmart_params\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 21\u001b[0m \u001b[38;5;66;03m# 3. Add measurements\u001b[39;00m\n\u001b[1;32m 22\u001b[0m \u001b[38;5;66;03m# Observation from Body Pose X(0), Camera 0\u001b[39;00m\n\u001b[1;32m 23\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m300\u001b[39m, \u001b[38;5;241m200\u001b[39m), X(\u001b[38;5;241m0\u001b[39m), \u001b[38;5;241m0\u001b[39m)\n", + "\u001b[0;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n" ] } ], @@ -208,24 +130,21 @@ "K = Cal3_S2(500, 500, 0, 320, 240)\n", "# Camera 0: Forward facing, slightly offset\n", "body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n", - "cam0 = PinholeCameraCal3_S2(body_P_cam0, K)\n", + "cam0 = PinholePoseCal3_S2(body_P_cam0, K)\n", "# Camera 1: Stereo camera, right of cam0\n", "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", - "cam1 = PinholeCameraCal3_S2(body_P_cam1, K)\n", + "cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n", "\n", - "rig_cameras = CameraSetPinholeCameraCal3_S2()\n", - "rig_cameras.append(cam0)\n", - "rig_cameras.append(cam1)\n", - "rig_cameras_ptr = gtsam.make_shared_CameraSetPinholeCameraCal3_S2(rig_cameras)\n", + "rig_cameras = [cam0,cam1]\n", "\n", "# 2. Create the Factor\n", - "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "# Ensure parameters are compatible (HESSIAN, ZERO_ON_DEGENERACY)\n", - "smart_params = SmartProjectionParams(linearizationMode=gtsam.LinearizationMode.HESSIAN,\n", - " degeneracyMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", + "smart_params = SmartProjectionParams(linMode=gtsam.LinearizationMode.HESSIAN,\n", + " degMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", "\n", "# Factor type includes the Camera type\n", - "smart_factor = SmartProjectionRigFactorPinholeCameraCal3_S2(smart_noise, rig_cameras_ptr, smart_params)\n", + "smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(noise_model, rig_cameras, smart_params)\n", "\n", "# 3. Add measurements\n", "# Observation from Body Pose X(0), Camera 0\n", @@ -261,7 +180,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "metadata": { "id": "eval_example_code" }, @@ -321,7 +240,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "metadata": { "id": "linearize_example_code" }, @@ -396,7 +315,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -410,7 +329,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 b733d0c6a..a0c482fdd 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -157,16 +157,22 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { #include -template +template < + CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, + gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2, + gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler, + gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}> 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; + + void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; }; #include @@ -193,10 +199,12 @@ class SmartProjectionParams { void print(const std::string& str = "") const; }; -template +template < + CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, + gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2, + gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler, + gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}> virtual class SmartProjectionFactor : gtsam::SmartFactorBase { SmartProjectionFactor(); @@ -204,10 +212,6 @@ virtual class SmartProjectionFactor : gtsam::SmartFactorBase { const gtsam::noiseModel::Base* sharedNoiseModel, const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); - void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; - bool decideIfTriangulate(const gtsam::CameraSet& cameras) const; gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet& cameras) const; bool triangulateForLinearize(const gtsam::CameraSet& cameras) const; @@ -284,41 +288,29 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { gtsam::TriangulationResult point(const gtsam::Values& values) const; }; -// WIP -// #include -// typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -// #include -// template -// class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { -// SmartProjectionRigFactor(); +#include +template +virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { + SmartProjectionRigFactor(); -// SmartProjectionRigFactor( -// const gtsam::noiseModel::Base* sharedNoiseModel, -// const gtsam::CameraSet& cameraRig, -// const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); + SmartProjectionRigFactor( + const gtsam::noiseModel::Base* sharedNoiseModel, + const gtsam::CameraSet* cameraRig, + const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); -// void add(const CAMERA::Measurement& measured, const gtsam::Key& poseKey, -// const size_t& cameraId = 0); + void add(const gtsam::Point2& measured, const gtsam::Key& poseKey, + const size_t& cameraId = 0); -// void add(const CAMERA::MeasurementVector& measurements, const gtsam::KeyVector& poseKeys, -// const gtsam::FastVector& cameraIds = gtsam::FastVector()); + void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& poseKeys, + const gtsam::FastVector& cameraIds = gtsam::FastVector()); -// const gtsam::KeyVector& nonUniqueKeys() const; - -// const gtsam::CameraSet& cameraRig() const; - -// const gtsam::FastVector& cameraIds() const; - -// void print( -// const std::string& s = "", -// const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - -// bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; - -// gtsam::CameraSet cameras(const gtsam::Values& values) const; - -// double error(const gtsam::Values& values) const; -// }; + const gtsam::KeyVector& nonUniqueKeys() const; + const gtsam::CameraSet& cameraRig() const; + const gtsam::FastVector& cameraIds() const; +}; #include template From 8bdf5326c022b42fcbf12ae22fb91f94e1447e92 Mon Sep 17 00:00:00 2001 From: p-zach Date: Sun, 27 Apr 2025 12:43:10 -0400 Subject: [PATCH 15/17] Fix ReferenceFrameFactor --- gtsam/slam/doc/ReferenceFrameFactor.ipynb | 38 +++++++---------------- gtsam/slam/slam.i | 2 +- 2 files changed, 12 insertions(+), 28 deletions(-) diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb index b60f9fa4d..a0fd0388d 100644 --- a/gtsam/slam/doc/ReferenceFrameFactor.ipynb +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -61,7 +61,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "[]\n" + "['ReferenceFrameFactorPoint3Pose3']\n" ] } ], @@ -72,34 +72,18 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 3, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'ReferenceFrameFactorPoint3Pose3' 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 5\u001b[0m\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 Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# The Python wrapper creates specific instantiations\u001b[39;00m\n\u001b[1;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 ReferenceFrameFactorPoint3Pose3\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 8\u001b[0m L \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mL \u001b[38;5;66;03m# Global landmark\u001b[39;00m\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", "from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n", "# The Python wrapper creates specific instantiations\n", "from gtsam import ReferenceFrameFactorPoint3Pose3\n", - "from gtsam import symbol_shorthand\n", - "\n", - "L = symbol_shorthand.L # Global landmark\n", - "T = symbol_shorthand.T # Transform global_T_local\n", - "l = symbol_shorthand.l # Local landmark" + "from gtsam.symbol_shorthand import L, T, O" ] }, { @@ -123,7 +107,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "create_example_code" }, @@ -132,15 +116,15 @@ "name": "stdout", "output_type": "stream", "text": [ - "ReferenceFrameFactor: : ReferenceFrameFactor(Global: L0, Transform: T0, Local: l0)\n", - " noise model: diagonal sigmas [0.1; 0.1; 0.1];\n" + "ReferenceFrameFactor: : ReferenceFrameFactor(Global: l0, Transform: t0, Local: o0)\n", + "isotropic dim=3 sigma=0.1\n" ] } ], "source": [ "global_landmark_key = L(0)\n", "transform_key = T(0)\n", - "local_landmark_key = l(0)\n", + "local_landmark_key = O(0)\n", "\n", "# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)\n", "noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev\n", @@ -173,7 +157,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 5, "metadata": { "id": "eval_example_code" }, @@ -184,8 +168,8 @@ "text": [ "Expected local landmark: [ 2. -4. 1.]\n", "\n", - "Error at ground truth: [-0. 0. 0.] (Should be zero)\n", - "Error with noisy local landmark: [-1. 1. -0.5]\n" + "Error at ground truth: 4500.0 (Should be zero)\n", + "Error with noisy local landmark: 4621.125\n" ] } ], diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index a0c482fdd..0b03000a9 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -342,7 +342,7 @@ typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include -template +template class ReferenceFrameFactor : gtsam::NoiseModelFactor { ReferenceFrameFactor(gtsam::Key globalKey, gtsam::Key transKey, gtsam::Key localKey, const gtsam::noiseModel::Base* model); From cbc45c9f4ef9e9bae9bceeebc880e28ddddb4a08 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 12:47:37 -0400 Subject: [PATCH 16/17] Working rig ! --- gtsam/geometry/geometry.i | 2 +- gtsam/geometry/triangulation.h | 7 + gtsam/slam/SmartProjectionRigFactor.h | 24 ++ gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 216 +++++++++--------- gtsam/slam/slam.i | 2 + 5 files changed, 144 insertions(+), 107 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index b5f6f1f3f..87bdde7c9 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1317,7 +1317,7 @@ class Similarity3 { double scale() const; }; -template +template class CameraSet { CameraSet(); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 749824845..a715f32f9 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -758,10 +758,17 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } // Vector of Cameras - used by the Python/MATLAB wrapper +using CameraSetPinholePoseCal3Bundler = CameraSet>; +using CameraSetPinholePoseCal3_S2 = CameraSet>; +using CameraSetPinholePoseCal3DS2 = CameraSet>; +using CameraSetPinholePoseCal3Fisheye = CameraSet>; +using CameraSetPinholePoseCal3Unified = CameraSet>; + using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; using CameraSetCal3DS2 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; + using CameraSetSpherical = CameraSet; } // \namespace gtsam diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index dd4237299..b8d8e42dc 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -46,6 +46,30 @@ namespace gtsam { * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor * instead! If the calibration should be optimized, as well, use * SmartProjectionFactor instead! + * + * Note on Template Parameter `CAMERA`: + * While this factor is templated on `CAMERA` to allow for generality (e.g., + * `SphericalCamera`), the current internal implementation for linearization + * (specifically, methods like `createHessianFactor` involving Schur complement + * calculations inherited or adapted from base classes) has limitations. It + * implicitly assumes that the CAMERA only has a Pose3 unknown. + * + * Consequently: + * - This factor works correctly with `CAMERA` types where this is the case, + * such as `PinholePose` or `SphericalCamera`. + * - Using `CAMERA` types where `dimension != 6`, such as + * `PinholeCamera` (which has dimension `6 + CalDim`), will lead + * to compilation errors due to matrix dimension mismatches. + * + * Therefore, for standard pinhole cameras within a fixed rig, use + * `PinholePose` as the `CAMERA` template parameter when defining + * the `CameraSet` passed to this factor's constructor. + * + * TODO(dellaert): Refactor the internal linearization logic (e.g., in + * `createHessianFactor`) to explicitly compute Jacobians with respect to the + * 6-DoF body pose by correctly applying the chain rule, rather than relying on + * `traits::dimension` for downstream calculations. + * * @ingroup slam */ template diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index 10a9e4092..1395440f1 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -63,7 +63,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 1, "metadata": { "id": "imports_code" }, @@ -81,6 +81,7 @@ " SmartProjectionParams,\n", " SmartProjectionRigFactorPinholePoseCal3_S2,\n", " PinholePoseCal3_S2,\n", + " CameraSetPinholePoseCal3_S2,\n", " Cal3_S2,\n", ")\n", "from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)" @@ -108,20 +109,112 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 3, "metadata": { "id": "create_example_code" }, "outputs": [ { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[10], line 19\u001b[0m\n\u001b[1;32m 15\u001b[0m smart_params \u001b[38;5;241m=\u001b[39m SmartProjectionParams(linMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mLinearizationMode\u001b[38;5;241m.\u001b[39mHESSIAN,\n\u001b[1;32m 16\u001b[0m degMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mDegeneracyMode\u001b[38;5;241m.\u001b[39mZERO_ON_DEGENERACY)\n\u001b[1;32m 18\u001b[0m \u001b[38;5;66;03m# Factor type includes the Camera type\u001b[39;00m\n\u001b[0;32m---> 19\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionRigFactorPinholePoseCal3_S2\u001b[49m\u001b[43m(\u001b[49m\u001b[43msmart_noise\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrig_cameras\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43msmart_params\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 21\u001b[0m \u001b[38;5;66;03m# 3. Add measurements\u001b[39;00m\n\u001b[1;32m 22\u001b[0m \u001b[38;5;66;03m# Observation from Body Pose X(0), Camera 0\u001b[39;00m\n\u001b[1;32m 23\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m300\u001b[39m, \u001b[38;5;241m200\u001b[39m), X(\u001b[38;5;241m0\u001b[39m), \u001b[38;5;241m0\u001b[39m)\n", - "\u001b[0;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n" + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 2 measurements from 2 unique poses.\n", + "SmartFactorRig: SmartProjectionRigFactor: \n", + " -- Measurement nr 0\n", + "key: x0\n", + "cameraId: 0\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "-- Measurement nr 1\n", + "key: x0\n", + "cameraId: 1\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 -0.1 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "-- Measurement nr 2\n", + "key: x1\n", + "cameraId: 0\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "-- Measurement nr 3\n", + "key: x1\n", + "cameraId: 1\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 -0.1 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "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", + "300\n", + "200\n", + "noise model = unit (2) \n", + "measurement 1, px = \n", + "250\n", + "201\n", + "noise model = unit (2) \n", + "measurement 2, px = \n", + "310\n", + "210\n", + "noise model = unit (2) \n", + "measurement 3, px = \n", + "260\n", + "211\n", + "noise model = unit (2) \n", + " keys = { x0 x1 }\n" ] } ], @@ -135,7 +228,9 @@ "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", "cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n", "\n", - "rig_cameras = [cam0,cam1]\n", + "rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()\n", + "rig_cameras.push_back(cam0)\n", + "rig_cameras.push_back(cam1)\n", "\n", "# 2. Create the Factor\n", "noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", @@ -180,7 +275,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 4, "metadata": { "id": "eval_example_code" }, @@ -190,9 +285,10 @@ "output_type": "stream", "text": [ "Triangulated point result:\n", - "Valid triangulation with point [0.70307883 0.20615766 5.18676602]\n", + "\n", "\n", - "Total reprojection error (0.5 * sum(err^2/sigma^2)): 181.1904\n" + "Triangulation failed, error calculation depends on degeneracyMode.\n", + "Error when degenerate: 0.0\n" ] } ], @@ -219,98 +315,6 @@ " total_error = smart_factor.error(values)\n", " print(f\"Error when degenerate: {total_error}\")" ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_header_md" - }, - "source": [ - "## Linearization" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_desc_md" - }, - "source": [ - "Linearization (currently restricted to HESSIAN mode) produces a `RegularHessianFactor` connecting the unique body pose (`Pose3`) variables involved." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "linearize_example_code" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "Linearized Factor (HessianFactor structure):\n", - "RegularHessianFactor(6): density=100% keys={ x0 x1 }\n", - "Augmented information matrix: (dimensions: 6, 6) : \n", - "{\n", - "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", - "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", - "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", - "\t[ -15.09, -101.6, -33.87, 181.2, -46.13, 41.54; ]\n", - "\t[ 3.829, 25.77, 8.589, -46.13, 11.71, -10.54; ]\n", - "\t[ -3.448, -23.21, -7.737, 41.54, -10.54, 9.497; ]\n", - "\t[ 1.257, 8.427, 2.81, -1.257, -8.427, -2.81; ]\n", - "\t[ 8.427, 56.73, 18.91, -8.427, -56.73, -18.91; ]\n", - "\t[ 2.81, 18.91, 6.302, -2.81, -18.91, -6.302; ]\n", - "\t[ -15.09, -101.6, -33.87, 15.09, 101.6, 33.87; ]\n", - "\t[ 3.829, 25.77, 8.589, -3.829, -25.77, -8.589; ]\n", - "\t[ -3.448, -23.21, -7.737, 3.448, 23.21, 7.737; ]\n", - "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", - "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", - "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", - "\t[ 15.09, 101.6, 33.87, -181.2, 46.13, -41.54; ]\n", - "\t[ -3.829, -25.77, -8.589, 46.13, -11.71, 10.54; ]\n", - "\t[ 3.448, 23.21, 7.737, -41.54, 10.54, -9.497; ]\n", - "}\n", - "Augmented Diagonal Block [0,0]:\n", - "[ 1.257, 8.427, 2.81; ]\n", - "[ 8.427, 56.73, 18.91; ]\n", - "[ 2.81, 18.91, 6.302; ]\n", - "\n", - "Augmented Diagonal Block [1,1]:\n", - "[ 1.257, 8.427, 2.81; ]\n", - "[ 8.427, 56.73, 18.91; ]\n", - "[ 2.81, 18.91, 6.302; ]\n", - "\n", - "Off-Diagonal Block [0,1]:\n", - "[ -1.257, -8.427, -2.81; ]\n", - "[ -8.427, -56.73, -18.91; ]\n", - "[ -2.81, -18.91, -6.302; ]\n", - "\n", - "Error vector:\n", - "[-15.087; -101.588; -33.8672; 181.19; -46.1294; 41.5391; 15.087; 101.588; 33.8672; -181.19; 46.1294; -41.5391]\n", - "Constant error term: 181.19\n" - ] - } - ], - "source": [ - "graph = NonlinearFactorGraph()\n", - "graph.add(smart_factor)\n", - "\n", - "# Linearize (HESSIAN mode)\n", - "linear_factor = smart_factor.linearize(values)\n", - "\n", - "if linear_factor:\n", - " print(\"\\nLinearized Factor (HessianFactor structure):\")\n", - " hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n", - " if hessian_factor:\n", - " hessian_factor.print()\n", - " else:\n", - " print(\"Linearized factor is not a HessianFactor\")\n", - "else:\n", - " print(\"Linearization failed (likely due to triangulation degeneracy)\")" - ] } ], "metadata": { diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 0b03000a9..ec6d135b2 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -157,6 +157,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { #include +// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3 template < CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, @@ -289,6 +290,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { }; #include +// Only for PinholePose cameras -> PinholeCamera is not supported template Date: Sun, 27 Apr 2025 13:01:03 -0400 Subject: [PATCH 17/17] Better docs --- gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 525 +++++++++++++----- 1 file changed, 385 insertions(+), 140 deletions(-) diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index 1395440f1..6d1e38210 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -24,12 +24,19 @@ "- **Multiple Observations per Pose:** Allows multiple measurements associated with the *same* body pose key, but originating from different cameras within the rig.\n", "- **Camera Indexing:** Each measurement must be associated with both a body pose key and a `cameraId` (index) specifying which camera in the rig took the measurement.\n", "- **Fixed Calibration/Extrinsics:** The intrinsics and relative extrinsics of the cameras within the rig are assumed fixed.\n", - "- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholeCamera`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts.\n", + "- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholePose`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts. **Important Note:** See the **Note on Template Parameter `CAMERA`** below.\n", "- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n", - "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", + "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of C++ header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", "\n", "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized.\n", "\n", + "**Note on Template Parameter `CAMERA`:**\n", + "While this factor is templated on `CAMERA` for generality, the current internal implementation for linearization has limitations. It implicitly assumes that `traits::dimension` matches the optimized variable dimension (`Pose3::dimension`, which is 6).\n", + "Consequently:\n", + "- It works correctly with `CAMERA` types where `dimension == 6`, such as `PinholePose` or `SphericalCamera`.\n", + "- Using `CAMERA` types where `dimension != 6`, such as `PinholeCamera` (dim = 6 + CalDim), **will cause compilation errors**.\n", + "- **Recommendation:** For standard pinhole cameras in a fixed rig, **use `PinholePose`** as the `CAMERA` type when defining the `CameraSet` for this factor.\n", + "\n", "If you are using the factor, please cite:\n", "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014.\n" ] @@ -63,25 +70,25 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, "outputs": [], "source": [ "import gtsam\n", - "import numpy as np\n", "from gtsam import (\n", " Values,\n", " Point2,\n", " Point3,\n", " Pose3,\n", " Rot3,\n", - " NonlinearFactorGraph,\n", " SmartProjectionParams,\n", + " LinearizationMode,\n", + " DegeneracyMode,\n", + " # Use PinholePose variant for wrapping\n", " SmartProjectionRigFactorPinholePoseCal3_S2,\n", " PinholePoseCal3_S2,\n", - " CameraSetPinholePoseCal3_S2,\n", " Cal3_S2,\n", ")\n", "from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)" @@ -93,98 +100,34 @@ "id": "create_header_md" }, "source": [ - "## Creating the Rig and Factor" + "## Constructor" ] }, { "cell_type": "markdown", "metadata": { - "id": "create_desc_md" + "id": "constructor_desc_md" }, "source": [ - "1. Define the camera rig configuration: Create a `CameraSet` containing the `CAMERA` objects (with fixed intrinsics and rig-relative extrinsics).\n", - "2. Create the `SmartProjectionRigFactor` with noise, the rig, and parameters.\n", - "3. Add measurements, specifying the 2D point, the corresponding **body pose key**, and the **camera ID** within the rig." + "You create a `SmartProjectionRigFactor` by providing:\n", + "1. A noise model for the 2D pixel measurements (typically `noiseModel.Isotropic`).\n", + "2. A `CameraSet` object defining the *fixed* rig configuration. Each `CAMERA` in the set contains its fixed intrinsics and its fixed pose relative to the rig's body frame (`body_P_camera`).\n", + "3. Optionally, `SmartProjectionParams` to configure linearization and degeneracy handling. Remember the current restrictions (HESSIAN, ZERO_ON_DEGENERACY)." ] }, { "cell_type": "code", "execution_count": 3, "metadata": { - "id": "create_example_code" + "id": "constructor_example_code" }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Smart factor involves 2 measurements from 2 unique poses.\n", "SmartFactorRig: SmartProjectionRigFactor: \n", - " -- Measurement nr 0\n", - "key: x0\n", - "cameraId: 0\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 0 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "-- Measurement nr 1\n", - "key: x0\n", - "cameraId: 1\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 -0.1 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "-- Measurement nr 2\n", - "key: x1\n", - "cameraId: 0\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 0 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "-- Measurement nr 3\n", - "key: x1\n", - "cameraId: 1\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 -0.1 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "SmartProjectionFactor\n", + " SmartProjectionFactor\n", "linearizationMode: 0\n", "triangulationParameters:\n", "rankTolerance = 1\n", @@ -198,122 +141,424 @@ "no point, status = 1\n", "\n", "SmartFactorBase, z = \n", - "measurement 0, px = \n", - "300\n", - "200\n", - "noise model = unit (2) \n", - "measurement 1, px = \n", - "250\n", - "201\n", - "noise model = unit (2) \n", - "measurement 2, px = \n", - "310\n", - "210\n", - "noise model = unit (2) \n", - "measurement 3, px = \n", - "260\n", - "211\n", - "noise model = unit (2) \n", - " keys = { x0 x1 }\n" + " keys = { }\n" ] } ], "source": [ - "# 1. Define the Camera Rig\n", + "# Define the Camera Rig (using PinholePose)\n", "K = Cal3_S2(500, 500, 0, 320, 240)\n", - "# Camera 0: Forward facing, slightly offset\n", "body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n", "cam0 = PinholePoseCal3_S2(body_P_cam0, K)\n", - "# Camera 1: Stereo camera, right of cam0\n", - "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", + "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", "cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n", "\n", "rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()\n", "rig_cameras.push_back(cam0)\n", "rig_cameras.push_back(cam1)\n", "\n", - "# 2. Create the Factor\n", + "# Noise model and parameters\n", "noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", - "# Ensure parameters are compatible (HESSIAN, ZERO_ON_DEGENERACY)\n", - "smart_params = SmartProjectionParams(linMode=gtsam.LinearizationMode.HESSIAN,\n", - " degMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", + "smart_params = SmartProjectionParams(\n", + " linMode=LinearizationMode.HESSIAN, degMode=DegeneracyMode.ZERO_ON_DEGENERACY\n", + ")\n", "\n", - "# Factor type includes the Camera type\n", - "smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(noise_model, rig_cameras, smart_params)\n", - "\n", - "# 3. Add measurements\n", - "# Observation from Body Pose X(0), Camera 0\n", - "smart_factor.add(Point2(300, 200), X(0), 0)\n", - "# Observation from Body Pose X(0), Camera 1 (stereo pair?)\n", - "smart_factor.add(Point2(250, 201), X(0), 1)\n", - "# Observation from Body Pose X(1), Camera 0\n", - "smart_factor.add(Point2(310, 210), X(1), 0)\n", - "# Observation from Body Pose X(1), Camera 1\n", - "smart_factor.add(Point2(260, 211), X(1), 1)\n", - "\n", - "print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n", + "# Create the Factor\n", + "smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(\n", + " noise_model, rig_cameras, smart_params\n", + ")\n", "smart_factor.print(\"SmartFactorRig: \")" ] }, { "cell_type": "markdown", "metadata": { - "id": "eval_header_md" + "id": "add_header_md" }, "source": [ - "## Evaluating the Error" + "## `add(measurement, poseKey, cameraId)`" ] }, { "cell_type": "markdown", "metadata": { - "id": "eval_desc_md" + "id": "add_desc_md" }, "source": [ - "The `.error(values)` method uses the `Pose3` objects (body poses) from `values` and the fixed rig configuration to triangulate the point and compute the error." + "This method adds a single 2D measurement (`Point2`) associated with a specific camera in the rig and a specific body pose.\n", + "- `measurement`: The observed pixel coordinates.\n", + "- `poseKey`: The key (`Symbol` or `Key`) of the **body's `Pose3`** variable at the time of observation.\n", + "- `cameraId`: The integer index of the camera within the `CameraSet` (provided during construction) that captured this measurement." ] }, { "cell_type": "code", "execution_count": 4, "metadata": { - "id": "eval_example_code" + "id": "add_example_code" }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Triangulated point result:\n", - "\n", - "\n", - "Triangulation failed, error calculation depends on degeneracyMode.\n", - "Error when degenerate: 0.0\n" + "Smart factor involves 2 measurements from 2 unique poses.\n" ] } ], "source": [ - "# Create Values containing Body Pose3 objects\n", + "# --- Use Pre-calculated Valid Measurements ---\n", + "# These measurements were calculated offline using:\n", + "# gt_landmark = Point3(1.0, 0.5, 5.0)\n", + "# gt_pose0 = Pose3()\n", + "# gt_pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n", + "# And the rig defined above.\n", + "\n", + "z00 = Point2(400.0, 290.0) # Measurement from Body Pose X(0), Camera 0\n", + "z01 = Point2(350.0, 290.0) # Measurement from Body Pose X(0), Camera 1\n", + "z10 = Point2(372.787, 297.553) # Measurement from Body Pose X(1), Camera 0\n", + "z11 = Point2(323.308, 297.674) # Measurement from Body Pose X(1), Camera 1\n", + "# --------------------------------------------\n", + "\n", + "# 3. Add pre-calculated measurements\n", + "smart_factor.add(z00, X(0), 0)\n", + "smart_factor.add(z01, X(0), 1)\n", + "smart_factor.add(z10, X(1), 0)\n", + "smart_factor.add(z11, X(1), 1)\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n", + "# smart_factor.print(\"SmartFactorRig (with pre-calculated measurements): \") # Optional\n" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "inherited_header_md" + }, + "source": [ + "## Inherited and Core Methods" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "error_header_md" + }, + "source": [ + "### `error(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "error_desc_md" + }, + "source": [ + "Inherited from `SmartFactorBase`. Calculates the total reprojection error for the landmark.\n", + "**Internal Process:**\n", + "1. Retrieves the body `Pose3` estimates for all relevant keys from the `values` object.\n", + "2. For each measurement, calculates the corresponding camera's world pose using the body pose and the fixed rig extrinsics (`world_P_sensor = world_P_body * body_P_camera`).\n", + "3. Triangulates the 3D landmark position using these calculated camera poses and the stored 2D measurements.\n", + "4. Reprojects the triangulated point back into each calculated camera view.\n", + "5. Computes the sum of squared differences between the reprojections and the original measurements, weighted by the noise model.\n", + "6. Handles degenerate cases (e.g., failed triangulation) based on the `degeneracyMode` set in `SmartProjectionParams`." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "error_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulation Result Status: Status.VALID\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 1316.4717\n" + ] + } + ], + "source": [ + "# Assuming smart_factor created and measurements added above\n", + "\n", "values = Values()\n", - "pose0 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))\n", - "pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n", + "pose0 = Pose3() # Body at origin\n", + "pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0)) # Body moved\n", "values.insert(X(0), pose0)\n", "values.insert(X(1), pose1)\n", "\n", - "# Triangulate first to see the implicit point\n", - "# The 'cameras' method internally combines body poses with rig extrinsics\n", + "# Need to check triangulation first, as error calculation depends on it\n", "point_result = smart_factor.point(values)\n", - "print(f\"Triangulated point result:\\n{point_result}\")\n", + "print(f\"Triangulation Result Status: {point_result.status}\")\n", + "\n", + "total_error = smart_factor.error(values)\n", + "print(f\"Total reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "# Note: If degenerate and DegeneracyMode is ZERO_ON_DEGENERACY, error will be 0." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "point_header_md" + }, + "source": [ + "### `point(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "point_desc_md" + }, + "source": [ + "Inherited from `SmartProjectionFactor`. Performs the internal triangulation based on the body poses in `values` and the fixed rig geometry.\n", + "Returns a `TriangulationResult` object which contains:\n", + "- The triangulated `Point3` (if successful).\n", + "- A status indicating whether the triangulation was `VALID`, `DEGENERATE`, `BEHIND_CAMERA`, `OUTLIER`, or `FAR_POINT`." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "id": "point_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulation Result Status: Status.VALID\n", + "Triangulated Point: [0.94370846 0.79793704 7.63497051]\n" + ] + } + ], + "source": [ + "# Assuming smart_factor and values from the previous cell\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulation Result Status: {point_result.status}\")\n", "\n", "if point_result.valid():\n", - " # Calculate error\n", - " total_error = smart_factor.error(values)\n", - " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + " triangulated_point = point_result.get()\n", + " print(f\"Triangulated Point: {triangulated_point}\")\n", "else:\n", - " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")\n", - " # Since mode is ZERO_ON_DEGENERACY, error should be 0\n", - " total_error = smart_factor.error(values)\n", - " print(f\"Error when degenerate: {total_error}\")" + " print(\"Triangulation did not produce a valid point.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "cameras_header_md" + }, + "source": [ + "### `cameras(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "cameras_desc_md" + }, + "source": [ + "Inherited from `SmartFactorBase`. Computes and returns a `CameraSet` containing the effective cameras corresponding to *each measurement*.\n", + "For each measurement `i` associated with body pose key `k` and camera ID `cid`:\n", + "1. Retrieves the body pose `world_P_body = values.atPose3(k)`.\n", + "2. Retrieves the fixed relative pose `body_P_camera = rig_cameras.at(cid).pose()`.\n", + "3. Computes the camera's world pose `world_P_camera = world_P_body * body_P_camera`.\n", + "4. Creates a `CAMERA` object using this `world_P_camera` and the fixed intrinsics `rig_cameras.at(cid).calibration()`.\n", + "The returned `CameraSet` contains these calculated cameras, one for each measurement added via `add()`." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "id": "cameras_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Pose of camera for measurement 0 (Body X(0), Cam 0):\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + "\n", + "\n", + "Pose of camera for measurement 1 (Body X(0), Cam 1):\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 -0.1 0\n", + "\n", + "\n", + "Pose of camera for measurement 2 (Body X(1), Cam 0):\n", + "R: [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.5995 0.00998334 0\n", + "\n", + "\n", + "Pose of camera for measurement 3 (Body X(1), Cam 1):\n", + "R: [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.609484 -0.0895171 0\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Assuming smart_factor and values from previous cells\n", + "\n", + "calculated_cameras = smart_factor.cameras(values)\n", + "\n", + "# Print the world pose of each calculated camera\n", + "print(f\"Pose of camera for measurement 0 (Body X(0), Cam 0):\\n{calculated_cameras.at(0).pose()}\\n\")\n", + "print(f\"Pose of camera for measurement 1 (Body X(0), Cam 1):\\n{calculated_cameras.at(1).pose()}\\n\")\n", + "print(f\"Pose of camera for measurement 2 (Body X(1), Cam 0):\\n{calculated_cameras.at(2).pose()}\\n\")\n", + "print(f\"Pose of camera for measurement 3 (Body X(1), Cam 1):\\n{calculated_cameras.at(3).pose()}\\n\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_header_md" + }, + "source": [ + "### `linearize(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_desc_md" + }, + "source": [ + "Inherited from `SmartProjectionFactor`. Computes the linear approximation (GaussianFactor) of the factor at the linearization point defined by `values`.\n", + "For `SmartProjectionRigFactor`, due to current implementation limitations, this **must** be configured via `SmartProjectionParams` to use `LinearizationMode.HESSIAN`.\n", + "The resulting `RegularHessianFactor` connects the **unique body pose keys** involved in the measurements." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": { + "id": "linearize_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearized Factor (HessianFactor structure):\n", + "Linear Factor: \n", + " keys: x0(6) x1(6) \n", + "Augmented information matrix: [\n", + "\t255621, 1454.13, -31747.6, 636.066, -33103.6, 3605.16, -254669, 22279.1, 15195.9, 2671.95, 33001.7, -3605.16, -5437.65;\n", + "\t1454.13, 9642.56, -1187.49, 1253.63, -198.336, -75.3949, -2405.75, -9411.71, 1088.32, -1227.56, 322.499, 75.3949, -653.552;\n", + "\t-31747.6, -1187.49, 4048.22, -209.638, 4112.44, -437.73, 31729.4, -1770.15, -1992, -201.969, -4112.82, 437.73, 740.416;\n", + "\t636.066, 1253.63, -209.638, 163.769, -83.6702, -3.45048, -757.87, -1182.15, 167.803, -154.598, 99.6018, 3.45048, -94.317;\n", + "\t-33103.6, -198.336, 4112.44, -83.6702, 4287, -466.758, 32981.3, -2875.28, -1968.94, -344.734, -4273.93, 466.758, 704.833;\n", + "\t3605.16, -75.3949, -437.73, -3.45048, -466.758, 51.9764, -3582.21, 409.075, 204.351, 50.0313, 464.082, -51.9764, -70.5256;\n", + "\t-254669, -2405.75, 31729.4, -757.87, 32981.3, -3582.21, 253816, -21248.6, -15238.8, -2538.55, -32892.2, 3582.21, 5479.25;\n", + "\t22279.1, -9411.71, -1770.15, -1182.15, -2875.28, 409.075, -21248.6, 11385.4, 332.508, 1463.29, " + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "2742.9, -409.075, 142.514;\n", + "\t15195.9, 1088.32, -1992, 167.803, -1968.94, 204.351, -15238.8, 332.508, 1007.53, 29.6019, 1975.86, -204.351, -387.999;\n", + "\t2671.95, -1227.56, -201.969, -154.598, -344.734, 50.0313, -2538.55, 1463.29, 29.6019, 188.241, 327.577, -50.0313, 23.48;\n", + "\t33001.7, 322.499, -4112.82, 99.6018, -4273.93, 464.082, -32892.2, 2742.9, 1975.86, 327.577, 4262.53, -464.082, -710.727;\n", + "\t-3605.16, 75.3949, 437.73, 3.45048, 466.758, -51.9764, 3582.21, -409.075, -204.351, -50.0313, -464.082, 51.9764, 70.5256;\n", + "\t-5437.65, -653.552, 740.416, -94.317, 704.833, -70.5256, 5479.25, 142.514, -387.999, 23.48, -710.727, 70.5256, 2632.94\n", + "]\n" + ] + } + ], + "source": [ + "# Assuming smart_factor and values from previous cells\n", + "\n", + "# Check if triangulation succeeded before linearizing\n", + "if not smart_factor.point(values).valid():\n", + " print(\"Cannot linearize: triangulation failed or degenerate.\")\n", + "else:\n", + " linear_factor = smart_factor.linearize(values)\n", + "\n", + " if linear_factor:\n", + " print(\"\\nLinearized Factor (HessianFactor structure):\")\n", + " linear_factor.print(\"Linear Factor: \")\n", + " else:\n", + " print(\"\\nLinearization failed (potentially due to triangulation degeneracy and params setting).\")\n", + "\n", + "# Note: The printed Hessian is often zero when ZERO_ON_DEGENERACY is used and triangulation fails." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "other_methods_header_md" + }, + "source": [ + "### Other Inherited Methods" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "other_methods_desc_md" + }, + "source": [ + "The factor also inherits standard methods from `NonlinearFactor` and `SmartFactorBase`:\n", + "- **`keys()`**: Returns a `KeyVector` containing the **unique body pose keys** involved.\n", + "- **`measured()`**: Returns a `Point2Vector` containing all the added 2D measurements.\n", + "- **`dim()`**: Returns the dimension of the error vector (2 * number of measurements).\n", + "- **`size()`**: Returns the number of measurements added.\n", + "- **`print(s, keyFormatter)`**: Prints details about the factor.\n", + "- **`equals(other, tol)`**: Compares two factors for equality." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "id": "other_methods_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Keys: ['x0', 'x1']\n", + "Number of Measurements (size): 2\n", + "Dimension (dim): 8\n", + "Measurements: [array([400., 290.]), array([350., 290.]), array([372.787, 297.553]), array([323.308, 297.674])]\n" + ] + } + ], + "source": [ + "# Assuming smart_factor from previous cells\n", + "print(f\"Keys: {[gtsam.DefaultKeyFormatter(k) for k in smart_factor.keys()]}\")\n", + "print(f\"Number of Measurements (size): {smart_factor.size()}\")\n", + "print(f\"Dimension (dim): {smart_factor.dim()}\")\n", + "print(f\"Measurements: {smart_factor.measured()}\")" ] } ],