IterativeSolver.cpp
Go to the documentation of this file.
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 
22 #include <iostream>
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 /*****************************************************************************/
29 string IterativeOptimizationParameters::getVerbosity() const {
30  return verbosityTranslator(verbosity_);
31 }
32 
33 /*****************************************************************************/
34 void IterativeOptimizationParameters::setVerbosity(const string &src) {
35  verbosity_ = verbosityTranslator(src);
36 }
37 
38 /*****************************************************************************/
40  print(cout);
41 }
42 
43 /*****************************************************************************/
44 void IterativeOptimizationParameters::print(ostream &os) const {
45  os << "IterativeOptimizationParameters:" << endl << "verbosity: "
46  << verbosityTranslator(verbosity_) << endl;
47 }
48 
49 /*****************************************************************************/
50 ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
51  p.print(os);
52  return os;
53 }
54 
55 /*****************************************************************************/
56 IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(
57  const string &src) {
58  string s = src;
59  // Convert to upper case
60  std::transform(s.begin(), s.end(), s.begin(), ::toupper);
61  if (s == "SILENT")
62  return IterativeOptimizationParameters::SILENT;
63  else if (s == "COMPLEXITY")
64  return IterativeOptimizationParameters::COMPLEXITY;
65  else if (s == "ERROR")
66  return IterativeOptimizationParameters::ERROR;
67  /* default is default */
68  else
69  return IterativeOptimizationParameters::SILENT;
70 }
71 
72 /*****************************************************************************/
73 string IterativeOptimizationParameters::verbosityTranslator(
75  if (verbosity == SILENT)
76  return "SILENT";
77  else if (verbosity == COMPLEXITY)
78  return "COMPLEXITY";
79  else if (verbosity == ERROR)
80  return "ERROR";
81  else
82  return "UNKNOWN";
83 }
84 
85 /*****************************************************************************/
87  const KeyInfo* keyInfo, const std::map<Key, Vector>* lambda) {
88  return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
89  lambda ? *lambda : std::map<Key, Vector>());
90 }
91 
92 /*****************************************************************************/
94  const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
95  return optimize(gfg, keyInfo, lambda, keyInfo.x0());
96 }
97 
98 /****************************************************************************/
99 KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) :
100  ordering_(ordering) {
101  initialize(fg);
102 }
103 
104 /****************************************************************************/
106  ordering_(Ordering::Natural(fg)) {
107  initialize(fg);
108 }
109 
110 /****************************************************************************/
112  const map<Key, size_t> colspec = fg.getKeyDimMap();
113  const size_t n = ordering_.size();
114  size_t start = 0;
115 
116  for (size_t i = 0; i < n; ++i) {
117  const Key key = ordering_[i];
118  const auto it_key = colspec.find(key);
119  if (it_key==colspec.end())
120  throw std::runtime_error("KeyInfo: Inconsistency in key-dim map");
121  const size_t dim = it_key->second;
122  this->emplace(key, KeyInfoEntry(i, dim, start));
123  start += dim;
124  }
125  numCols_ = start;
126 }
127 
128 /****************************************************************************/
129 vector<size_t> KeyInfo::colSpec() const {
130  std::vector<size_t> result(size(), 0);
131  for ( const auto &item: *this ) {
132  result[item.second.index] = item.second.dim;
133  }
134  return result;
135 }
136 
137 /****************************************************************************/
140  for ( const auto &item: *this ) {
141  result.emplace(item.first, Vector::Zero(item.second.dim));
142  }
143  return result;
144 }
145 
146 /****************************************************************************/
148  return Vector::Zero(numCols_);
149 }
150 
151 }
152 
const gtsam::Symbol key('X', 0)
int n
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
std::map< Key, size_t > getKeyDimMap() const
static enum @1107 ordering
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Definition: VectorValues.h:185
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
Factor Graph Values.
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
RealScalar s
Linear Factor Graph where all factors are Gaussians.
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
Some support classes for iterative solvers.
KeyInfo()
Default Constructor.
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
traits
Definition: chartTesting.h:28
ofstream os("timeSchurFactors.csv")
float * p
Vector x0vector() const
Return zero Vector of correct dimension.
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
VectorValues x0() const
Return VectorValues with zeros, of correct dimension.
ostream & operator<<(ostream &os, const IterativeOptimizationParameters &p)
std::vector< size_t > colSpec() const
Return a vector of dimensions ordered by ordering()
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void initialize(const GaussianFactorGraph &fg)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:26