From cbc45c9f4ef9e9bae9bceeebc880e28ddddb4a08 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 12:47:37 -0400 Subject: [PATCH 1/2] 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 2/2] 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()}\")" ] } ],