From 2cfd2e00a51fbe9ae2e468e9b483476812c434c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 00:22:36 -0400 Subject: [PATCH] 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