Added GlobalPositioner Class

gtsam/sfm/GlobalPositioner.cpp

@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * 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;
+
+namespace {
+std::mt19937 kPRNG(42);
+}  // namespace
+
+NonlinearFactorGraph GlobalPositioner::buildGraph(
+    const CameraPointDirections &cameraPointDirections) const {
+  return LocationRecovery::buildGraph(cameraPointDirections,
+                                     /*bilinear=*/true);
+}
+
+void GlobalPositioner::addPrior(
+    Key anchorCameraKey, NonlinearFactorGraph *graph,
+    const SharedNoiseModel &priorNoiseModel) const {
+  addAnchorPrior(anchorCameraKey, graph, priorNoiseModel);
+}
+
+Values GlobalPositioner::initializeRandomly(
+    const std::set<Key> &cameraKeys, const std::set<Key> &landmarkKeys,
+    size_t numEdges, std::mt19937 *rng, const Values &initialValues) const {
+  std::set<Key> allKeys(cameraKeys);
+  allKeys.insert(landmarkKeys.begin(), landmarkKeys.end());
+  return LocationRecovery::initializeRandomly(allKeys, numEdges,
+                                              /*bilinear=*/true, rng,
+                                              initialValues);
+}
+
+Values GlobalPositioner::initializeRandomly(
+    const std::set<Key> &cameraKeys, const std::set<Key> &landmarkKeys,
+    size_t numEdges, const Values &initialValues) const {
+  return initializeRandomly(cameraKeys, landmarkKeys, numEdges, &kPRNG,
+                            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);
+  addPrior(anchorCameraKey, &graph);
+  Values initial = initializeRandomly(cameraKeys, landmarkKeys,
+                                      cameraPointDirections.size(),
+                                      initialValues);
+
+  LevenbergMarquardtOptimizer lm(graph, initial, lmParams_);
+  return lm.optimize();
+}

gtsam/sfm/GlobalPositioner.h

@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Build BATA factor graph from camera-to-point directions.
+   * Delegates to LocationRecovery::buildGraph with bilinear=true.
+   */
+  NonlinearFactorGraph buildGraph(
+      const CameraPointDirections &cameraPointDirections) const;
+
+  /**
+   * @brief Anchor one camera at the origin.
+   * Scale is handled by BATA scale variables — no landmark prior needed.
+   */
+  void addPrior(Key anchorCameraKey, NonlinearFactorGraph *graph,
+                const SharedNoiseModel &priorNoiseModel =
+                    noiseModel::Isotropic::Sigma(3, 0.01)) const;
+
+  /**
+   * @brief Initialize cameras, landmarks, and BATA scale variables.
+   */
+  Values initializeRandomly(const std::set<Key> &cameraKeys,
+                            const std::set<Key> &landmarkKeys, size_t numEdges,
+                            std::mt19937 *rng,
+                            const Values &initialValues = Values()) const;
+
+  /// Version with a fixed default RNG seed.
+  Values initializeRandomly(const std::set<Key> &cameraKeys,
+                            const std::set<Key> &landmarkKeys, size_t numEdges,
+                            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);
+}