Added GlobalPositioner Class

gtsam/sfm/GlobalPositioner.cpp

@@ -0,0 +1,54 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file GlobalPositioner.cpp
+ * @author Kathir Gounder
+ * @date 2026
+ * @brief GLOMAP-style joint estimation of camera and landmark positions.
+ */
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/sfm/GlobalPositioner.h>
+
+#include <stdexcept>
+
+using namespace gtsam;
+using namespace std;
+
+Values GlobalPositioner::initializeRandomly(
+    const std::set<Key> &cameraKeys, const std::set<Key> &landmarkKeys,
+    const CameraPointDirections &cameraPointDirections,
+    const Values &initialValues) const {
+  std::set<Key> allKeys(cameraKeys);
+  allKeys.insert(landmarkKeys.begin(), landmarkKeys.end());
+  return LocationRecovery::initializeRandomly(allKeys,
+      cameraPointDirections.size(), true, initialValues);
+}
+
+Values GlobalPositioner::run(
+    const CameraPointDirections &cameraPointDirections,
+    const std::set<Key> &cameraKeys, const std::set<Key> &landmarkKeys,
+    Key anchorCameraKey, const Values &initialValues) const {
+  if (cameraKeys.find(anchorCameraKey) == cameraKeys.end()) {
+    throw std::invalid_argument(
+        "GlobalPositioner::run: anchorCameraKey must be in cameraKeys.");
+  }
+
+  NonlinearFactorGraph graph = buildGraph(cameraPointDirections, true);
+  addAnchorPrior(anchorCameraKey, &graph);
+  Values initial =
+      initializeRandomly(cameraKeys, landmarkKeys, cameraPointDirections,
+                          initialValues);
+
+  LevenbergMarquardtOptimizer lm(graph, initial, lmParams_);
+  return lm.optimize();
+}

gtsam/sfm/GlobalPositioner.h

@@ -0,0 +1,78 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ * @file GlobalPositioner.h
+ * @author Kathir Gounder
+ * @date 2026
+ * @brief GLOMAP-style joint estimation of camera and landmark positions
+ *        from camera-to-point direction measurements.
+ */
+
+#include <gtsam/sfm/LocationRecovery.h>
+
+#include <set>
+
+namespace gtsam {
+
+// Opinionated composition of LocationRecovery for the bipartite
+// camera+landmark estimation problem (GLOMAP-style global positioning).
+// Enforces:
+//   - Explicit separation of camera keys and landmark keys
+//   - Validation that the anchor key is a camera
+//   - BILINEAR (BATA) cost function exclusively
+//   - No DSFMap handling (not applicable to camera-landmark edges)
+//
+// For custom graph configurations (mixed edges, custom gauge, etc.),
+// use LocationRecovery directly.
+//
+// Reference: Pan, L. et al., "Global Structure-from-Motion Revisited,"
+// ECCV 2024.
+class GTSAM_EXPORT GlobalPositioner : public LocationRecovery {
+ public:
+  using CameraPointDirection = BinaryMeasurement<Unit3>;
+  using CameraPointDirections = std::vector<CameraPointDirection>;
+
+  /**
+   * @brief Construct with LM parameters.
+   */
+  explicit GlobalPositioner(const LevenbergMarquardtParams &lmParams)
+      : LocationRecovery(lmParams) {}
+
+  /**
+   * @brief Default constructor.
+   */
+  GlobalPositioner() = default;
+
+  /**
+   * @brief Initialize cameras, landmarks, and BATA scale variables.
+   * Unions cameraKeys and landmarkKeys, derives numEdges from measurements.
+   */
+  Values initializeRandomly(
+      const std::set<Key> &cameraKeys, const std::set<Key> &landmarkKeys,
+      const CameraPointDirections &cameraPointDirections,
+      const Values &initialValues = Values()) const;
+
+  /**
+   * @brief Build graph, fix gauge, initialize, and optimize.
+   *
+   * Enforces bipartite structure: anchorCameraKey must be in cameraKeys.
+   * For custom configurations, use LocationRecovery directly.
+   */
+  Values run(const CameraPointDirections &cameraPointDirections,
+             const std::set<Key> &cameraKeys,
+             const std::set<Key> &landmarkKeys, Key anchorCameraKey,
+             const Values &initialValues = Values()) const;
+};
+
+}  // namespace gtsam

gtsam/sfm/LocationRecovery.cpp

@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file LocationRecovery.cpp
+ * @author Kathir Gounder, Frank Dellaert
+ * @date April 2026
+ * @brief Recover absolute Point3 locations from pairwise Unit3 direction
+ *        measurements.
+ */
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/LocationRecovery.h>
+#include <gtsam/sfm/TranslationFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+
+#include <random>
+#include <set>
+
+using namespace gtsam;
+using namespace std;
+
+namespace {
+// In Wrappers we have no access to this so have a default ready.
+std::mt19937 kPRNG(42);
+}  // namespace
+
+SharedNoiseModel LocationRecovery::convertNoiseModel(
+    const SharedNoiseModel &unit3NoiseModel) {
+  using noiseModel::Isotropic;
+  if (auto isotropic =
+          std::dynamic_pointer_cast<Isotropic>(unit3NoiseModel)) {
+    return noiseModel::Isotropic::Sigma(3, isotropic->sigma());
+  }
+  if (auto robust = std::dynamic_pointer_cast<noiseModel::Robust>(
+          unit3NoiseModel)) {
+    return noiseModel::Robust::Create(robust->robust(),
+                                      convertNoiseModel(robust->noise()));
+  }
+  throw std::runtime_error(
+      "LocationRecovery::convertNoiseModel: only isotropic (optionally "
+      "robust-wrapped) noise model supported.");
+}
+
+NonlinearFactorGraph LocationRecovery::buildGraph(
+    const DirectionEdges &edges, bool bilinear) const {
+  NonlinearFactorGraph graph;
+  uint64_t i = 0;
+  for (const auto &edge : edges) {
+    auto model = convertNoiseModel(edge.noiseModel());
+    if (bilinear) {
+      graph.emplace_shared<BilinearAngleTranslationFactor>(
+          edge.key1(), edge.key2(), Symbol('S', i), edge.measured(), model);
+    } else {
+      graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
+                                              edge.measured(), model);
+    }
+    i++;
+  }
+  return graph;
+}
+
+void LocationRecovery::addAnchorPrior(
+    Key anchorKey, NonlinearFactorGraph *graph,
+    const SharedNoiseModel &priorNoiseModel) const {
+  graph->addPrior<Point3>(anchorKey, Point3(0, 0, 0), priorNoiseModel);
+}
+
+Values LocationRecovery::initializeRandomly(
+    const std::set<Key> &keys, size_t numEdges, bool bilinear,
+    std::mt19937 *rng, const Values &initialValues) const {
+  uniform_real_distribution<double> randomVal(-1, 1);
+  Values initial;
+
+  auto insert = [&](Key j) {
+    if (initial.exists(j)) return;
+    if (initialValues.exists(j)) {
+      initial.insert<Point3>(j, initialValues.at<Point3>(j));
+    } else {
+      initial.insert<Point3>(
+          j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
+    }
+  };
+
+  for (const Key &key : keys) {
+    insert(key);
+  }
+
+  if (bilinear) {
+    for (uint64_t i = 0; i < numEdges; i++) {
+      initial.insert<Vector1>(Symbol('S', i), Vector1(1.0));
+    }
+  }
+
+  return initial;
+}
+
+Values LocationRecovery::initializeRandomly(
+    const std::set<Key> &keys, size_t numEdges, bool bilinear,
+    const Values &initialValues) const {
+  return initializeRandomly(keys, numEdges, bilinear, &kPRNG, initialValues);
+}

gtsam/sfm/LocationRecovery.h

@@ -0,0 +1,116 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ * @file LocationRecovery.h
+ * @author Kathir Gounder, Akshay Krishnan, Frank Dellaert
+ * @date April 2026
+ * @brief Recover absolute Point3 locations from pairwise Unit3 direction
+ *        measurements, using either chordal or bilinear (BATA) cost functions.
+ */
+
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/BinaryMeasurement.h>
+
+#include <random>
+#include <set>
+#include <vector>
+
+namespace gtsam {
+
+// Recover absolute Point3 locations from pairwise Unit3 direction
+// measurements. This base class is unopinionated about graph structure:
+// measurements can connect any pair of Point3 unknowns (cameras, landmarks,
+// sensors, etc.). Subclasses add problem-specific opinions:
+//   - GlobalPositioner: bipartite camera+landmark, anchor-only gauge.
+//   - TranslationRecovery: homogeneous camera-camera, DSFMap, two-key gauge.
+//
+// Supports two cost functions via the bilinear flag:
+//   false — TranslationFactor: normalized(Tb-Ta) - measured  (chordal)
+//   true  — BilinearAngleTranslationFactor: scale*(Tb-Ta) - measured  (BATA)
+class GTSAM_EXPORT LocationRecovery {
+ public:
+  using DirectionEdges = std::vector<BinaryMeasurement<Unit3>>;
+
+ protected:
+  LevenbergMarquardtParams lmParams_;
+
+  /// Convert Unit3 (2D manifold) noise to Point3 (3D ambient) noise.
+  static SharedNoiseModel convertNoiseModel(
+      const SharedNoiseModel &unit3NoiseModel);
+
+ public:
+  /**
+   * @brief Construct with LM parameters.
+   */
+  explicit LocationRecovery(const LevenbergMarquardtParams &lmParams)
+      : lmParams_(lmParams) {}
+
+  /**
+   * @brief Default constructor.
+   */
+  LocationRecovery() = default;
+
+  /**
+   * @brief Build factor graph from direction measurements.
+   *
+   * If bilinear is true, uses BilinearAngleTranslationFactor (BATA) with
+   * per-observation scale variables Symbol('S', i). Otherwise uses the
+   * chordal TranslationFactor.
+   *
+   * @param edges    Unit3 direction measurements between Point3 unknowns.
+   * @param bilinear if true, use BATA factors (default: true).
+   * @return NonlinearFactorGraph
+   */
+  NonlinearFactorGraph buildGraph(const DirectionEdges &edges,
+                                  bool bilinear = true) const;
+
+  /**
+   * @brief Add a prior pinning one key to the origin.
+   *
+   * Fixes 3 translation DOF. Does not assume the key is a camera or
+   * landmark — it just pins a Point3.
+   */
+  void addAnchorPrior(
+      Key anchorKey, NonlinearFactorGraph *graph,
+      const SharedNoiseModel &priorNoiseModel =
+          noiseModel::Isotropic::Sigma(3, 0.01)) const;
+
+  /**
+   * @brief Random initialization of Point3 keys and BATA scale variables.
+   *
+   * Point3 keys are initialized uniformly in [-1, 1]^3. If bilinear is
+   * true, also creates numEdges scale variables initialized to 1.0.
+   *
+   * @param keys          Point3 keys to initialize.
+   * @param numEdges      number of edges (for scale variable count).
+   * @param bilinear      whether to create BATA scale variables.
+   * @param rng           random number generator.
+   * @param initialValues optional seed values for specific keys.
+   * @return Values
+   */
+  Values initializeRandomly(const std::set<Key> &keys, size_t numEdges,
+                             bool bilinear, std::mt19937 *rng,
+                             const Values &initialValues = Values()) const;
+
+  /**
+   * @brief Version of initializeRandomly with a fixed seed.
+   */
+  Values initializeRandomly(const std::set<Key> &keys, size_t numEdges,
+                             bool bilinear,
+                             const Values &initialValues = Values()) const;
+};
+
+}  // namespace gtsam