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

Fix ReferenceFrameFactor

release/4.3a0
p-zach 2025-04-27 12:43:10 -04:00
parent 2cfd2e00a5
commit 8bdf5326c0
2 changed files with 12 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/slam/doc/ReferenceFrameFactor.ipynb
View File

@ -61,7 +61,7 @@
"name": "stdout",
"output_type": "stream",
"text": [
"[]\n"
"['ReferenceFrameFactorPoint3Pose3']\n"
]
}
],
@ -72,34 +72,18 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 3,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 5\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# The Python wrapper creates specific instantiations\u001b[39;00m\n\u001b[1;32m----> 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m ReferenceFrameFactorPoint3Pose3\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 8\u001b[0m L \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mL \u001b[38;5;66;03m# Global landmark\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n",
"# The Python wrapper creates specific instantiations\n",
"from gtsam import ReferenceFrameFactorPoint3Pose3\n",
"from gtsam import symbol_shorthand\n",
"\n",
"L = symbol_shorthand.L # Global landmark\n",
"T = symbol_shorthand.T # Transform global_T_local\n",
"l = symbol_shorthand.l # Local landmark"
"from gtsam.symbol_shorthand import L, T, O"
]
},
{
@ -123,7 +107,7 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 4,
"metadata": {
"id": "create_example_code"
},
@ -132,15 +116,15 @@
"name": "stdout",
"output_type": "stream",
"text": [
"ReferenceFrameFactor: : ReferenceFrameFactor(Global: L0, Transform: T0, Local: l0)\n",
" noise model: diagonal sigmas [0.1; 0.1; 0.1];\n"
"ReferenceFrameFactor: : ReferenceFrameFactor(Global: l0, Transform: t0, Local: o0)\n",
"isotropic dim=3 sigma=0.1\n"
]
}
],
"source": [
"global_landmark_key = L(0)\n",
"transform_key = T(0)\n",
"local_landmark_key = l(0)\n",
"local_landmark_key = O(0)\n",
"\n",
"# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)\n",
"noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev\n",
@ -173,7 +157,7 @@
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 5,
"metadata": {
"id": "eval_example_code"
},
@ -184,8 +168,8 @@
"text": [
"Expected local landmark: [ 2. -4. 1.]\n",
"\n",
"Error at ground truth: [-0. 0. 0.] (Should be zero)\n",
"Error with noisy local landmark: [-1. 1. -0.5]\n"
"Error at ground truth: 4500.0 (Should be zero)\n",
"Error with noisy local landmark: 4621.125\n"
]
}
],

2
gtsam/slam/slam.i
View File

@ -342,7 +342,7 @@ typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>
GenericStereoFactor3D;
#include <gtsam/slam/ReferenceFrameFactor.h>
template<LANDMARK, POSE>
template<LANDMARK = {gtsam::Point3}, POSE = {gtsam::Pose3}>
class ReferenceFrameFactor : gtsam::NoiseModelFactor {
ReferenceFrameFactor(gtsam::Key globalKey, gtsam::Key transKey,
gtsam::Key localKey, const gtsam::noiseModel::Base* model);