Expanded SmartFactor wrapping
Frank Dellaert
parent
513bbb43ed
commit
47812cd899
|
|
@ -66,13 +66,19 @@
|
|||
"source": [
|
||||
"import gtsam\n",
|
||||
"import numpy as np\n",
|
||||
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n",
|
||||
" SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n",
|
||||
" PinholeCameraCal3_S2, Cal3_S2)\n",
|
||||
"from gtsam import symbol_shorthand\n",
|
||||
"import graphviz\n",
|
||||
"\n",
|
||||
"C = symbol_shorthand.C # Key for Camera object"
|
||||
"from gtsam import (\n",
|
||||
" Values,\n",
|
||||
" Point2,\n",
|
||||
" Point3,\n",
|
||||
" Pose3,\n",
|
||||
" Rot3,\n",
|
||||
" 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": [
|
||||
{
|
||||
"ename": "AttributeError",
|
||||
"evalue": "'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'",
|
||||
"output_type": "error",
|
||||
"traceback": [
|
||||
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
|
||||
"\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)",
|
||||
"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",
|
||||
"\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'"
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Smart factor involves 3 measurements.\n",
|
||||
"SmartFactor: 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 = { c0 c1 c2 }\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
|
|
@ -149,7 +178,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 7,
|
||||
"execution_count": 4,
|
||||
"metadata": {
|
||||
"id": "eval_example_code"
|
||||
},
|
||||
|
|
@ -159,7 +188,7 @@
|
|||
"output_type": "stream",
|
||||
"text": [
|
||||
"Triangulated point result:\n",
|
||||
"Status.DEGENERATE\n",
|
||||
"Status.BEHIND_CAMERA\n",
|
||||
"\n",
|
||||
"Triangulation failed, error calculation depends on degeneracyMode.\n"
|
||||
]
|
||||
|
|
@ -188,110 +217,11 @@
|
|||
"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": [
|
||||
"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": {
|
||||
"kernelspec": {
|
||||
"display_name": "gtsam",
|
||||
"display_name": "py312",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
|
|
@ -305,7 +235,7 @@
|
|||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.13.1"
|
||||
"version": "3.12.6"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"execution_count": 2,
|
||||
"metadata": {
|
||||
"id": "pip_code",
|
||||
"tags": [
|
||||
|
|
@ -57,12 +57,16 @@
|
|||
},
|
||||
"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": 3,
|
||||
"metadata": {
|
||||
"id": "imports_code"
|
||||
},
|
||||
|
|
@ -83,7 +87,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 9,
|
||||
"execution_count": 4,
|
||||
"metadata": {
|
||||
"id": "create_example_code"
|
||||
},
|
||||
|
|
@ -159,7 +163,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 10,
|
||||
"execution_count": 5,
|
||||
"metadata": {
|
||||
"id": "usage_example_code"
|
||||
},
|
||||
|
|
@ -189,13 +193,13 @@
|
|||
}
|
||||
],
|
||||
"source": [
|
||||
"from gtsam import SmartProjectionPose3Factor, Cal3_S2\n",
|
||||
"from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n",
|
||||
"\n",
|
||||
"# Example: Using custom params with a smart factor\n",
|
||||
"noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
|
||||
"K = Cal3_S2(500, 500, 0, 320, 240)\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",
|
||||
"smart_factor.print()"
|
||||
]
|
||||
|
|
@ -203,7 +207,7 @@
|
|||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "gtsam",
|
||||
"display_name": "py312",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
|
|
@ -217,7 +221,7 @@
|
|||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.13.1"
|
||||
"version": "3.12.6"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"execution_count": 1,
|
||||
"metadata": {
|
||||
"id": "pip_code",
|
||||
"tags": [
|
||||
|
|
@ -49,27 +49,34 @@
|
|||
},
|
||||
"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": null,
|
||||
"execution_count": 2,
|
||||
"metadata": {
|
||||
"id": "imports_code"
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import gtsam\n",
|
||||
"import numpy as np\n",
|
||||
" # SmartProjectionPoseFactor with Cal3_S2 is called SmartProjectionPose3Factor\n",
|
||||
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n",
|
||||
" SmartProjectionParams, SmartProjectionPose3Factor,\n",
|
||||
" Cal3_S2)\n",
|
||||
"from gtsam import symbol_shorthand\n",
|
||||
"import graphviz\n",
|
||||
"\n",
|
||||
"X = symbol_shorthand.X # Key for Pose3 variable"
|
||||
"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"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -93,7 +100,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 5,
|
||||
"execution_count": 3,
|
||||
"metadata": {
|
||||
"id": "create_example_code"
|
||||
},
|
||||
|
|
@ -125,13 +132,7 @@
|
|||
"measurement 1, px = \n",
|
||||
"470\n",
|
||||
"495\n",
|
||||
"noise model = unit (2) \n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"noise model = unit (2) \n",
|
||||
"measurement 2, px = \n",
|
||||
"480\n",
|
||||
"150\n",
|
||||
|
|
@ -145,7 +146,7 @@
|
|||
"smart_params = SmartProjectionParams() # Use default params\n",
|
||||
"K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
|
||||
"\n",
|
||||
"smart_factor = SmartProjectionPose3Factor(smart_noise, K, smart_params)\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",
|
||||
|
|
@ -176,7 +177,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 16,
|
||||
"execution_count": 4,
|
||||
"metadata": {
|
||||
"id": "eval_example_code"
|
||||
},
|
||||
|
|
@ -236,7 +237,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"execution_count": 5,
|
||||
"metadata": {
|
||||
"id": "linearize_example_code"
|
||||
},
|
||||
|
|
@ -295,7 +296,7 @@
|
|||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "gtsam",
|
||||
"display_name": "py312",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
|
|
@ -309,7 +310,7 @@
|
|||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.13.1"
|
||||
"version": "3.12.6"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
|
|
|
|||
|
|
@ -152,6 +152,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
|
|||
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>
|
||||
|
||||
/// Linearization mode: what factor to linearize to
|
||||
|
|
@ -176,8 +193,11 @@ class SmartProjectionParams {
|
|||
void print(const std::string& str = "") const;
|
||||
};
|
||||
|
||||
template <CAMERA = {gtsam::PinholeCamera<gtsam::Cal3_S2>}>
|
||||
class SmartProjectionFactor : gtsam::NonlinearFactor {
|
||||
template <CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
|
||||
gtsam::PinholeCameraCal3Bundler,
|
||||
gtsam::PinholeCameraCal3Fisheye,
|
||||
gtsam::PinholeCameraCal3Unified}>
|
||||
virtual class SmartProjectionFactor : gtsam::SmartFactorBase<CAMERA> {
|
||||
SmartProjectionFactor();
|
||||
|
||||
SmartProjectionFactor(
|
||||
|
|
@ -238,7 +258,9 @@ class SmartProjectionFactor : gtsam::NonlinearFactor {
|
|||
};
|
||||
|
||||
#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 {
|
||||
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
|
||||
const CALIBRATION* K);
|
||||
|
|
@ -262,9 +284,6 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
|
|||
gtsam::TriangulationResult point(const gtsam::Values& values) const;
|
||||
};
|
||||
|
||||
typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
|
||||
SmartProjectionPose3Factor;
|
||||
|
||||
// WIP
|
||||
// #include <gtsam/geometry/PinholeCamera.h>
|
||||
// typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
|
||||
|
|
|
|||
|
|
@ -38,10 +38,10 @@ for i=0:length(measurements)
|
|||
projectionFactorSeenBy = [];
|
||||
if options.includeCameraFactors == 1
|
||||
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
|
||||
% camera as a 90 degree rotation about the X axis
|
||||
% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ...
|
||||
% SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2( ...
|
||||
% 1, ... % rankTol
|
||||
% -1, ... % linThreshold
|
||||
% false, ... % manageDegeneracy
|
||||
|
|
|
|||
Loading…
Reference in New Issue