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()}\")" ] } ],