Merge branch 'develop' into improved-hybrid-api

CMakeLists.txt

@@ -1,4 +1,16 @@
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.5)
+if (POLICY CMP0082)
+    cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately
+endif()
+if (POLICY CMP0102)
+cmake_policy(SET CMP0102 NEW) # set policy on advanced variables and cmake cache
+endif()
+if (POLICY CMP0156)
+cmake_policy(SET CMP0156 NEW) # new linker strategies
+endif()
+if (POLICY CMP0167)
+cmake_policy(SET CMP0167 OLD) # Don't complain about boost
+endif()
 
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 4)

CppUnitLite/CMakeLists.txt

@@ -6,6 +6,7 @@ file(GLOB cppunitlite_src "*.cpp")
 add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
 list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+target_compile_features(CppUnitLite PUBLIC ${GTSAM_COMPILE_FEATURES_PUBLIC})
 
 gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
 

cmake/HandleBoost.cmake

@@ -25,7 +25,7 @@ endif()
 set(BOOST_FIND_MINIMUM_VERSION 1.65)
 set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
 
-find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
+find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} REQUIRED)
 
 # Required components
 if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR

cmake/example_cmake_find_gtsam/CMakeLists.txt

@@ -1,7 +1,7 @@
 # This file shows how to build and link a user project against GTSAM using CMake
 ###################################################################################
 # To create your own project, replace "example" with the actual name of your project
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.5)
 project(example CXX)
 
 # Find GTSAM, either from a local build, or from a Debian/Ubuntu package.

gtsam/hybrid/GaussianMixture.cpp

@@ -330,19 +330,28 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
   return DecisionTree<Key, double>(conditionals_, probFunc);
 }
 
+/* ************************************************************************* */
+double GaussianMixture::conditionalError(
+    const GaussianConditional::shared_ptr &conditional,
+    const VectorValues &continuousValues) const {
+  // Check if valid pointer
+  if (conditional) {
+    return conditional->error(continuousValues) +  //
+           logConstant_ - conditional->logNormalizationConstant();
+  } else {
+    // If not valid, pointer, it means this conditional was pruned,
+    // so we return maximum error.
+    // This way the negative exponential will give
+    // a probability value close to 0.0.
+    return std::numeric_limits<double>::max();
+  }
+}
+
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
     const VectorValues &continuousValues) const {
   auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) {
-    // Check if valid pointer
+    return conditionalError(conditional, continuousValues);
-    if (conditional) {
-      return conditional->error(continuousValues) +  //
-             logConstant_ - conditional->logNormalizationConstant();
-    } else {
-      // If not valid, pointer, it means this conditional was pruned,
-      // so we return maximum error.
-      return std::numeric_limits<double>::max();
-    }
   };
   DecisionTree<Key, double> error_tree(conditionals_, errorFunc);
   return error_tree;
@@ -350,33 +359,9 @@ AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
 
 /* *******************************************************************************/
 double GaussianMixture::error(const HybridValues &values) const {
-  // Check if discrete keys in discrete assignment are
-  // present in the GaussianMixture
-  KeyVector dKeys = this->discreteKeys_.indices();
-  bool valid_assignment = false;
-  for (auto &&kv : values.discrete()) {
-    if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) {
-      valid_assignment = true;
-      break;
-    }
-  }
-
-  // The discrete assignment is not valid so we throw an error.
-  if (!valid_assignment) {
-    throw std::runtime_error(
-        "Invalid discrete values in values. Not all discrete keys specified.");
-  }
-
   // Directly index to get the conditional, no need to build the whole tree.
   auto conditional = conditionals_(values.discrete());
-  if (conditional) {
+  return conditionalError(conditional, values.continuous());
-    return conditional->error(values.continuous()) +  //
-           logConstant_ - conditional->logNormalizationConstant();
-  } else {
-    // If not valid, pointer, it means this conditional was pruned,
-    // so we return maximum error.
-    return std::numeric_limits<double>::max();
-  }
 }
 
 /* *******************************************************************************/

gtsam/hybrid/GaussianMixture.h

@@ -256,6 +256,10 @@ class GTSAM_EXPORT GaussianMixture
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;
 
+  /// Helper method to compute the error of a conditional.
+  double conditionalError(const GaussianConditional::shared_ptr &conditional,
+                          const VectorValues &continuousValues) const;
+
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;

gtsam/hybrid/HybridFactorGraph.cpp

@@ -49,15 +49,6 @@ KeySet HybridFactorGraph::discreteKeySet() const {
   return keys;
 }
 
-/* ************************************************************************* */
-std::unordered_map<Key, DiscreteKey> HybridFactorGraph::discreteKeyMap() const {
-  std::unordered_map<Key, DiscreteKey> result;
-  for (const DiscreteKey& k : discreteKeys()) {
-    result[k.first] = k;
-  }
-  return result;
-}
-
 /* ************************************************************************* */
 const KeySet HybridFactorGraph::continuousKeySet() const {
   KeySet keys;

gtsam/hybrid/HybridFactorGraph.h

@@ -38,7 +38,7 @@ using SharedFactor = std::shared_ptr<Factor>;
 class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
  public:
   using Base = FactorGraph<Factor>;
-  using This = HybridFactorGraph;              ///< this class
+  using This = HybridFactorGraph;            ///< this class
   using shared_ptr = std::shared_ptr<This>;  ///< shared_ptr to This
 
   using Values = gtsam::Values;  ///< backwards compatibility
@@ -69,9 +69,6 @@ class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
   /// Get all the discrete keys in the factor graph, as a set of Keys.
   KeySet discreteKeySet() const;
 
-  /// Get a map from Key to corresponding DiscreteKey.
-  std::unordered_map<Key, DiscreteKey> discreteKeyMap() const;
-
   /// Get all the continuous keys in the factor graph.
   const KeySet continuousKeySet() const;
 

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -312,8 +312,8 @@ using Result = std::pair<std::shared_ptr<GaussianConditional>,
                          GaussianMixtureFactor::sharedFactor>;
 
 /**
- * Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
+ * Compute the probability p(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
- * from the residual error at the mean μ.
+ * from the residual error ||b||^2 at the mean μ.
  * The residual error contains no keys, and only
  * depends on the discrete separator if present.
  */
@@ -523,19 +523,9 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
         std::inserter(continuousSeparator, continuousSeparator.begin()));
 
     // Similarly for the discrete separator.
-    auto discreteKeySet = factors.discreteKeySet();
+    // Since we eliminate all continuous variables first,
-    // Use set-difference to get the difference in keys
+    // the discrete separator will be *all* the discrete keys.
-    KeySet discreteSeparatorSet;
+    std::set<DiscreteKey> discreteSeparator = factors.discreteKeys();
-    std::set_difference(
-        discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(),
-        frontalKeysSet.end(),
-        std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin()));
-    // Convert from set of keys to set of DiscreteKeys
-    std::set<DiscreteKey> discreteSeparator;
-    auto discreteKeyMap = factors.discreteKeyMap();
-    for (auto key : discreteSeparatorSet) {
-      discreteSeparator.insert(discreteKeyMap.at(key));
-    }
 
     return hybridElimination(factors, frontalKeys, continuousSeparator,
                              discreteSeparator);

gtsam/hybrid/tests/testGaussianMixtureFactor.cpp

@@ -434,12 +434,12 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0,
 
 /* ************************************************************************* */
 /**
- * Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
+ * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
  *
  * P(f01|x1,x0,m1) has different means and same covariance.
  *
  * Converting to a factor graph gives us
- * P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
+ * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
  *
  * If we only have a measurement on z0, then
  * the probability of m1 should be 0.5/0.5.
@@ -488,12 +488,12 @@ TEST(GaussianMixtureFactor, TwoStateModel) {
 
 /* ************************************************************************* */
 /**
- * Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
+ * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
  *
  * P(f01|x1,x0,m1) has different means and different covariances.
  *
  * Converting to a factor graph gives us
- * P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
+ * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
  *
  * If we only have a measurement on z0, then
  * the P(m1) should be 0.5/0.5.

gtsam/hybrid/tests/testMixtureFactor.cpp

@@ -118,156 +118,6 @@ TEST(MixtureFactor, Dim) {
   EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
 }
 
-/* ************************************************************************* */
-// Test components with differing means
-TEST(MixtureFactor, DifferentMeans) {
-  DiscreteKey m1(M(1), 2), m2(M(2), 2);
-
-  Values values;
-  double x1 = 0.0, x2 = 1.75, x3 = 2.60;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-  values.insert(X(3), x3);
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
-
-  auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0);
-  auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
-  HybridNonlinearFactorGraph hnfg;
-  hnfg.push_back(mixtureFactor);
-
-  f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0);
-  f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors23{f0, f1};
-  hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23));
-
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise);
-  hnfg.push_back(prior);
-
-  hnfg.emplace_shared<PriorFactor<double>>(X(2), 2.0, prior_noise);
-
-  auto hgfg = hnfg.linearize(values);
-  auto bn = hgfg->eliminateSequential();
-  HybridValues actual = bn->optimize();
-
-  HybridValues expected(
-      VectorValues{
-          {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
-      DiscreteValues{{M(1), 1}, {M(2), 0}});
-
-  EXPECT(assert_equal(expected, actual));
-
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 0}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 0}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-  {
-    DiscreteValues dv{{M(1), 1}, {M(2), 1}};
-    VectorValues cont = bn->optimize(dv);
-    double error = bn->error(HybridValues(cont, dv));
-    // regression
-    EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
-  }
-}
-
-/* ************************************************************************* */
-// Test components with differing covariances
-TEST(MixtureFactor, DifferentCovariances) {
-  DiscreteKey m1(M(1), 2);
-
-  Values values;
-  double x1 = 1.0, x2 = 1.0;
-  values.insert(X(1), x1);
-  values.insert(X(2), x2);
-
-  double between = 0.0;
-
-  auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
-  auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
-  auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
-
-  auto f0 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
-  auto f1 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
-
-  // Create via toFactorGraph
-  using symbol_shorthand::Z;
-  Matrix H0_1, H0_2, H1_1, H1_2;
-  Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
-  std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H0_1 /*Sp1*/},
-                                                {X(2), H0_2 /*Tp2*/}};
-
-  Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
-  std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
-                                                //
-                                                {X(1), H1_1 /*Sp1*/},
-                                                {X(2), H1_2 /*Tp2*/}};
-  auto gm = new gtsam::GaussianMixture(
-      {Z(1)}, {X(1), X(2)}, {m1},
-      {std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
-       std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
-  gtsam::HybridBayesNet bn;
-  bn.emplace_back(gm);
-
-  gtsam::VectorValues measurements;
-  measurements.insert(Z(1), gtsam::Z_1x1);
-  // Create FG with single GaussianMixtureFactor
-  HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
-
-  // Linearized prior factor on X1
-  auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
-  mixture_fg.push_back(prior);
-
-  auto hbn = mixture_fg.eliminateSequential();
-
-  VectorValues cv;
-  cv.insert(X(1), Vector1(0.0));
-  cv.insert(X(2), Vector1(0.0));
-  // P(m1) = [0.5, 0.5], so we should pick 0
-  DiscreteValues dv;
-  dv.insert({M(1), 0});
-  HybridValues expected_values(cv, dv);
-
-  HybridValues actual_values = hbn->optimize();
-  EXPECT(assert_equal(expected_values, actual_values));
-
-  // Check that we get different error values at the MLE point μ.
-  AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
-
-  HybridValues hv0(cv, DiscreteValues{{M(1), 0}});
-  HybridValues hv1(cv, DiscreteValues{{M(1), 1}});
-
-  AlgebraicDecisionTree<Key> expectedErrorTree(m1, 9.90348755254,
-                                               0.69314718056);
-  EXPECT(assert_equal(expectedErrorTree, errorTree));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/linear/IterativeSolver.cpp

@@ -46,6 +46,12 @@ void IterativeOptimizationParameters::print(ostream &os) const {
       << verbosityTranslator(verbosity_) << endl;
 }
 
+/*****************************************************************************/
+bool IterativeOptimizationParameters::equals(
+    const IterativeOptimizationParameters &other, double tol) const {
+  return verbosity_ == other.verbosity();
+}
+
 /*****************************************************************************/
 ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
   p.print(os);

gtsam/linear/IterativeSolver.h

@@ -41,15 +41,14 @@ class VectorValues;
  * parameters for iterative linear solvers
  */
 class IterativeOptimizationParameters {
-
+ public:
-public:
-
   typedef std::shared_ptr<IterativeOptimizationParameters> shared_ptr;
-  enum Verbosity {
+  enum Verbosity { SILENT = 0, COMPLEXITY, ERROR };
-    SILENT = 0, COMPLEXITY, ERROR
-  } verbosity_;
 
-public:
+ protected:
+  Verbosity verbosity_;
+
+ public:
 
   IterativeOptimizationParameters(Verbosity v = SILENT) :
       verbosity_(v) {
@@ -71,6 +70,9 @@ public:
   /* virtual print function */
   GTSAM_EXPORT virtual void print(std::ostream &os) const;
 
+  GTSAM_EXPORT virtual bool equals(const IterativeOptimizationParameters &other,
+                                   double tol = 1e-9) const;
+
   /* for serialization */
   GTSAM_EXPORT friend std::ostream &operator<<(
       std::ostream &os, const IterativeOptimizationParameters &p);

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -236,9 +236,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
     if (currentState->iterations == 0) {
       cout << "iter      cost      cost_change    lambda  success iter_time" << endl;
     }
-    cout << setw(4) << currentState->iterations << " " << setw(8) << newError << " " << setw(3) << setprecision(2)
+    cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2)
-         << costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4)
+         << costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6)
-         << systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl;
+         << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl;
   }
   if (step_is_successful) {
     // we have successfully decreased the cost and we have good modelFidelity

gtsam/nonlinear/NonlinearOptimizerParams.cpp

@@ -123,6 +123,28 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
   std::cout.flush();
 }
 
+/* ************************************************************************* */
+bool NonlinearOptimizerParams::equals(const NonlinearOptimizerParams& other,
+                                      double tol) const {
+  // Check for equality of shared ptrs
+  bool iterative_params_equal = iterativeParams == other.iterativeParams;
+  // Check equality of components
+  if (iterativeParams && other.iterativeParams) {
+    iterative_params_equal = iterativeParams->equals(*other.iterativeParams);
+  } else {
+    // Check if either is null. If both are null, then true
+    iterative_params_equal = !iterativeParams && !other.iterativeParams;
+  }
+
+  return maxIterations == other.getMaxIterations() &&
+         std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol &&
+         std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol &&
+         std::abs(errorTol - other.getErrorTol()) <= tol &&
+         verbosityTranslator(verbosity) == other.getVerbosity() &&
+         orderingType == other.orderingType && ordering == other.ordering &&
+         linearSolverType == other.linearSolverType && iterative_params_equal;
+}
+
 /* ************************************************************************* */
 std::string NonlinearOptimizerParams::linearSolverTranslator(
     LinearSolverType linearSolverType) const {

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -114,16 +114,7 @@ public:
 
   virtual void print(const std::string& str = "") const;
 
-  bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
+  bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const;
-    return maxIterations == other.getMaxIterations()
-        && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
-        && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
-        && std::abs(errorTol - other.getErrorTol()) <= tol
-        && verbosityTranslator(verbosity) == other.getVerbosity();
-    //  && orderingType.equals(other.getOrderingType()_;
-    // && linearSolverType == other.getLinearSolverType();
-    // TODO: check ordering, iterativeParams, and iterationsHook
-  }
 
   inline bool isMultifrontal() const {
     return (linearSolverType == MULTIFRONTAL_CHOLESKY)

python/gtsam/examples/SelfCalibrationExample.py

@@ -0,0 +1,122 @@
+# pylint: disable=unused-import,consider-using-from-import,invalid-name,no-name-in-module,no-member,missing-function-docstring,too-many-locals
+"""
+Transcription of SelfCalibrationExample.cpp
+"""
+import math
+
+from gtsam import Cal3_S2
+from gtsam.noiseModel import Diagonal, Isotropic
+
+# SFM-specific factors
+from gtsam import GeneralSFMFactor2Cal3_S2  # does calibration !
+from gtsam import PinholeCameraCal3_S2
+
+# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
+from gtsam import Point2
+from gtsam import Point3, Pose3, Rot3
+
+# Inference and optimization
+from gtsam import NonlinearFactorGraph, DoglegOptimizer, Values
+from gtsam.symbol_shorthand import K, L, X
+
+
+# this is a direct translation of examples/SFMData.h
+# which is slightly different from python/gtsam/examples/SFMdata.py.
+def createPoints() -> list[Point3]:
+    """
+    Create the set of ground-truth landmarks
+    """
+    return [
+        Point3(10.0, 10.0, 10.0),
+        Point3(-10.0, 10.0, 10.0),
+        Point3(-10.0, -10.0, 10.0),
+        Point3(10.0, -10.0, 10.0),
+        Point3(10.0, 10.0, -10.0),
+        Point3(-10.0, 10.0, -10.0),
+        Point3(-10.0, -10.0, -10.0),
+        Point3(10.0, -10.0, -10.0),
+    ]
+
+
+def createPoses(
+    init: Pose3 = Pose3(Rot3.Ypr(math.pi / 2, 0, -math.pi / 2), Point3(30, 0, 0)),
+    delta: Pose3 = Pose3(
+        Rot3.Ypr(0, -math.pi / 4, 0),
+        Point3(math.sin(math.pi / 4) * 30, 0, 30 * (1 - math.sin(math.pi / 4))),
+    ),
+    steps: int = 8,
+) -> list[Pose3]:
+    """
+    Create the set of ground-truth poses
+    Default values give a circular trajectory,
+    radius 30 at pi/4 intervals, always facing the circle center
+    """
+    poses: list[Pose3] = []
+    poses.append(init)
+    for i in range(1, steps):
+        poses.append(poses[i - 1].compose(delta))
+    return poses
+
+
+def main() -> None:
+    # Create the set of ground-truth
+    points: list[Point3] = createPoints()
+    poses: list[Pose3] = createPoses()
+
+    # Create the factor graph
+    graph = NonlinearFactorGraph()
+
+    # Add a prior on pose x1.
+    # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+    poseNoise = Diagonal.Sigmas([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])
+    graph.addPriorPose3(X(0), poses[0], poseNoise)
+
+    # Simulated measurements from each camera pose, adding them to the factor graph
+    Kcal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+    measurementNoise = Isotropic.Sigma(2, 1.0)
+    for i, pose in enumerate(poses):
+        for j, point in enumerate(points):
+            camera = PinholeCameraCal3_S2(pose, Kcal)
+            measurement: Point2 = camera.project(point)
+            # The only real difference with the Visual SLAM example is that here we
+            # use a different factor type, that also calculates the Jacobian with
+            # respect to calibration
+            graph.add(
+                GeneralSFMFactor2Cal3_S2(
+                    measurement,
+                    measurementNoise,
+                    X(i),
+                    L(j),
+                    K(0),
+                )
+            )
+
+    # Add a prior on the position of the first landmark.
+    pointNoise = Isotropic.Sigma(3, 0.1)
+    graph.addPriorPoint3(L(0), points[0], pointNoise)  # add directly to graph
+
+    # Add a prior on the calibration.
+    calNoise = Diagonal.Sigmas([500, 500, 0.1, 100, 100])
+    graph.addPriorCal3_S2(K(0), Kcal, calNoise)
+
+    # Create the initial estimate to the solution
+    # now including an estimate on the camera calibration parameters
+    initialEstimate = Values()
+    initialEstimate.insert(K(0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0))
+    for i, pose in enumerate(poses):
+        initialEstimate.insert(
+            X(i),
+            pose.compose(
+                Pose3(Rot3.Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))
+            ),
+        )
+    for j, point in enumerate(points):
+        initialEstimate.insert(L(j), point + Point3(-0.25, 0.20, 0.15))
+
+    # Optimize the graph and print results
+    result: Values = DoglegOptimizer(graph, initialEstimate).optimize()
+    result.print("Final results:\n")
+
+
+if __name__ == "__main__":
+    main()