NonlinearFactorGraph.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 
20 #include <gtsam/geometry/Pose2.h>
21 #include <gtsam/geometry/Pose3.h>
23 #include <gtsam/nonlinear/Values.h>
30 #include <gtsam/config.h> // for GTSAM_USE_TBB
31 
32 #ifdef GTSAM_USE_TBB
33 # include <tbb/parallel_for.h>
34 #endif
35 
36 #include <algorithm>
37 #include <cmath>
38 #include <fstream>
39 #include <set>
40 
41 using namespace std;
42 
43 namespace gtsam {
44 
45 // Instantiate base classes
46 template class FactorGraph<NonlinearFactor>;
47 
48 /* ************************************************************************* */
49 double NonlinearFactorGraph::probPrime(const Values& values) const {
50  // NOTE the 0.5 constant is handled by the factor error.
51  return exp(-error(values));
52 }
53 
54 /* ************************************************************************* */
55 void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
56  cout << str << "size: " << size() << endl << endl;
57  for (size_t i = 0; i < factors_.size(); i++) {
58  stringstream ss;
59  ss << "Factor " << i << ": ";
60  if (factors_[i] != nullptr) {
61  factors_[i]->print(ss.str(), keyFormatter);
62  cout << "\n";
63  } else {
64  cout << ss.str() << "nullptr\n";
65  }
66  }
67  std::cout.flush();
68 }
69 
70 /* ************************************************************************* */
71 void NonlinearFactorGraph::printErrors(const Values& values, const std::string& str,
72  const KeyFormatter& keyFormatter,
73  const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>& printCondition) const
74 {
75  cout << str << "size: " << size() << endl
76  << endl;
77  for (size_t i = 0; i < factors_.size(); i++) {
78  const sharedFactor& factor = factors_[i];
79  const double errorValue = (factor != nullptr ? factors_[i]->error(values) : .0);
80  if (!printCondition(factor.get(),errorValue,i))
81  continue; // User-provided filter did not pass
82 
83  stringstream ss;
84  ss << "Factor " << i << ": ";
85  if (factor == nullptr) {
86  cout << "nullptr" << "\n";
87  } else {
88  factor->print(ss.str(), keyFormatter);
89  cout << "error = " << errorValue << "\n";
90  }
91  cout << "\n";
92  }
93  std::cout.flush();
94 }
95 
96 /* ************************************************************************* */
97 bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) const {
98  return Base::equals(other, tol);
99 }
100 
101 /* ************************************************************************* */
102 void NonlinearFactorGraph::dot(std::ostream& os, const Values& values,
103  const KeyFormatter& keyFormatter,
104  const GraphvizFormatting& writer) const {
105  writer.graphPreamble(&os);
106 
107  // Find bounds (imperative)
108  KeySet keys = this->keys();
109  Vector2 min = writer.findBounds(values, keys);
110 
111  // Create nodes for each variable in the graph
112  for (Key key : keys) {
113  auto position = writer.variablePos(values, min, key);
114  writer.drawVariable(key, keyFormatter, position, &os);
115  }
116  os << "\n";
117 
118  if (writer.mergeSimilarFactors) {
119  // Remove duplicate factors
120  std::set<KeyVector> structure;
121  for (const sharedFactor& factor : factors_) {
122  if (factor) {
123  KeyVector factorKeys = factor->keys();
124  std::sort(factorKeys.begin(), factorKeys.end());
125  structure.insert(factorKeys);
126  }
127  }
128 
129  // Create factors and variable connections
130  size_t i = 0;
131  for (const KeyVector& factorKeys : structure) {
132  writer.processFactor(i++, factorKeys, keyFormatter, {}, &os);
133  }
134  } else {
135  // Create factors and variable connections
136  for (size_t i = 0; i < size(); ++i) {
137  const NonlinearFactor::shared_ptr& factor = at(i);
138  if (factor) {
139  const KeyVector& factorKeys = factor->keys();
140  writer.processFactor(i, factorKeys, keyFormatter,
141  writer.factorPos(min, i), &os);
142  }
143  }
144  }
145 
146  os << "}\n";
147  std::flush(os);
148 }
149 
150 /* ************************************************************************* */
151 std::string NonlinearFactorGraph::dot(const Values& values,
152  const KeyFormatter& keyFormatter,
153  const GraphvizFormatting& writer) const {
154  std::stringstream ss;
155  dot(ss, values, keyFormatter, writer);
156  return ss.str();
157 }
158 
159 /* ************************************************************************* */
160 void NonlinearFactorGraph::saveGraph(const std::string& filename,
161  const Values& values,
162  const KeyFormatter& keyFormatter,
163  const GraphvizFormatting& writer) const {
164  std::ofstream of(filename);
165  dot(of, values, keyFormatter, writer);
166  of.close();
167 }
168 
169 /* ************************************************************************* */
170 double NonlinearFactorGraph::error(const Values& values) const {
171  gttic(NonlinearFactorGraph_error);
172  double total_error = 0.;
173  // iterate over all the factors_ to accumulate the log probabilities
174  for(const sharedFactor& factor: factors_) {
175  if(factor)
176  total_error += factor->error(values);
177  }
178  return total_error;
179 }
180 
181 /* ************************************************************************* */
182 Ordering NonlinearFactorGraph::orderingCOLAMD() const
183 {
184  return Ordering::Colamd(*this);
185 }
186 
187 /* ************************************************************************* */
188 Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const
189 {
190  return Ordering::ColamdConstrained(*this, constraints);
191 }
192 
193 /* ************************************************************************* */
194 SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const
195 {
196  // Generate the symbolic factor graph
197  SymbolicFactorGraph::shared_ptr symbolic = std::make_shared<SymbolicFactorGraph>();
198  symbolic->reserve(size());
199 
200  for (const sharedFactor& factor: factors_) {
201  if(factor)
202  symbolic->push_back(SymbolicFactor(*factor));
203  else
204  symbolic->push_back(SymbolicFactorGraph::sharedFactor());
205  }
206 
207  return symbolic;
208 }
209 
210 /* ************************************************************************* */
211 namespace {
212 
213 #ifdef GTSAM_USE_TBB
214 class _LinearizeOneFactor {
215  const NonlinearFactorGraph& nonlinearGraph_;
216  const Values& linearizationPoint_;
217  GaussianFactorGraph& result_;
218 public:
219  // Create functor with constant parameters
220  _LinearizeOneFactor(const NonlinearFactorGraph& graph,
221  const Values& linearizationPoint, GaussianFactorGraph& result) :
222  nonlinearGraph_(graph), linearizationPoint_(linearizationPoint), result_(result) {
223  }
224  // Operator that linearizes a given range of the factors
225  void operator()(const tbb::blocked_range<size_t>& blocked_range) const {
226  for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) {
227  if (nonlinearGraph_[i] && nonlinearGraph_[i]->sendable())
228  result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_);
229  else
230  result_[i] = GaussianFactor::shared_ptr();
231  }
232  }
233 };
234 #endif
235 
236 }
237 
238 /* ************************************************************************* */
239 GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& linearizationPoint) const
240 {
241  gttic(NonlinearFactorGraph_linearize);
242 
243  // create an empty linear FG
244  GaussianFactorGraph::shared_ptr linearFG = std::make_shared<GaussianFactorGraph>();
245 
246 #ifdef GTSAM_USE_TBB
247 
248  linearFG->resize(size());
249  TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
250 
251  // First linearize all sendable factors
252  tbb::parallel_for(tbb::blocked_range<size_t>(0, size()),
253  _LinearizeOneFactor(*this, linearizationPoint, *linearFG));
254 
255  // Linearize all non-sendable factors
256  for(size_t i = 0; i < size(); i++) {
257  auto& factor = (*this)[i];
258  if(factor && !(factor->sendable())) {
259  (*linearFG)[i] = factor->linearize(linearizationPoint);
260  }
261  }
262 
263 #else
264 
265  linearFG->reserve(size());
266 
267  // linearize all factors
268  for (const sharedFactor& factor : factors_) {
269  if (factor) {
270  linearFG->push_back(factor->linearize(linearizationPoint));
271  } else
272  linearFG->push_back(GaussianFactor::shared_ptr());
273  }
274 
275 #endif
276 
277  return linearFG;
278 }
279 
280 /* ************************************************************************* */
281 static Scatter scatterFromValues(const Values& values) {
283 
284  Scatter scatter;
285  scatter.reserve(values.size());
286 
287  // use "natural" ordering with keys taken from the initial values
288  for (const auto& key_dim : values.dims()) {
289  scatter.add(key_dim.first, key_dim.second);
290  }
291 
292  return scatter;
293 }
294 
295 /* ************************************************************************* */
296 static Scatter scatterFromValues(const Values& values, const Ordering& ordering) {
298 
299  Scatter scatter;
300  scatter.reserve(values.size());
301 
302  // copy ordering into keys and lookup dimension in values, is O(n*log n)
303  for (Key key : ordering) {
304  const Value& value = values.at(key);
305  scatter.add(key, value.dim());
306  }
307 
308  return scatter;
309 }
310 
311 /* ************************************************************************* */
312 HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
313  const Values& values, const Scatter& scatter, const Dampen& dampen) const {
314  // NOTE(frank): we are heavily leaning on friendship below
315  HessianFactor::shared_ptr hessianFactor(new HessianFactor(scatter));
316 
317  // Initialize so we can rank-update below
318  hessianFactor->info_.setZero();
319 
320  // linearize all factors straight into the Hessian
321  // TODO(frank): this saves on creating the graph, but still mallocs a gaussianFactor!
322  for (const sharedFactor& nonlinearFactor : factors_) {
323  if (nonlinearFactor) {
324  const auto& gaussianFactor = nonlinearFactor->linearize(values);
325  gaussianFactor->updateHessian(hessianFactor->keys_, &hessianFactor->info_);
326  }
327  }
328 
329  if (dampen) dampen(hessianFactor);
330 
331  return hessianFactor;
332 }
333 
334 /* ************************************************************************* */
335 HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
336  const Values& values, const Ordering& order, const Dampen& dampen) const {
337  gttic(NonlinearFactorGraph_linearizeToHessianFactor);
338 
339  Scatter scatter = scatterFromValues(values, order);
340  return linearizeToHessianFactor(values, scatter, dampen);
341 }
342 
343 /* ************************************************************************* */
344 HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
345  const Values& values, const Dampen& dampen) const {
346  gttic(NonlinearFactorGraph_linearizeToHessianFactor);
347 
348  Scatter scatter = scatterFromValues(values);
349  return linearizeToHessianFactor(values, scatter, dampen);
350 }
351 
352 /* ************************************************************************* */
353 Values NonlinearFactorGraph::updateCholesky(const Values& values,
354  const Dampen& dampen) const {
355  gttic(NonlinearFactorGraph_updateCholesky);
356  auto hessianFactor = linearizeToHessianFactor(values, dampen);
357  VectorValues delta = hessianFactor->solve();
358  return values.retract(delta);
359 }
360 
361 /* ************************************************************************* */
362 Values NonlinearFactorGraph::updateCholesky(const Values& values,
363  const Ordering& ordering,
364  const Dampen& dampen) const {
365  gttic(NonlinearFactorGraph_updateCholesky);
366  auto hessianFactor = linearizeToHessianFactor(values, ordering, dampen);
367  VectorValues delta = hessianFactor->solve();
368  return values.retract(delta);
369 }
370 
371 /* ************************************************************************* */
372 NonlinearFactorGraph NonlinearFactorGraph::clone() const {
374  for (const sharedFactor& f : factors_) {
375  if (f)
376  result.push_back(f->clone());
377  else
378  result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
379  }
380  return result;
381 }
382 
383 /* ************************************************************************* */
384 NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const {
386  for (const sharedFactor& f : factors_) {
387  if (f)
388  result.push_back(f->rekey(rekey_mapping));
389  else
390  result.push_back(sharedFactor());
391  }
392  return result;
393 }
394 
395 /* ************************************************************************* */
396 
397 } // namespace gtsam
const gtsam::Symbol key('X', 0)
void processFactor(size_t i, const KeyVector &keys, const KeyFormatter &keyFormatter, const std::optional< Vector2 > &position, std::ostream *os) const
Draw a single factor, specified by its index i and its variable keys.
Definition: DotWriter.cpp:98
GTSAM_EXPORT void add(Key key, size_t dim)
Add a key/dim pair.
Definition: Scatter.cpp:76
std::optional< Vector2 > variablePos(const Values &values, const Vector2 &min, Key key) const
Return affinely transformed variable position if it exists.
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
const ValueType at(Key j) const
Definition: Values-inl.h:204
#define min(a, b)
Definition: datatypes.h:19
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
leaf::MyValues values
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Vector2 findBounds(const Values &values, const KeySet &keys) const
NonlinearFactorGraph graph
static enum @1107 ordering
void graphPreamble(std::ostream *os) const
Write out preamble for graph, including size.
Definition: DotWriter.cpp:30
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
EIGEN_DEVICE_FUNC const ExpReturnType exp() const
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
#define gttic(label)
Definition: timing.h:295
Factor Graph Values.
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Values result
Definition: pytypes.h:1403
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Linear Factor Graph where all factors are Gaussians.
size_t size() const
Definition: Values.h:178
std::map< Key, size_t > dims() const
Definition: Values.cpp:251
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
virtual size_t dim() const =0
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Exceptions that may be thrown by linear solver components.
static std::stringstream ss
Definition: testBTree.cpp:31
static Scatter scatterFromValues(const Values &values, const Ordering &ordering)
traits
Definition: chartTesting.h:28
void drawVariable(Key key, const KeyFormatter &keyFormatter, const std::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
Definition: DotWriter.cpp:42
ofstream os("timeSchurFactors.csv")
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::shared_ptr< This > shared_ptr
A Gaussian factor using the canonical parameters (information form)
static double error
Definition: testRot3.cpp:37
const G double tol
Definition: Group.h:86
std::shared_ptr< NonlinearFactor > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:105
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
2D Pose
Factor Graph Base Class.
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
3D Pose
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
std::optional< Vector2 > factorPos(const Vector2 &min, size_t i) const
Return affinely transformed factor position if it exists.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


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