From 8bdf5326c022b42fcbf12ae22fb91f94e1447e92 Mon Sep 17 00:00:00 2001
From: p-zach <porterdzach@gmail.com>
Date: Sun, 27 Apr 2025 12:43:10 -0400
Subject: [PATCH] Fix ReferenceFrameFactor

---
 gtsam/slam/doc/ReferenceFrameFactor.ipynb | 38 +++++++----------------
 gtsam/slam/slam.i                         |  2 +-
 2 files changed, 12 insertions(+), 28 deletions(-)

diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb
index b60f9fa4d..a0fd0388d 100644
--- a/gtsam/slam/doc/ReferenceFrameFactor.ipynb
+++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb
@@ -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"
           ]
         }
       ],
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index a0c482fdd..0b03000a9 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -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);