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 bool IterativeOptimizationParameters::equals(
51  const IterativeOptimizationParameters &other, double tol) const {
52  return verbosity_ == other.verbosity();
53 }
54 
55 /*****************************************************************************/
56 ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
57  p.print(os);
58  return os;
59 }
60 
61 /*****************************************************************************/
62 IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(
63  const string &src) {
64  string s = src;
65  // Convert to upper case
66  std::transform(s.begin(), s.end(), s.begin(), ::toupper);
67  if (s == "SILENT")
68  return IterativeOptimizationParameters::SILENT;
69  else if (s == "COMPLEXITY")
70  return IterativeOptimizationParameters::COMPLEXITY;
71  else if (s == "ERROR")
72  return IterativeOptimizationParameters::ERROR;
73  /* default is default */
74  else
75  return IterativeOptimizationParameters::SILENT;
76 }
77 
78 /*****************************************************************************/
79 string IterativeOptimizationParameters::verbosityTranslator(
81  if (verbosity == SILENT)
82  return "SILENT";
83  else if (verbosity == COMPLEXITY)
84  return "COMPLEXITY";
85  else if (verbosity == ERROR)
86  return "ERROR";
87  else
88  return "UNKNOWN";
89 }
90 
91 /*****************************************************************************/
93  const KeyInfo* keyInfo, const std::map<Key, Vector>* lambda) {
94  return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
95  lambda ? *lambda : std::map<Key, Vector>());
96 }
97 
98 /*****************************************************************************/
100  const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
101  return optimize(gfg, keyInfo, lambda, keyInfo.x0());
102 }
103 
104 /****************************************************************************/
105 KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) :
106  ordering_(ordering) {
107  initialize(fg);
108 }
109 
110 /****************************************************************************/
112  ordering_(Ordering::Natural(fg)) {
113  initialize(fg);
114 }
115 
116 /****************************************************************************/
118  const map<Key, size_t> colspec = fg.getKeyDimMap();
119  const size_t n = ordering_.size();
120  size_t start = 0;
121 
122  for (size_t i = 0; i < n; ++i) {
123  const Key key = ordering_[i];
124  const auto it_key = colspec.find(key);
125  if (it_key==colspec.end())
126  throw std::runtime_error("KeyInfo: Inconsistency in key-dim map");
127  const size_t dim = it_key->second;
128  this->emplace(key, KeyInfoEntry(i, dim, start));
129  start += dim;
130  }
131  numCols_ = start;
132 }
133 
134 /****************************************************************************/
135 vector<size_t> KeyInfo::colSpec() const {
136  std::vector<size_t> result(size(), 0);
137  for ( const auto &item: *this ) {
138  result[item.second.index] = item.second.dim;
139  }
140  return result;
141 }
142 
143 /****************************************************************************/
146  for ( const auto &item: *this ) {
147  result.emplace(item.first, Vector::Zero(item.second.dim));
148  }
149  return result;
150 }
151 
152 /****************************************************************************/
154  return Vector::Zero(numCols_);
155 }
156 
157 }
158 
gtsam::KeyInfo::ordering_
Ordering ordering_
Definition: IterativeSolver.h:135
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::KeyInfo::x0
VectorValues x0() const
Return VectorValues with zeros, of correct dimension.
Definition: IterativeSolver.cpp:144
s
RealScalar s
Definition: level1_cplx_impl.h:126
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
gtsam::operator<<
ostream & operator<<(ostream &os, const IterativeOptimizationParameters &p)
Definition: IterativeSolver.cpp:56
os
ofstream os("timeSchurFactors.csv")
gtsam::IterativeSolver::optimize
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map< Key, Vector > &lambda)
Definition: IterativeSolver.cpp:99
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::KeyInfo
Definition: IterativeSolver.h:127
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::VectorValues
Definition: VectorValues.h:74
IterativeSolver.h
Some support classes for iterative solvers.
gtsam::KeyInfo::colSpec
std::vector< size_t > colSpec() const
Return a vector of dimensions ordered by ordering()
Definition: IterativeSolver.cpp:135
gtsam::KeyInfo::x0vector
Vector x0vector() const
Return zero Vector of correct dimension.
Definition: IterativeSolver.cpp:153
VectorValues.h
Factor Graph Values.
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
gtsam::IterativeOptimizationParameters::Verbosity
Verbosity
Definition: IterativeSolver.h:46
lambda
static double lambda[]
Definition: jv.c:524
ordering
static enum @1096 ordering
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
gtsam::GaussianFactorGraph::getKeyDimMap
std::map< Key, size_t > getKeyDimMap() const
Definition: GaussianFactorGraph.cpp:57
gtsam::KeyInfo::initialize
void initialize(const GaussianFactorGraph &fg)
Definition: IterativeSolver.cpp:117
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::IterativeOptimizationParameters
Definition: IterativeSolver.h:43
gtsam::KeyInfo::numCols_
size_t numCols_
Definition: IterativeSolver.h:136
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::KeyInfo::KeyInfo
KeyInfo()
Default Constructor.
Definition: IterativeSolver.h:143
gtsam::Ordering
Definition: inference/Ordering.h:33
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::KeyInfoEntry
Definition: IterativeSolver.h:115


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:54