1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file IterativeSolver.h
14 * @brief Some support classes for iterative solvers
15 * @date 2010
16 * @author Yong-Dian Jian
17 */
18
19#pragma once
20
21#include <gtsam/inference/Ordering.h>
22#include <gtsam/base/Vector.h>
23
24#include <memory>
25
26#include <iosfwd>
27#include <string>
28#include <map>
29#include <optional>
30
31namespace gtsam {
32
33// Forward declarations
34struct KeyInfoEntry;
35class KeyInfo;
36class GaussianFactorGraph;
37class Values;
38class VectorValues;
39
40/**
41 * parameters for iterative linear solvers
42 */
43class IterativeOptimizationParameters {
44 public:
45 typedef std::shared_ptr<IterativeOptimizationParameters> shared_ptr;
46 enum Verbosity { SILENT = 0, COMPLEXITY, ERROR };
47
48 protected:
49 Verbosity verbosity_;
50
51 public:
52
53 IterativeOptimizationParameters(Verbosity v = SILENT) :
54 verbosity_(v) {
55 }
56
57 virtual ~IterativeOptimizationParameters() {
58 }
59
60 /* utility */
61 inline Verbosity verbosity() const {
62 return verbosity_;
63 }
64 GTSAM_EXPORT std::string getVerbosity() const;
65 GTSAM_EXPORT void setVerbosity(const std::string &s);
66
67 /* matlab interface */
68 GTSAM_EXPORT void print() const;
69
70 /* virtual print function */
71 GTSAM_EXPORT virtual void print(std::ostream &os) const;
72
73 GTSAM_EXPORT virtual bool equals(const IterativeOptimizationParameters &other,
74 double tol = 1e-9) const;
75
76 /* for serialization */
77 GTSAM_EXPORT friend std::ostream &operator<<(
78 std::ostream &os, const IterativeOptimizationParameters &p);
79
80 GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s);
81 GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v);
82};
83
84/**
85 * Base class for Iterative Solvers like SubgraphSolver
86 */
87class IterativeSolver {
88public:
89 typedef std::shared_ptr<IterativeSolver> shared_ptr;
90 IterativeSolver() {
91 }
92 virtual ~IterativeSolver() {
93 }
94
95 /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
96 GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
97 const KeyInfo* = nullptr,
98 const std::map<Key, Vector>* lambda = nullptr);
99
100 /* interface to the nonlinear optimizer, without initial estimate */
101 GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
102 const std::map<Key, Vector> &lambda);
103
104 /* interface to the nonlinear optimizer that the subclasses have to implement */
105 virtual VectorValues optimize(const GaussianFactorGraph &gfg,
106 const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
107 const VectorValues &initial) = 0;
108
109};
110
111/**
112 * Handy data structure for iterative solvers
113 * key to (index, dimension, start)
114 */
115struct GTSAM_EXPORT KeyInfoEntry {
116 size_t index, dim, start;
117 KeyInfoEntry() {
118 }
119 KeyInfoEntry(size_t idx, size_t d, Key start) :
120 index(idx), dim(d), start(start) {
121 }
122};
123
124/**
125 * Handy data structure for iterative solvers
126 */
127class GTSAM_EXPORT KeyInfo: public std::map<Key, KeyInfoEntry> {
128
129public:
130
131 typedef std::map<Key, KeyInfoEntry> Base;
132
133protected:
134
135 Ordering ordering_;
136 size_t numCols_;
137
138 void initialize(const GaussianFactorGraph &fg);
139
140public:
141
142 /// Default Constructor
143 KeyInfo() :
144 numCols_(0) {
145 }
146
147 /// Construct from Gaussian factor graph, use "Natural" ordering
148 KeyInfo(const GaussianFactorGraph &fg);
149
150 /// Construct from Gaussian factor graph and a given ordering
151 KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
152
153 /// Return the total number of columns (scalar variables = sum of dimensions)
154 inline size_t numCols() const {
155 return numCols_;
156 }
157
158 /// Return the ordering
159 inline const Ordering & ordering() const {
160 return ordering_;
161 }
162
163 /// Return a vector of dimensions ordered by ordering()
164 std::vector<size_t> colSpec() const;
165
166 /// Return VectorValues with zeros, of correct dimension
167 VectorValues x0() const;
168
169 /// Return zero Vector of correct dimension
170 Vector x0vector() const;
171
172};
173
174} // \ namespace gtsam
175