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