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

Wrap Rig version

release/4.3a0
Frank Dellaert 2025-04-27 00:22:36 -04:00
parent 603caf18b4
commit 2cfd2e00a5
3 changed files with 86 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/SimpleCamera.h
View File

@ -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<gtsam::Cal3_S2>;
using PinholePoseCal3Bundler = gtsam::PinholePose<gtsam::Cal3Bundler>;
using PinholePoseCal3DS2 = gtsam::PinholePose<gtsam::Cal3DS2>;
using PinholePoseCal3Unified = gtsam::PinholePose<gtsam::Cal3Unified>;
using PinholePoseCal3Fisheye = gtsam::PinholePose<gtsam::Cal3Fisheye>;
using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
} // namespace gtsam

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

@ -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<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"
]
}
],
@ -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,

78
gtsam/slam/slam.i
View File

@ -157,16 +157,22 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
#include <gtsam/slam/SmartFactorBase.h>
template <CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified}>
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<gtsam::Point2>& measured() const;
std::vector<CAMERA> 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 <gtsam/slam/SmartProjectionFactor.h>
@ -193,10 +199,12 @@ class SmartProjectionParams {
void print(const std::string& str = "") const;
};
template <CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified}>
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<CAMERA> {
SmartProjectionFactor();
@ -204,10 +212,6 @@ virtual class SmartProjectionFactor : gtsam::SmartFactorBase<CAMERA> {
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<CAMERA>& cameras) const;
gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet<CAMERA>& cameras) const;
bool triangulateForLinearize(const gtsam::CameraSet<CAMERA>& cameras) const;
@ -284,41 +288,29 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
gtsam::TriangulationResult point(const gtsam::Values& values) const;
};
// WIP
// #include <gtsam/geometry/PinholeCamera.h>
// typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
// #include <gtsam/slam/SmartProjectionRigFactor.h>
// template <CAMERA = {PinholeCameraCal3_S2}>
// class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
// SmartProjectionRigFactor();
#include <gtsam/slam/SmartProjectionRigFactor.h>
template <CAMERA = {gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2,
gtsam::PinholePoseCal3Bundler,
gtsam::PinholePoseCal3Fisheye,
gtsam::PinholePoseCal3Unified}>
virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
SmartProjectionRigFactor();
// SmartProjectionRigFactor(
// const gtsam::noiseModel::Base* sharedNoiseModel,
// const gtsam::CameraSet<CAMERA>& cameraRig,
// const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
SmartProjectionRigFactor(
const gtsam::noiseModel::Base* sharedNoiseModel,
const gtsam::CameraSet<CAMERA>* 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<size_t>& cameraIds = gtsam::FastVector<size_t>());
void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& poseKeys,
const gtsam::FastVector<size_t>& cameraIds = gtsam::FastVector<size_t>());
// const gtsam::KeyVector& nonUniqueKeys() const;
// const gtsam::CameraSet<CAMERA>& cameraRig() const;
// const gtsam::FastVector<size_t>& 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<CAMERA> cameras(const gtsam::Values& values) const;
// double error(const gtsam::Values& values) const;
// };
const gtsam::KeyVector& nonUniqueKeys() const;
const gtsam::CameraSet<CAMERA>& cameraRig() const;
const gtsam::FastVector<size_t>& cameraIds() const;
};
#include <gtsam/slam/StereoFactor.h>
template <POSE, LANDMARK>