322 lines
13 KiB
Plaintext
322 lines
13 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# SmartProjectionPoseFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"`SmartProjectionPoseFactor<CALIBRATION>` is a \"smart\" factor optimized for visual SLAM problems where the **camera calibration is known and fixed**, and only the camera **poses** are being optimized.\n",
|
|
"It inherits from `SmartProjectionFactor` but simplifies the setup by taking a single shared `CALIBRATION` object (e.g., `Cal3_S2`, `Cal3Bundler`) assumed to be used by all cameras observing the landmark.\n",
|
|
"\n",
|
|
"Key characteristics:\n",
|
|
"- **Implicit Point:** Like its base class, the 3D point is not an explicit variable.\n",
|
|
"- **Fixed Calibration:** Assumes a single, known calibration `K` for all measurements.\n",
|
|
"- **Pose Variables:** The factor connects `Pose3` variables directly.\n",
|
|
"- **`Values` Requirement:** Requires `Pose3` objects in the `Values` container for its keys.\n",
|
|
"- **Configuration:** Behavior is controlled by `SmartProjectionParams`.\n",
|
|
"\n",
|
|
"**Use Case:** Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant.\n",
|
|
"**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`.\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."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {
|
|
"id": "pip_code",
|
|
"tags": [
|
|
"remove-cell"
|
|
]
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"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": 2,
|
|
"metadata": {
|
|
"id": "imports_code"
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"import gtsam\n",
|
|
"from gtsam import (\n",
|
|
" Values,\n",
|
|
" Point2,\n",
|
|
" Point3,\n",
|
|
" Pose3,\n",
|
|
" Rot3,\n",
|
|
" NonlinearFactorGraph,\n",
|
|
" SmartProjectionParams,\n",
|
|
" SmartProjectionPoseFactorCal3_S2,\n",
|
|
" Cal3_S2,\n",
|
|
")\n",
|
|
"from gtsam.symbol_shorthand import X # Key for Pose3 variable"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_header_md"
|
|
},
|
|
"source": [
|
|
"## Creating and Adding Measurements"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_desc_md"
|
|
},
|
|
"source": [
|
|
"1. Create the factor with a noise model, the shared calibration `K`, and parameters.\n",
|
|
"2. Add measurements (2D points) and the corresponding pose keys."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {
|
|
"id": "create_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Smart factor involves 3 measurements.\n",
|
|
"SmartFactorPose: SmartProjectionPoseFactor, z = \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",
|
|
"150\n",
|
|
"505\n",
|
|
"noise model = unit (2) \n",
|
|
"measurement 1, px = \n",
|
|
"470\n",
|
|
"495\n",
|
|
"noise model = unit (2) \n",
|
|
"measurement 2, px = \n",
|
|
"480\n",
|
|
"150\n",
|
|
"noise model = unit (2) \n",
|
|
" keys = { x0 x1 x2 }\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
|
|
"smart_params = SmartProjectionParams() # Use default params\n",
|
|
"K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
|
|
"\n",
|
|
"smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n",
|
|
"\n",
|
|
"# Add measurements and keys (Pose keys)\n",
|
|
"smart_factor.add(Point2(150, 505), X(0))\n",
|
|
"smart_factor.add(Point2(470, 495), X(1))\n",
|
|
"smart_factor.add(Point2(480, 150), X(2))\n",
|
|
"\n",
|
|
"print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n",
|
|
"smart_factor.print(\"SmartFactorPose: \")"
|
|
]
|
|
},
|
|
{
|
|
"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 from `values` and the fixed `K` to triangulate the point and compute the error."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {
|
|
"id": "eval_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Triangulated point result: Status.VALID\n",
|
|
"Triangulated point: [0.28416823 1.95555615 5.67688675]\n",
|
|
"\n",
|
|
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 120243.1626\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"# Create Values containing Pose3 objects\n",
|
|
"values = Values()\n",
|
|
"pose0 = Pose3(Rot3.Ypr(0.1, 0, 0), Point3(-1, 0, 0.5))\n",
|
|
"pose1 = Pose3(Rot3.Ypr(0.0, -0.1, 0), Point3(1, 0, 0.5))\n",
|
|
"pose2 = Pose3(Rot3.Ypr(0, 0.0, 0.2), Point3(0, 1, 0.5))\n",
|
|
"\n",
|
|
"values.insert(X(0), pose0)\n",
|
|
"values.insert(X(1), pose1)\n",
|
|
"values.insert(X(2), pose2)\n",
|
|
"\n",
|
|
"# Triangulate first to see the implicit point\n",
|
|
"point_result = smart_factor.point(values)\n",
|
|
"print(f\"Triangulated point result: {point_result.status}\")\n",
|
|
"\n",
|
|
"if point_result.valid():\n",
|
|
" print(f\"Triangulated point: {point_result.get()}\")\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",
|
|
"else:\n",
|
|
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "linearize_header_md"
|
|
},
|
|
"source": [
|
|
"## Linearization"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "linearize_desc_md"
|
|
},
|
|
"source": [
|
|
"Linearization works similarly to `SmartProjectionFactor`, producing a linear factor (default: Hessian) connecting the `Pose3` variables."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 5,
|
|
"metadata": {
|
|
"id": "linearize_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"\n",
|
|
"Linearized Factor (showing HessianFactor structure):\n",
|
|
"\n",
|
|
" keys: x0(6) x1(6) x2(6) \n",
|
|
"Augmented information matrix: [\n",
|
|
"\t202102, 6373.47, -59741.9, 4386.91, -35145.2, 11091.1, -115337, -44197.1, 12233.6, -8181.39, 19268.4, -7521.18, -81683.2, -724.877, 5044.41, -484.005, 14404.6, -5767.48, 89097.4;\n",
|
|
"\t6373.47, 81115.2, -30292.7, 14717.5, -2404.93, -3343.21, 63410.4, 20584.6, -5335.01, 3787.66, -10602.2, 4111.71, -60743.7, -88128.4, 38992.5, -18242.3, 11072.3, -3393.47, 105458;\n",
|
|
"\t-59741.9, -30292.7, 27634, -6415.45, 10844.3, -1981.98, 10553.8, 5348.27, -1607.72, 998.024, -1760.06, 696.401, 44568.2, 31147.8, -15125.4, 6542.51, -7986, 2832.46, -62376.9;\n",
|
|
"\t4386.91, 14717.5, -6415.45, 2722.09, -996.496, -424.653, 9577.1, 3000.7, -765.229, 551.36, -1601.54, 620.327, -12253.7, -15890.6, 7106.5, -3294.65, 2225.84, -703.855, 20429.2;\n",
|
|
"\t-35145.2, -2404.93, 10844.3, -996.496, 6132.48, -1869.56, 18982.5, 7333.62, -2035.73, 1357.9, -3171.11, 1238.23, 15136.7, 1537.88, -1499.49, 376.242, -2675.09, 1054.42, -17138.8;\n",
|
|
"\t11091.1, -3343.21, -1981.98, -424.653, -1869.56, 777.22, -9389.67, -3428.6, 932.466, -633.633, 1569.05, -611.241, -1827.86, 3981.42, -1495.55, 805.333, 305.895, -169.934, 204.626;\n",
|
|
"\t-115337, 63410.4, 10553.8, 9577.1, 18982.5, -9389.67, 210063, -32012.2, 19847.7, -6615.86, -35357.8, 12992.1, -83063.6, 2877.07, 3675.5, 245.696, 14633.2, -5901.9, 91375.5;\n",
|
|
"\t-44197.1, 20584.6, 5348.27, 3000.7, 7333.62, -3428.6, -32012.2, 79820.6, -31086.7, 15340.5, 5564.44, -1509.55, 71381, -87978.3, 31244.3, -17668.1, -12223.3, 5946.04, -40235;\n",
|
|
"\t12233.6, -5335.01, -1607.72, -765.229, -2035.73, 932.466, 19847.7, -31086.7, 12383.1, -5991.81, -3406.72, 1051.42, -29836.9, 33051, -11561.9, 6625.01, 5124.61, -2447.34, 18485.3;\n",
|
|
"\t-8181.39, 3787.66, 998.024, 551.36, 1357.9, -633.633, -6615.86, 15340.5, -5991.81, 2949.34, 1147.27, -319.227, 13846.5, -16832.1, 5966.68, -3379.52, -2372.04, 1151.02, -7909.48;\n",
|
|
"\t19268.4, -10602.2, -1760.06, -1601.54, -3171.11, 1569.05, -35357.8, 5564.44, -3406.72, 1147.27, 5951.86, -2185.74, 14119.3, -690.008, -543.913, -82.7916, -2486.55, 1005.27, -15442.2;\n",
|
|
"\t-7521.18, 4111.71, 696.401, 620.327, 1238.23, -611.241, 12992.1, -1509.55, 1051.42, -319.227, -2185.74, 806.502, -4768.82, -371.498, 426.945, -95.4627, 842.323, -333.351, 5486.34;\n",
|
|
"\t-81683.2, -60743.7, 44568.2, -12253.7, 15136.7, -1827.86, -83063.6, 71381, -29836.9, 13846.5, 14119.3, -4768.82, 149690, -5708.64, -6412.92, -549.716, -26368.6, 10641.3, -162324;\n",
|
|
"\t-724.877, -88128.4, 31147.8, -15890.6, 1537.88, 3981.42, 2877.07, -87978.3, 33051, -16832.1, -690.008, -371.498, -5708.64, 160163, -64108.9, 32675.7, 347.645, -2041.12, -63443.5;\n",
|
|
"\t5044.41, 38992.5, -15125.4, 7106.5, -1499.49, -1495.55, 3675.5, 31244.3, -11561.9, 5966.68, -543.913, 426.945, -6412.92, -64108.9, 26167.1, -13114.9, 1394.39, 202.071, 34971.1;\n",
|
|
"\t-484.005, -18242.3, 6542.51, -3294.65, 376.242, 805.333, 245.696, -17668.1, 6625.01, -3379.52, -82.7916, -95.4627, -549.716, 32675.7, -13114.9, 6668.86, -37.4944, -372.943, -13620.5;\n",
|
|
"\t14404.6, 11072.3, -7986, 2225.84, -2675.09, 305.895, 14633.2, -12223.3, 5124.61, -2372.04, -2486.55, 842.323, -26368.6, 347.645, 1394.39, -37.4944, 4647.64, -1867.77, 28880.4;\n",
|
|
"\t-5767.48, -3393.47, 2832.46, -703.855, 1054.42, -169.934, -5901.9, 5946.04, -2447.34, 1151.02, 1005.27, -333.351, 10641.3, -2041.12, 202.071, -372.943, -1867.77, 773.191, -10827.4;\n",
|
|
"\t89097.4, 105458, -62376.9, 20429.2, -17138.8, 204.626, 91375.5, -40235, 18485.3, -7909.48, -15442.2, 5486.34, -162324, -63443.5, 34971.1, -13620.5, 28880.4, -10827.4, 240486\n",
|
|
"]\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"graph = NonlinearFactorGraph()\n",
|
|
"graph.add(smart_factor)\n",
|
|
"\n",
|
|
"# Linearize (default mode is HESSIAN)\n",
|
|
"linear_factor = smart_factor.linearize(values)\n",
|
|
"\n",
|
|
"if linear_factor:\n",
|
|
" print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n",
|
|
" # Cast to HessianFactor to print its details\n",
|
|
" hessian_factor = gtsam.HessianFactor(linear_factor)\n",
|
|
" if hessian_factor:\n",
|
|
" hessian_factor.print()\n",
|
|
" else:\n",
|
|
" print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n",
|
|
"else:\n",
|
|
" print(\"Linearization failed (likely due to triangulation degeneracy)\")"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "py312",
|
|
"language": "python",
|
|
"name": "python3"
|
|
},
|
|
"language_info": {
|
|
"codemirror_mode": {
|
|
"name": "ipython",
|
|
"version": 3
|
|
},
|
|
"file_extension": ".py",
|
|
"mimetype": "text/x-python",
|
|
"name": "python",
|
|
"nbconvert_exporter": "python",
|
|
"pygments_lexer": "ipython3",
|
|
"version": "3.12.6"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 0
|
|
}
|