SfmLevenbergMarquardt.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010-2026, 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
 
#include <gtsam/dllexport.h>
#include <gtsam/linear/MultifrontalSolver.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/sfm/SfmEliminationMode.h>
 
#include <memory>
#include <string>
 
namespace gtsam {
 
class SfmLevenbergMarquardtOptimizer;

class GTSAM_EXPORT SfmLevenbergMarquardtParams
    : public LevenbergMarquardtParams {
 public:
  using OptimizerType = SfmLevenbergMarquardtOptimizer;

  SfmEliminationMode eliminationMode = SfmEliminationMode::Full;

  SfmLevenbergMarquardtParams();

  static SfmLevenbergMarquardtParams legacyDefaults();
  static SfmLevenbergMarquardtParams ceresDefaults();
 
  SfmEliminationMode getEliminationMode() const { return eliminationMode; }
  void setEliminationMode(SfmEliminationMode mode) { eliminationMode = mode; }
 
  void print(const std::string& str = "") const override;
  bool equals(const SfmLevenbergMarquardtParams& other,
              double tol = 1e-9) const;

  std::shared_ptr<NonlinearOptimizerParams> clone() const {
    return std::make_shared<SfmLevenbergMarquardtParams>(*this);
  }
};

class GTSAM_EXPORT SfmLevenbergMarquardtOptimizer
    : public LevenbergMarquardtOptimizer {
 public:
  static Ordering CreateReducedOrdering(const NonlinearFactorGraph& graph,
                                        const Values& initialValues);

  static Ordering CreateSchurOrdering(const NonlinearFactorGraph& graph,
                                      const Ordering& reducedOrdering);
 
  SfmLevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
                                 const Values& initialValues,
                                 const SfmLevenbergMarquardtParams& params =
                                     SfmLevenbergMarquardtParams());
  ~SfmLevenbergMarquardtOptimizer() override;
 
  const SfmLevenbergMarquardtParams& sfmParams() const { return sfmParams_; }
 
 protected:
  VectorValues solve(const GaussianFactorGraph& graph,
                     const NonlinearOptimizerParams& params) const override;
 
  bool ensureMultifrontalSolver(const NonlinearOptimizerParams& params,
                                const Values& values) const override;
 
 private:
  struct SchurState;
 
  SfmLevenbergMarquardtParams sfmParams_;
  mutable std::unique_ptr<SchurState> schurState_;
};
 
}  // namespace gtsam