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

Expanded SmartFactor wrapping

release/4.3a0
Frank Dellaert 2025-04-26 23:35:03 -04:00
parent 513bbb43ed
commit 47812cd899
5 changed files with 114 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

166
gtsam/slam/doc/SmartProjectionFactor.ipynb
View File

@ -66,13 +66,19 @@
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", "from gtsam import (\n",
" SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n", " Values,\n",
" PinholeCameraCal3_S2, Cal3_S2)\n", " Point2,\n",
"from gtsam import symbol_shorthand\n", " Point3,\n",
"import graphviz\n", " Pose3,\n",
"\n", " Rot3,\n",
"C = symbol_shorthand.C # Key for Camera object" " NonlinearFactorGraph,\n",
" SmartProjectionParams,\n",
" SmartProjectionFactorPinholeCameraCal3_S2,\n",
" PinholeCameraCal3_S2,\n",
" Cal3_S2,\n",
")\n",
"from gtsam.symbol_shorthand import C"
] ]
}, },
{ {
@ -102,14 +108,37 @@
}, },
"outputs": [ "outputs": [
{ {
"ename": "AttributeError", "name": "stdout",
"evalue": "'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'", "output_type": "stream",
"output_type": "error", "text": [
"traceback": [ "Smart factor involves 3 measurements.\n",
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", "SmartFactor: SmartProjectionFactor\n",
"\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", "linearizationMode: 0\n",
"Cell \u001b[1;32mIn[3], line 8\u001b[0m\n\u001b[0;32m 5\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Add measurements and keys\u001b[39;00m\n\u001b[1;32m----> 8\u001b[0m \u001b[43msmart_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43madd\u001b[49m(Point2(\u001b[38;5;241m150\u001b[39m, \u001b[38;5;241m505\u001b[39m), C(\u001b[38;5;241m0\u001b[39m))\n\u001b[0;32m 9\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m470\u001b[39m, \u001b[38;5;241m495\u001b[39m), C(\u001b[38;5;241m1\u001b[39m))\n\u001b[0;32m 10\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m480\u001b[39m, \u001b[38;5;241m150\u001b[39m), C(\u001b[38;5;241m2\u001b[39m))\n", "triangulationParameters:\n",
"\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'" "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 = { c0 c1 c2 }\n"
] ]
} }
], ],
@ -149,7 +178,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 7, "execution_count": 4,
"metadata": { "metadata": {
"id": "eval_example_code" "id": "eval_example_code"
}, },
@ -159,7 +188,7 @@
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Triangulated point result:\n", "Triangulated point result:\n",
"Status.DEGENERATE\n", "Status.BEHIND_CAMERA\n",
"\n", "\n",
"Triangulation failed, error calculation depends on degeneracyMode.\n" "Triangulation failed, error calculation depends on degeneracyMode.\n"
] ]
@ -188,110 +217,11 @@
"else:\n", "else:\n",
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")" " 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": [
"The `.linearize(values)` method creates a linear factor (e.g., `RegularHessianFactor`) connecting the camera variables, effectively marginalizing the implicit point."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "linearize_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Linearized Factor (showing HessianFactor structure):\n",
"RegularHessianFactor(9): density=100% keys={ C0 C1 C2 }\n",
"Augmented information matrix: (dimensions: 9, 9, 9) : \n",
"{\n",
"\t[ 4.383e+04, 2.596e+04, -1.466e+04, -6.732e+03, 1.357e+04, -4.327e+03, -2.004e+04, -1.143e+03, -3.459e+03; ]\n",
"\t[ 2.596e+04, 6.879e+04, 5.416e+03, 3.589e+03, -1.075e+04, 2.158e+04, 3.107e+04, -8.167e+03, -1.237e+04; ]\n",
"\t[ -1.466e+04, 5.416e+03, 2.336e+04, -1.026e+03, -3.572e+03, -1.124e+04, -1.497e+04, -5.631e+03, 4.149e+03; ]\n",
"\t[ -6.732e+03, 3.589e+03, -1.026e+03, 7.027e+03, 1.434e+03, 3.742e+03, 2.527e+03, 2.454e+03, -6.619e+03; ]\n",
"\t[ 1.357e+04, -1.075e+04, -3.572e+03, 1.434e+03, 1.511e+04, -8.935e+02, -1.484e+04, 3.811e+03, -1.993e+03; ]\n",
"\t[ -4.327e+03, 2.158e+04, -1.124e+04, 3.742e+03, -8.935e+02, 2.587e+04, 2.085e+04, -1.193e+04, -5.818e+03; ]\n",
"\t[ -2.004e+04, 3.107e+04, -1.497e+04, 2.527e+03, -1.484e+04, 2.085e+04, 3.128e+04, -5.349e+03, -6.557e+03; ]\n",
"\t[ -1.143e+03, -8.167e+03, -5.631e+03, 2.454e+03, 3.811e+03, -1.193e+04, -5.349e+03, 1.537e+04, -1.987e+03; ]\n",
"\t[ -3.459e+03, -1.237e+04, 4.149e+03, -6.619e+03, -1.993e+03, -5.818e+03, -6.557e+03, -1.987e+03, 1.043e+04; ]\n",
"}\n",
"Augmented Diagonal Block [0,0]:\n",
"[ 4.383e+04, 2.596e+04, -1.466e+04; ]\n",
"[ 2.596e+04, 6.879e+04, 5.416e+03; ]\n",
"[ -1.466e+04, 5.416e+03, 2.336e+04; ]\n",
"\n",
"Augmented Diagonal Block [1,1]:\n",
"[ 7.027e+03, 1.434e+03, 3.742e+03; ]\n",
"[ 1.434e+03, 1.511e+04, -8.935e+02; ]\n",
"[ 3.742e+03, -8.935e+02, 2.587e+04; ]\n",
"\n",
"Augmented Diagonal Block [2,2]:\n",
"[ 3.128e+04, -5.349e+03, -6.557e+03; ]\n",
"[ -5.349e+03, 1.537e+04, -1.987e+03; ]\n",
"[ -6.557e+03, -1.987e+03, 1.043e+04; ]\n",
"\n",
"Off-Diagonal Block [0,1]:\n",
"[ -6.732e+03, 1.357e+04, -4.327e+03; ]\n",
"[ 3.589e+03, -1.075e+04, 2.158e+04; ]\n",
"[ -1.026e+03, -3.572e+03, -1.124e+04; ]\n",
"\n",
"Off-Diagonal Block [0,2]:\n",
"[ -2.004e+04, -1.143e+03, -3.459e+03; ]\n",
"[ 3.107e+04, -8.167e+03, -1.237e+04; ]\n",
"[ -1.497e+04, -5.631e+03, 4.149e+03; ]\n",
"\n",
"Off-Diagonal Block [1,2]:\n",
"[ 2.527e+03, 2.454e+03, -6.619e+03; ]\n",
"[ -1.484e+04, 3.811e+03, -1.993e+03; ]\n",
"[ 2.085e+04, -1.193e+04, -5.818e+03; ]\n",
"\n",
"Error vector:\n",
"[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n",
"Constant error term: 103876\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.RegularHessianFactorCal3_S2.Downcast(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": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "gtsam", "display_name": "py312",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -305,7 +235,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.13.1" "version": "3.12.6"
} }
}, },
"nbformat": 4, "nbformat": 4,

22
gtsam/slam/doc/SmartProjectionParams.ipynb
View File

@ -48,7 +48,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 2,
"metadata": { "metadata": {
"id": "pip_code", "id": "pip_code",
"tags": [ "tags": [
@ -57,12 +57,16 @@
}, },
"outputs": [], "outputs": [],
"source": [ "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", "cell_type": "code",
"execution_count": 1, "execution_count": 3,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
@ -83,7 +87,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 9, "execution_count": 4,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
@ -159,7 +163,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 10, "execution_count": 5,
"metadata": { "metadata": {
"id": "usage_example_code" "id": "usage_example_code"
}, },
@ -189,13 +193,13 @@
} }
], ],
"source": [ "source": [
"from gtsam import SmartProjectionPose3Factor, Cal3_S2\n", "from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n",
"\n", "\n",
"# Example: Using custom params with a smart factor\n", "# Example: Using custom params with a smart factor\n",
"noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"K = Cal3_S2(500, 500, 0, 320, 240)\n", "K = Cal3_S2(500, 500, 0, 320, 240)\n",
"\n", "\n",
"smart_factor = SmartProjectionPose3Factor(noise, K, custom_params)\n", "smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n",
"print(\"Smart Factor created with custom params:\")\n", "print(\"Smart Factor created with custom params:\")\n",
"smart_factor.print()" "smart_factor.print()"
] ]
@ -203,7 +207,7 @@
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "gtsam", "display_name": "py312",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -217,7 +221,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.13.1" "version": "3.12.6"
} }
}, },
"nbformat": 4, "nbformat": 4,

51
gtsam/slam/doc/SmartProjectionPoseFactor.ipynb
View File

@ -40,7 +40,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "pip_code", "id": "pip_code",
"tags": [ "tags": [
@ -49,27 +49,34 @@
}, },
"outputs": [], "outputs": [],
"source": [ "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", "cell_type": "code",
"execution_count": null, "execution_count": 2,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
"outputs": [], "outputs": [],
"source": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "from gtsam import (\n",
" # SmartProjectionPoseFactor with Cal3_S2 is called SmartProjectionPose3Factor\n", " Values,\n",
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", " Point2,\n",
" SmartProjectionParams, SmartProjectionPose3Factor,\n", " Point3,\n",
" Cal3_S2)\n", " Pose3,\n",
"from gtsam import symbol_shorthand\n", " Rot3,\n",
"import graphviz\n", " NonlinearFactorGraph,\n",
"\n", " SmartProjectionParams,\n",
"X = symbol_shorthand.X # Key for Pose3 variable" " SmartProjectionPoseFactorCal3_S2,\n",
" Cal3_S2,\n",
")\n",
"from gtsam.symbol_shorthand import X # Key for Pose3 variable"
] ]
}, },
{ {
@ -93,7 +100,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 5, "execution_count": 3,
"metadata": { "metadata": {
"id": "create_example_code" "id": "create_example_code"
}, },
@ -125,13 +132,7 @@
"measurement 1, px = \n", "measurement 1, px = \n",
"470\n", "470\n",
"495\n", "495\n",
"noise model = unit (2) \n" "noise model = unit (2) \n",
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"measurement 2, px = \n", "measurement 2, px = \n",
"480\n", "480\n",
"150\n", "150\n",
@ -145,7 +146,7 @@
"smart_params = SmartProjectionParams() # Use default params\n", "smart_params = SmartProjectionParams() # Use default params\n",
"K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", "K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
"\n", "\n",
"smart_factor = SmartProjectionPose3Factor(smart_noise, K, smart_params)\n", "smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n",
"\n", "\n",
"# Add measurements and keys (Pose keys)\n", "# Add measurements and keys (Pose keys)\n",
"smart_factor.add(Point2(150, 505), X(0))\n", "smart_factor.add(Point2(150, 505), X(0))\n",
@ -176,7 +177,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 16, "execution_count": 4,
"metadata": { "metadata": {
"id": "eval_example_code" "id": "eval_example_code"
}, },
@ -236,7 +237,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 5,
"metadata": { "metadata": {
"id": "linearize_example_code" "id": "linearize_example_code"
}, },
@ -295,7 +296,7 @@
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "gtsam", "display_name": "py312",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -309,7 +310,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.13.1" "version": "3.12.6"
} }
}, },
"nbformat": 4, "nbformat": 4,

31
gtsam/slam/slam.i
View File

@ -152,6 +152,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
void serialize() const; void serialize() const;
}; };
// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/slam/SmartFactorBase.h>
template <CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified}>
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;
};
#include <gtsam/slam/SmartProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
/// Linearization mode: what factor to linearize to /// Linearization mode: what factor to linearize to
@ -176,8 +193,11 @@ class SmartProjectionParams {
void print(const std::string& str = "") const; void print(const std::string& str = "") const;
}; };
template <CAMERA = {gtsam::PinholeCamera<gtsam::Cal3_S2>}> template <CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
class SmartProjectionFactor : gtsam::NonlinearFactor { gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified}>
virtual class SmartProjectionFactor : gtsam::SmartFactorBase<CAMERA> {
SmartProjectionFactor(); SmartProjectionFactor();
SmartProjectionFactor( SmartProjectionFactor(
@ -238,7 +258,9 @@ class SmartProjectionFactor : gtsam::NonlinearFactor {
}; };
#include <gtsam/slam/SmartProjectionPoseFactor.h> #include <gtsam/slam/SmartProjectionPoseFactor.h>
template <CALIBRATION> // We are not deriving from SmartProjectionFactor yet - too complicated in wrapper
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K); const CALIBRATION* K);
@ -262,9 +284,6 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
gtsam::TriangulationResult point(const gtsam::Values& values) const; gtsam::TriangulationResult point(const gtsam::Values& values) const;
}; };
typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
SmartProjectionPose3Factor;
// WIP // WIP
// #include <gtsam/geometry/PinholeCamera.h> // #include <gtsam/geometry/PinholeCamera.h>
// typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2; // typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;

4
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -38,10 +38,10 @@ for i=0:length(measurements)
projectionFactorSeenBy = []; projectionFactorSeenBy = [];
if options.includeCameraFactors == 1 if options.includeCameraFactors == 1
for j=1:options.numberOfLandmarks for j=1:options.numberOfLandmarks
SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01); SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2(0.01);
% Use constructor with default values, but express the pose of the % Use constructor with default values, but express the pose of the
% camera as a 90 degree rotation about the X axis % camera as a 90 degree rotation about the X axis
% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ... % SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2( ...
% 1, ... % rankTol % 1, ... % rankTol
% -1, ... % linThreshold % -1, ... % linThreshold
% false, ... % manageDegeneracy % false, ... % manageDegeneracy