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

Merge pull request #2117 from borglab/fix/new-docs-slam 

Working rig !
release/4.3a0
Frank Dellaert 2025-04-27 13:04:39 -04:00 committed by GitHub
parent 8bdf5326c0 be267031ce
commit 8f5e78c2b5
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 439 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/geometry.i
View File

@ -1317,7 +1317,7 @@ class Similarity3 {
double scale() const; double scale() const;
}; };
template <T> template <T = {gtsam::PinholePoseCal3_S2}>
class CameraSet { class CameraSet {
CameraSet(); CameraSet();

7
gtsam/geometry/triangulation.h
View File

@ -758,10 +758,17 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
} }
// Vector of Cameras - used by the Python/MATLAB wrapper // Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetPinholePoseCal3Bundler = CameraSet<PinholePose<Cal3Bundler>>;
using CameraSetPinholePoseCal3_S2 = CameraSet<PinholePose<Cal3_S2>>;
using CameraSetPinholePoseCal3DS2 = CameraSet<PinholePose<Cal3DS2>>;
using CameraSetPinholePoseCal3Fisheye = CameraSet<PinholePose<Cal3Fisheye>>;
using CameraSetPinholePoseCal3Unified = CameraSet<PinholePose<Cal3Unified>>;
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>; using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>; using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>; using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>; using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>; using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>; using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam } // \namespace gtsam

24
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -46,6 +46,30 @@ namespace gtsam {
* calibration (i.e., are from the same camera), use SmartProjectionPoseFactor * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
* instead! If the calibration should be optimized, as well, use * instead! If the calibration should be optimized, as well, use
* SmartProjectionFactor instead! * SmartProjectionFactor instead!
*
* <b>Note on Template Parameter `CAMERA`:</b>
* 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<CALIBRATION>` or `SphericalCamera`.
* - Using `CAMERA` types where `dimension != 6`, such as
* `PinholeCamera<CALIBRATION>` (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<CALIBRATION>` 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<CAMERA>::dimension` for downstream calculations.
*
* @ingroup slam * @ingroup slam
*/ */
template <class CAMERA> template <class CAMERA>

561
gtsam/slam/doc/SmartProjectionRigFactor.ipynb
View File

@ -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", "- **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", "- **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", "- **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", "- **`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", "\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", "**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", "\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<CAMERA>::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<CALIBRATION>` or `SphericalCamera`.\n",
"- Using `CAMERA` types where `dimension != 6`, such as `PinholeCamera<CALIBRATION>` (dim = 6 + CalDim), **will cause compilation errors**.\n",
"- **Recommendation:** For standard pinhole cameras in a fixed rig, **use `PinholePose<CALIBRATION>`** as the `CAMERA` type when defining the `CameraSet` for this factor.\n",
"\n",
"If you are using the factor, please cite:\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" "> **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,22 +70,23 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 9, "execution_count": 2,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
"outputs": [], "outputs": [],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n",
"from gtsam import (\n", "from gtsam import (\n",
" Values,\n", " Values,\n",
" Point2,\n", " Point2,\n",
" Point3,\n", " Point3,\n",
" Pose3,\n", " Pose3,\n",
" Rot3,\n", " Rot3,\n",
" NonlinearFactorGraph,\n",
" SmartProjectionParams,\n", " SmartProjectionParams,\n",
" LinearizationMode,\n",
" DegeneracyMode,\n",
" # Use PinholePose variant for wrapping\n",
" SmartProjectionRigFactorPinholePoseCal3_S2,\n", " SmartProjectionRigFactorPinholePoseCal3_S2,\n",
" PinholePoseCal3_S2,\n", " PinholePoseCal3_S2,\n",
" Cal3_S2,\n", " Cal3_S2,\n",
@ -92,132 +100,337 @@
"id": "create_header_md" "id": "create_header_md"
}, },
"source": [ "source": [
"## Creating the Rig and Factor" "## Constructor"
] ]
}, },
{ {
"cell_type": "markdown", "cell_type": "markdown",
"metadata": { "metadata": {
"id": "create_desc_md" "id": "constructor_desc_md"
}, },
"source": [ "source": [
"1. Define the camera rig configuration: Create a `CameraSet` containing the `CAMERA` objects (with fixed intrinsics and rig-relative extrinsics).\n", "You create a `SmartProjectionRigFactor` by providing:\n",
"2. Create the `SmartProjectionRigFactor` with noise, the rig, and parameters.\n", "1. A noise model for the 2D pixel measurements (typically `noiseModel.Isotropic`).\n",
"3. Add measurements, specifying the 2D point, the corresponding **body pose key**, and the **camera ID** within the rig." "2. A `CameraSet<CAMERA>` 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", "cell_type": "code",
"execution_count": null, "execution_count": 3,
"metadata": { "metadata": {
"id": "create_example_code" "id": "constructor_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<gtsam::PinholePose<gtsam::Cal3_S2>>, 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<gtsam::PinholePose<gtsam::Cal3_S2>>, 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"
]
}
],
"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 = 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 = PinholePoseCal3_S2(body_P_cam1, K)\n",
"\n",
"rig_cameras = [cam0,cam1]\n",
"\n",
"# 2. Create the Factor\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",
"\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",
"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": null,
"metadata": {
"id": "eval_example_code"
}, },
"outputs": [ "outputs": [
{ {
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Triangulated point result:\n", "SmartFactorRig: SmartProjectionRigFactor: \n",
"Valid triangulation with point [0.70307883 0.20615766 5.18676602]\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", "\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 181.1904\n" "result:\n",
"no point, status = 1\n",
"\n",
"SmartFactorBase, z = \n",
" keys = { }\n"
] ]
} }
], ],
"source": [ "source": [
"# Create Values containing Body Pose3 objects\n", "# Define the Camera Rig (using PinholePose)\n",
"K = Cal3_S2(500, 500, 0, 320, 240)\n",
"body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n",
"cam0 = PinholePoseCal3_S2(body_P_cam0, K)\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",
"# Noise model and parameters\n",
"noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams(\n",
" linMode=LinearizationMode.HESSIAN, degMode=DegeneracyMode.ZERO_ON_DEGENERACY\n",
")\n",
"\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": "add_header_md"
},
"source": [
"## `add(measurement, poseKey, cameraId)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "add_desc_md"
},
"source": [
"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": "add_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Smart factor involves 2 measurements from 2 unique poses.\n"
]
}
],
"source": [
"# --- 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", "values = Values()\n",
"pose0 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))\n", "pose0 = Pose3() # Body at origin\n",
"pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\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(0), pose0)\n",
"values.insert(X(1), pose1)\n", "values.insert(X(1), pose1)\n",
"\n", "\n",
"# Triangulate first to see the implicit point\n", "# Need to check triangulation first, as error calculation depends on it\n",
"# The 'cameras' method internally combines body poses with rig extrinsics\n",
"point_result = smart_factor.point(values)\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", "\n",
"if point_result.valid():\n", "if point_result.valid():\n",
" # Calculate error\n", " triangulated_point = point_result.get()\n",
" total_error = smart_factor.error(values)\n", " print(f\"Triangulated Point: {triangulated_point}\")\n",
" print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
"else:\n", "else:\n",
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")\n", " print(\"Triangulation did not produce a valid point.\")"
" # 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": "cameras_header_md"
},
"source": [
"### `cameras(Values values)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "cameras_desc_md"
},
"source": [
"Inherited from `SmartFactorBase`. Computes and returns a `CameraSet<CAMERA>` 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\")"
] ]
}, },
{ {
@ -226,7 +439,7 @@
"id": "linearize_header_md" "id": "linearize_header_md"
}, },
"source": [ "source": [
"## Linearization" "### `linearize(Values values)`"
] ]
}, },
{ {
@ -235,12 +448,14 @@
"id": "linearize_desc_md" "id": "linearize_desc_md"
}, },
"source": [ "source": [
"Linearization (currently restricted to HESSIAN mode) produces a `RegularHessianFactor` connecting the unique body pose (`Pose3`) variables involved." "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", "cell_type": "code",
"execution_count": null, "execution_count": 10,
"metadata": { "metadata": {
"id": "linearize_example_code" "id": "linearize_example_code"
}, },
@ -251,65 +466,99 @@
"text": [ "text": [
"\n", "\n",
"Linearized Factor (HessianFactor structure):\n", "Linearized Factor (HessianFactor structure):\n",
"RegularHessianFactor(6): density=100% keys={ x0 x1 }\n", "Linear Factor: \n",
"Augmented information matrix: (dimensions: 6, 6) : \n", " keys: x0(6) x1(6) \n",
"{\n", "Augmented information matrix: [\n",
"\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\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",
"\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\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[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\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",
"\t[ -15.09, -101.6, -33.87, 181.2, -46.13, 41.54; ]\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[ 3.829, 25.77, 8.589, -46.13, 11.71, -10.54; ]\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",
"\t[ -3.448, -23.21, -7.737, 41.54, -10.54, 9.497; ]\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[ 1.257, 8.427, 2.81, -1.257, -8.427, -2.81; ]\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",
"\t[ 8.427, 56.73, 18.91, -8.427, -56.73, -18.91; ]\n", "\t22279.1, -9411.71, -1770.15, -1182.15, -2875.28, 409.075, -21248.6, 11385.4, 332.508, 1463.29, "
"\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", "name": "stdout",
"\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", "output_type": "stream",
"\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", "text": [
"\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", "2742.9, -409.075, 142.514;\n",
"\t[ 15.09, 101.6, 33.87, -181.2, 46.13, -41.54; ]\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",
"\t[ -3.829, -25.77, -8.589, 46.13, -11.71, 10.54; ]\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",
"\t[ 3.448, 23.21, 7.737, -41.54, 10.54, -9.497; ]\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",
"}\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",
"Augmented Diagonal Block [0,0]:\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",
"[ 1.257, 8.427, 2.81; ]\n", "]\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": [ "source": [
"graph = NonlinearFactorGraph()\n", "# Assuming smart_factor and values from previous cells\n",
"graph.add(smart_factor)\n",
"\n", "\n",
"# Linearize (HESSIAN mode)\n", "# Check if triangulation succeeded before linearizing\n",
"linear_factor = smart_factor.linearize(values)\n", "if not smart_factor.point(values).valid():\n",
"\n", " print(\"Cannot linearize: triangulation failed or degenerate.\")\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", "else:\n",
" print(\"Linearization failed (likely due to triangulation degeneracy)\")" " 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()}\")"
] ]
} }
], ],

2
gtsam/slam/slam.i
View File

@ -157,6 +157,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
#include <gtsam/slam/SmartFactorBase.h> #include <gtsam/slam/SmartFactorBase.h>
// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3
template < template <
CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye,
@ -289,6 +290,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
}; };
#include <gtsam/slam/SmartProjectionRigFactor.h> #include <gtsam/slam/SmartProjectionRigFactor.h>
// Only for PinholePose cameras -> PinholeCamera is not supported
template <CAMERA = {gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2, template <CAMERA = {gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2,
gtsam::PinholePoseCal3Bundler, gtsam::PinholePoseCal3Bundler,
gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Fisheye,