JacobianFactor.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 
23 #include <gtsam/linear/Scatter.h>
28 #include <gtsam/base/debug.h>
29 #include <gtsam/base/timing.h>
30 #include <gtsam/base/Matrix.h>
31 #include <gtsam/base/FastMap.h>
32 #include <gtsam/base/cholesky.h>
33 
34 #include <cmath>
35 #include <sstream>
36 #include <stdexcept>
37 
38 using namespace std;
39 
40 namespace gtsam {
41 
42 // Typedefs used in constructors below.
43 using Dims = std::vector<Key>;
44 using Pairs = std::vector<std::pair<Key, Matrix>>;
45 
46 /* ************************************************************************* */
47 JacobianFactor::JacobianFactor() :
48  Ab_(Dims{1}, 0) {
49  getb().setZero();
50 }
51 
52 /* ************************************************************************* */
54  // Copy the matrix data depending on what type of factor we're copying from
55  if (const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(&gf))
56  *this = JacobianFactor(*asJacobian);
57  else if (const HessianFactor* asHessian = dynamic_cast<const HessianFactor*>(&gf))
58  *this = JacobianFactor(*asHessian);
59  else
60  throw std::invalid_argument(
61  "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
62 }
63 
64 /* ************************************************************************* */
66  Ab_(Dims{1}, b_in.size()) {
67  getb() = b_in;
68 }
69 
70 /* ************************************************************************* */
72  const SharedDiagonal& model) {
73  fillTerms(Pairs{{i1, A1}}, b, model);
74 }
75 
76 /* ************************************************************************* */
78  const Matrix& A2, const Vector& b, const SharedDiagonal& model) {
79  fillTerms(Pairs{{i1, A1}, {i2, A2}}, b, model);
80 }
81 
82 /* ************************************************************************* */
84  const Matrix& A2, Key i3, const Matrix& A3, const Vector& b,
85  const SharedDiagonal& model) {
86  fillTerms(Pairs{{i1, A1}, {i2, A2}, {i3, A3}}, b, model);
87 }
88 
89 /* ************************************************************************* */
91  : Base(factor),
92  Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) {
93  // Copy Hessian into our matrix and then do in-place Cholesky
94  Ab_.full() = factor.info().selfadjointView();
95 
96  // Do Cholesky to get a Jacobian
97  const auto [maxrank, success] = choleskyCareful(Ab_.matrix());
98 
99  // Check that Cholesky succeeded OR it managed to factor the full Hessian.
100  // THe latter case occurs with non-positive definite matrices arising from QP.
101  if (success || maxrank == factor.rows() - 1) {
102  // Zero out lower triangle
103  Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
104  Matrix::Zero(maxrank, Ab_.matrix().cols());
105  // FIXME: replace with triangular system
106  Ab_.rowEnd() = maxrank;
107  model_ = SharedDiagonal(); // is equivalent to Unit::Create(maxrank)
108  } else {
109  // indefinite system
110  throw IndeterminantLinearSystemException(factor.keys().front());
111  }
112 }
113 
114 /* ************************************************************************* */
115 // Helper functions for combine constructor
116 namespace {
117 std::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
119  const FastVector<VariableSlots::const_iterator>& variableSlots) {
120  gttic(countDims);
121 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
122  FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
123 #else
124  FastVector<DenseIndex> varDims(variableSlots.size(),
126 #endif
127  DenseIndex m = 0;
128  DenseIndex n = 0;
129  for (size_t jointVarpos = 0; jointVarpos < variableSlots.size();
130  ++jointVarpos) {
131  const VariableSlots::const_iterator& slots = variableSlots[jointVarpos];
132 
133  assert(slots->second.size() == factors.size());
134 
135  bool foundVariable = false;
136  for (size_t sourceFactorI = 0; sourceFactorI < slots->second.size();
137  ++sourceFactorI) {
138  const size_t sourceVarpos = slots->second[sourceFactorI];
139  if (sourceVarpos != VariableSlots::Empty) {
140  const JacobianFactor& sourceFactor = *factors[sourceFactorI];
141  if (sourceFactor.cols() > 1) {
142  foundVariable = true;
143  DenseIndex vardim = sourceFactor.getDim(
144  sourceFactor.begin() + sourceVarpos);
145 
146 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
147  if(varDims[jointVarpos] == numeric_limits<DenseIndex>::max()) {
148  varDims[jointVarpos] = vardim;
149  n += vardim;
150  } else {
151  if(!(varDims[jointVarpos] == vardim)) {
152  std::stringstream ss;
153  ss << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) <<
154  " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos];
155  throw std::runtime_error(ss.str());
156  }
157  }
158 #else
159 
160  varDims[jointVarpos] = vardim;
161  n += vardim;
162  break;
163 #endif
164  }
165  }
166  }
167 
168  if (!foundVariable)
169  throw std::invalid_argument(
170  "Unable to determine dimensionality for all variables");
171  }
172 
173  for(const JacobianFactor::shared_ptr& factor: factors) {
174  m += factor->rows();
175  }
176 
177 #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
178  for(DenseIndex d: varDims) {
179  assert(d != numeric_limits<DenseIndex>::max());
180  }
181 #endif
182 
183  return std::make_tuple(varDims, m, n);
184 }
185 
186 /* ************************************************************************* */
187 FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
188  const GaussianFactorGraph& factors) {
189  gttic(Convert_to_Jacobians);
190  FastVector<JacobianFactor::shared_ptr> jacobians;
191  jacobians.reserve(factors.size());
192  for(const GaussianFactor::shared_ptr& factor: factors) {
193  if (factor) {
194  if (JacobianFactor::shared_ptr jf = std::dynamic_pointer_cast<
195  JacobianFactor>(factor))
196  jacobians.push_back(jf);
197  else
198  jacobians.push_back(std::make_shared<JacobianFactor>(*factor));
199  }
200  }
201  return jacobians;
202 }
203 }
204 
205 /* ************************************************************************* */
207  const FastVector<VariableSlots::const_iterator>& orderedSlots) {
208 
209  // Cast or convert to Jacobians
210  FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
211  graph);
212 
213  // Count dimensions
214  const auto [varDims, m, n] = _countDims(jacobians, orderedSlots);
215 
216  // Allocate matrix and copy keys in order
217  gttic(allocate);
218  Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
219  Base::keys_.resize(orderedSlots.size());
220  // Copy keys in order
221  std::transform(orderedSlots.begin(), orderedSlots.end(),
222  Base::keys_.begin(),
223  [](const VariableSlots::const_iterator& it) {return it->first;});
224  gttoc(allocate);
225 
226  // Loop over slots in combined factor and copy blocks from source factors
227  gttic(copy_blocks);
228  size_t combinedSlot = 0;
229  for(VariableSlots::const_iterator varslot: orderedSlots) {
230  JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot));
231  // Loop over source jacobians
232  DenseIndex nextRow = 0;
233  for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
234  // Slot in source factor
235  const size_t sourceSlot = varslot->second[factorI];
236  const DenseIndex sourceRows = jacobians[factorI]->rows();
237  if (sourceRows > 0) {
239  destSlot.middleRows(nextRow, sourceRows));
240  // Copy if exists in source factor, otherwise set zero
241  if (sourceSlot != VariableSlots::Empty)
242  destBlock = jacobians[factorI]->getA(
243  jacobians[factorI]->begin() + sourceSlot);
244  else
245  destBlock.setZero();
246  nextRow += sourceRows;
247  }
248  }
249  ++combinedSlot;
250  }
251  gttoc(copy_blocks);
252 
253  // Copy the RHS vectors and sigmas
254  gttic(copy_vectors);
255  bool anyConstrained = false;
256  std::optional<Vector> sigmas;
257  // Loop over source jacobians
258  DenseIndex nextRow = 0;
259  for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
260  const DenseIndex sourceRows = jacobians[factorI]->rows();
261  if (sourceRows > 0) {
262  this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
263  if (jacobians[factorI]->get_model()) {
264  // If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
265  if (!sigmas)
266  sigmas = Vector::Constant(m, 1.0);
267  sigmas->segment(nextRow, sourceRows) =
268  jacobians[factorI]->get_model()->sigmas();
269  if (jacobians[factorI]->isConstrained())
270  anyConstrained = true;
271  }
272  nextRow += sourceRows;
273  }
274  }
275  gttoc(copy_vectors);
276 
277  if (sigmas)
278  this->setModel(anyConstrained, *sigmas);
279 }
280 
281 /* ************************************************************************* */
282 // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
283 // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
284 // be added after all of the ordered variables.
286  const Ordering& ordering,
287  const VariableSlots& variableSlots) {
288  gttic(Order_slots);
289 
291  orderedSlots.reserve(variableSlots.size());
292 
293  // If an ordering is provided, arrange the slots first that ordering
295  size_t nOrderingSlotsUsed = 0;
296  orderedSlots.resize(ordering.size());
297  FastMap<Key, size_t> inverseOrdering = ordering.invert();
298  for (VariableSlots::const_iterator item = variableSlots.begin();
299  item != variableSlots.end(); ++item) {
300  FastMap<Key, size_t>::const_iterator orderingPosition =
301  inverseOrdering.find(item->first);
302  if (orderingPosition == inverseOrdering.end()) {
303  unorderedSlots.push_back(item);
304  } else {
305  orderedSlots[orderingPosition->second] = item;
306  ++nOrderingSlotsUsed;
307  }
308  }
309  if (nOrderingSlotsUsed != ordering.size())
310  throw std::invalid_argument(
311  "The ordering provided to the JacobianFactor combine constructor\n"
312  "contained extra variables that did not appear in the factors to combine.");
313  // Add the remaining slots
314  for(VariableSlots::const_iterator item: unorderedSlots) {
315  orderedSlots.push_back(item);
316  }
317 
318  gttoc(Order_slots);
319 
320  return orderedSlots;
321 }
322 
323 /* ************************************************************************* */
325  gttic(JacobianFactor_combine_constructor);
326 
327  // Compute VariableSlots if one was not provided
328  // Binds reference, does not copy VariableSlots
329  const VariableSlots & variableSlots = VariableSlots(graph);
330 
331  gttic(Order_slots);
332  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
333  // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
334  // be added after all of the ordered variables.
336  orderedSlots.reserve(variableSlots.size());
337 
338  // If no ordering is provided, arrange the slots as they were, which will be sorted
339  // numerically since VariableSlots uses a map sorting on Key.
340  for (VariableSlots::const_iterator item = variableSlots.begin();
341  item != variableSlots.end(); ++item)
342  orderedSlots.push_back(item);
343  gttoc(Order_slots);
344 
345  JacobianFactorHelper(graph, orderedSlots);
346 }
347 
348 /* ************************************************************************* */
350  const VariableSlots& p_variableSlots) {
351  gttic(JacobianFactor_combine_constructor);
352 
353  // Binds reference, does not copy VariableSlots
354  const VariableSlots & variableSlots = p_variableSlots;
355 
356  gttic(Order_slots);
357  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
358  // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
359  // be added after all of the ordered variables.
361  orderedSlots.reserve(variableSlots.size());
362 
363  // If no ordering is provided, arrange the slots as they were, which will be sorted
364  // numerically since VariableSlots uses a map sorting on Key.
365  for (VariableSlots::const_iterator item = variableSlots.begin();
366  item != variableSlots.end(); ++item)
367  orderedSlots.push_back(item);
368  gttoc(Order_slots);
369 
370  JacobianFactorHelper(graph, orderedSlots);
371 }
372 
373 /* ************************************************************************* */
375  const Ordering& ordering) {
376  gttic(JacobianFactor_combine_constructor);
377 
378  // Compute VariableSlots if one was not provided
379  // Binds reference, does not copy VariableSlots
380  const VariableSlots & variableSlots = VariableSlots(graph);
381 
382  // Order variable slots
384  orderedSlotsHelper(ordering, variableSlots);
385 
386  JacobianFactorHelper(graph, orderedSlots);
387 }
388 
389 /* ************************************************************************* */
391  const Ordering& ordering,
392  const VariableSlots& p_variableSlots) {
393  gttic(JacobianFactor_combine_constructor);
394 
395  // Order variable slots
397  orderedSlotsHelper(ordering, p_variableSlots);
398 
399  JacobianFactorHelper(graph, orderedSlots);
400 }
401 
402 /* ************************************************************************* */
403 void JacobianFactor::print(const string& s,
404  const KeyFormatter& formatter) const {
405  if (!s.empty())
406  cout << s << "\n";
407  for (const_iterator key = begin(); key != end(); ++key) {
408  cout << " A[" << formatter(*key) << "] = ";
409  cout << getA(key).format(matlabFormat()) << endl;
410  }
411  cout << formatMatrixIndented(" b = ", getb(), true) << "\n";
412  if (model_)
413  model_->print(" Noise model: ");
414  else
415  cout << " No noise model" << endl;
416 }
417 
418 /* ************************************************************************* */
419 // Check if two linear factors are equal
420 bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
421  static const bool verbose = false;
422  if (!dynamic_cast<const JacobianFactor*>(&f_)) {
423  if (verbose)
424  cout << "JacobianFactor::equals: Incorrect type" << endl;
425  return false;
426  } else {
427  const JacobianFactor& f(static_cast<const JacobianFactor&>(f_));
428 
429  // Check keys
430  if (keys() != f.keys()) {
431  if (verbose)
432  cout << "JacobianFactor::equals: keys do not match" << endl;
433  return false;
434  }
435 
436  // Check noise model
437  if ((model_ && !f.model_) || (!model_ && f.model_)) {
438  if (verbose)
439  cout << "JacobianFactor::equals: noise model mismatch" << endl;
440  return false;
441  }
442  if (model_ && f.model_ && !model_->equals(*f.model_, tol)) {
443  if (verbose)
444  cout << "JacobianFactor::equals: noise modesl are not equal" << endl;
445  return false;
446  }
447 
448  // Check matrix sizes
449  if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) {
450  if (verbose)
451  cout << "JacobianFactor::equals: augmented size mismatch" << endl;
452  return false;
453  }
454 
455  // Check matrix contents
456  constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
457  constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
458  for (size_t row = 0; row < (size_t) Ab1.rows(); ++row)
459  if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol)
460  && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) {
461  if (verbose)
462  cout << "JacobianFactor::equals: matrix mismatch at row " << row << endl;
463  return false;
464  }
465 
466  return true;
467  }
468 }
469 
470 /* ************************************************************************* */
472  Vector e = -getb();
473  for (size_t pos = 0; pos < size(); ++pos)
474  e += Ab_(pos) * c[keys_[pos]];
475  return e;
476 }
477 
478 /* ************************************************************************* */
481  if (model_) model_->whitenInPlace(e);
482  return e;
483 }
484 
485 /* ************************************************************************* */
486 double JacobianFactor::error(const VectorValues& c) const {
488  // Use the noise model distance function to get the correct error if available.
489  if (model_) return 0.5 * model_->squaredMahalanobisDistance(e);
490  return 0.5 * e.dot(e);
491 }
492 
493 /* ************************************************************************* */
495  if (model_) {
496  Matrix AbWhitened = Ab_.full();
497  model_->WhitenInPlace(AbWhitened);
498  return AbWhitened.transpose() * AbWhitened;
499  } else {
500  return Ab_.full().transpose() * Ab_.full();
501  }
502 }
503 
504 /* ************************************************************************* */
506  if (model_) {
507  Matrix AWhitened = this->getA();
508  model_->WhitenInPlace(AWhitened);
509  return AWhitened.transpose() * AWhitened;
510  } else {
511  return this->getA().transpose() * this->getA();
512  }
513 }
514 
515 /* ************************************************************************* */
517  for (size_t pos = 0; pos < size(); ++pos) {
518  Key j = keys_[pos];
519  size_t nj = Ab_(pos).cols();
520  auto result = d.emplace(j, nj);
521 
522  Vector& dj = result.first->second;
523 
524  for (size_t k = 0; k < nj; ++k) {
525  Eigen::Ref<const Vector> column_k = Ab_(pos).col(k);
526  if (model_) {
527  Vector column_k_copy = column_k;
528  model_->whitenInPlace(column_k_copy);
529  if(!result.second)
530  dj(k) += dot(column_k_copy, column_k_copy);
531  else
532  dj(k) = dot(column_k_copy, column_k_copy);
533  } else {
534  if (!result.second)
535  dj(k) += dot(column_k, column_k);
536  else
537  dj(k) = dot(column_k, column_k);
538  }
539  }
540  }
541 }
542 
543 /* ************************************************************************* */
544 // Raw memory access version should be called in Regular Factors only currently
545 void JacobianFactor::hessianDiagonal(double* d) const {
546  throw std::runtime_error("JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
547 }
548 
549 /* ************************************************************************* */
550 map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
551  map<Key, Matrix> blocks;
552  for (size_t pos = 0; pos < size(); ++pos) {
553  Key j = keys_[pos];
554  Matrix Aj = Ab_(pos);
555  if (model_)
556  Aj = model_->Whiten(Aj);
557  blocks.emplace(j, Aj.transpose() * Aj);
558  }
559  return blocks;
560 }
561 
562 /* ************************************************************************* */
564  SymmetricBlockMatrix* info) const {
565  gttic(updateHessian_JacobianFactor);
566 
567  if (rows() == 0) return;
568 
569  // Whiten the factor if it has a noise model
570  const SharedDiagonal& model = get_model();
571  if (model && !model->isUnit()) {
572  if (model->isConstrained())
573  throw invalid_argument(
574  "JacobianFactor::updateHessian: cannot update information with "
575  "constrained noise model");
576  JacobianFactor whitenedFactor = whiten();
577  whitenedFactor.updateHessian(infoKeys, info);
578  } else {
579  // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
580  DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;
581 
582  // Apply updates to the upper triangle
583  // Loop over blocks of A, including RHS with j==n
584  vector<DenseIndex> slots(n+1);
585  for (DenseIndex j = 0; j <= n; ++j) {
587  const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
588  slots[j] = J;
589  // Fill off-diagonal blocks with Ai'*Aj
590  for (DenseIndex i = 0; i < j; ++i) {
591  const DenseIndex I = slots[i]; // because i<j, slots[i] is valid.
592  info->updateOffDiagonalBlock(I, J, Ab_(i).transpose() * Ab_j);
593  }
594  // Fill diagonal block with Aj'*Aj
595  info->diagonalBlock(J).rankUpdate(Ab_j.transpose());
596  }
597  }
598 }
599 
600 /* ************************************************************************* */
602  Vector Ax(Ab_.rows());
603  Ax.setZero();
604  if (empty())
605  return Ax;
606 
607  // Just iterate over all A matrices and multiply in correct config part
608  for (size_t pos = 0; pos < size(); ++pos) {
609  // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
610  Ax.noalias() += Ab_(pos) * x[keys_[pos]];
611  }
612 
613  if (model_) model_->whitenInPlace(Ax);
614  return Ax;
615 }
616 
617 /* ************************************************************************* */
619  VectorValues& x) const {
620  Vector E(e.size());
621  E.noalias() = alpha * e;
622  if (model_) model_->whitenInPlace(E);
623  // Just iterate over all A matrices and insert Ai^e into VectorValues
624  for (size_t pos = 0; pos < size(); ++pos) {
625  const Key j = keys_[pos];
626  // To avoid another malloc if key exists, we explicitly check
627  auto it = x.find(j);
628  if (it != x.end()) {
629  // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
630  it->second.noalias() += Ab_(pos).transpose() * E;
631  } else {
632  x.emplace(j, Ab_(pos).transpose() * E);
633  }
634  }
635 }
636 
637 /* ************************************************************************* */
639  VectorValues& y) const {
640  Vector Ax = (*this) * x;
642 }
643 
644 /* ************************************************************************* */
653 void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y,
654  const std::vector<size_t>& accumulatedDims) const {
655 
657  typedef Eigen::Map<Vector> VectorMap;
658  typedef Eigen::Map<const Vector> ConstVectorMap;
659 
660  if (empty())
661  return;
662  Vector Ax = Vector::Zero(Ab_.rows());
663 
667  for (size_t pos = 0; pos < size(); ++pos) {
668  size_t offset = accumulatedDims[keys_[pos]];
669  size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
670  Ax += Ab_(pos) * ConstVectorMap(x + offset, dim);
671  }
673  if (model_) {
674  model_->whitenInPlace(Ax);
675  model_->whitenInPlace(Ax);
676  }
677 
679  Ax *= alpha;
680 
682  for (size_t pos = 0; pos < size(); ++pos) {
683  size_t offset = accumulatedDims[keys_[pos]];
684  size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
685  VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax;
686  }
687 }
688 
689 /* ************************************************************************* */
691  VectorValues g;
692  Vector b = getb();
693  // Gradient is really -A'*b / sigma^2
694  // transposeMultiplyAdd will divide by sigma once, so we need one more
695  if (model_) model_->whitenInPlace(b);
696  this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2
697  return g;
698 }
699 
700 /* ************************************************************************* */
701 // Raw memory access version should be called in Regular Factors only currently
702 void JacobianFactor::gradientAtZero(double* d) const {
703  throw std::runtime_error("JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
704 }
705 
706 /* ************************************************************************* */
708  // TODO: optimize it for JacobianFactor without converting to a HessianFactor
709  HessianFactor hessian(*this);
710  return hessian.gradient(key, x);
711 }
712 
713 /* ************************************************************************* */
714 pair<Matrix, Vector> JacobianFactor::jacobian() const {
715  pair<Matrix, Vector> result = jacobianUnweighted();
716  // divide in sigma so error is indeed 0.5*|Ax-b|
717  if (model_)
718  model_->WhitenSystem(result.first, result.second);
719  return result;
720 }
721 
722 /* ************************************************************************* */
723 pair<Matrix, Vector> JacobianFactor::jacobianUnweighted() const {
724  Matrix A(Ab_.range(0, size()));
725  Vector b(getb());
726  return make_pair(A, b);
727 }
728 
729 /* ************************************************************************* */
732  if (model_)
733  model_->WhitenInPlace(Ab);
734  return Ab;
735 }
736 
737 /* ************************************************************************* */
739  return Ab_.range(0, Ab_.nBlocks());
740 }
741 
742 /* ************************************************************************* */
744  JacobianFactor result(*this);
745  if (model_) {
746  result.model_->WhitenInPlace(result.Ab_.full());
747  result.model_ = SharedDiagonal();
748  }
749  return result;
750 }
751 
752 /* ************************************************************************* */
754  HessianFactor hessian(*this);
755  return hessian.negate();
756 }
757 
758 /* ************************************************************************* */
761  const Ordering& keys) {
763  graph.add(*this);
764  return EliminateQR(graph, keys);
765 }
766 
767 /* ************************************************************************* */
768 void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) {
769  if ((size_t) sigmas.size() != this->rows())
770  throw InvalidNoiseModel(this->rows(), sigmas.size());
771  if (anyConstrained)
773  else
775 }
776 
777 /* ************************************************************************* */
778 std::pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr> EliminateQR(
779  const GaussianFactorGraph& factors, const Ordering& keys) {
781  // Combine and sort variable blocks in elimination order
782  JacobianFactor::shared_ptr jointFactor;
783  try {
784  jointFactor = std::make_shared<JacobianFactor>(factors, keys);
785  } catch (std::invalid_argument&) {
787  "EliminateQR was called with a request to eliminate variables that are not\n"
788  "involved in the provided factors.");
789  }
790 
791  // Do dense elimination
792  // The following QR variants eliminate to fully triangular or trapezoidal
793  SharedDiagonal noiseModel;
794  VerticalBlockMatrix& Ab = jointFactor->Ab_;
795  if (jointFactor->model_) {
796  // The noiseModel QR can, in the case of constraints, yield a "staggered" QR where
797  // some rows have more leading zeros than in an upper triangular matrix.
798  // In either case, QR will put zeros below the "diagonal".
799  jointFactor->model_ = jointFactor->model_->QR(Ab.matrix());
800  } else {
801  // The inplace variant will have no valid rows anymore below m==n
802  // and only entries above the diagonal are valid.
803  inplace_QR(Ab.matrix());
804  // We zero below the diagonal to agree with the result from noieModel QR
805  Ab.matrix().triangularView<Eigen::StrictlyLower>().setZero();
806  size_t m = Ab.rows(), n = Ab.cols() - 1;
807  size_t maxRank = min(m, n);
808  jointFactor->model_ = noiseModel::Unit::Create(maxRank);
809  }
810 
811  // Split elimination result into conditional and remaining factor
812  GaussianConditional::shared_ptr conditional = //
813  jointFactor->splitConditional(keys.size());
814 
815  return make_pair(conditional, jointFactor);
816 }
817 
818 /* ************************************************************************* */
820  gttic(JacobianFactor_splitConditional);
821 
822  if (!model_) {
823  throw std::invalid_argument(
824  "JacobianFactor::splitConditional cannot be given a nullptr noise model");
825  }
826 
827  if (nrFrontals > size()) {
828  throw std::invalid_argument(
829  "JacobianFactor::splitConditional was requested to split off more variables than exist.");
830  }
831 
832  // Convert nr of keys to number of scalar columns
833  DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols();
834 
835  // Check that the noise model has at least this dimension
836  // If this is *not* the case, we do not have enough information on the frontal variables.
837  if ((DenseIndex)model_->dim() < frontalDim)
839 
840  // Restrict the matrix to be in the first nrFrontals variables and create the conditional
841  const DenseIndex originalRowEnd = Ab_.rowEnd();
842  Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
843  SharedDiagonal conditionalNoiseModel;
844  conditionalNoiseModel =
845  noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
846  GaussianConditional::shared_ptr conditional =
847  std::make_shared<GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
848 
849  const DenseIndex maxRemainingRows =
850  std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim;
851  const DenseIndex remainingRows = std::min(model_->sigmas().size() - frontalDim, maxRemainingRows);
852  Ab_.rowStart() += frontalDim;
853  Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
854  Ab_.firstBlock() += nrFrontals;
855 
856  // Take lower-right block of Ab to get the new factor
857  keys_.erase(begin(), begin() + nrFrontals);
858  // Set sigmas with the right model
859  if (model_->isConstrained())
860  model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows));
861  else
862  model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows));
863  assert(model_->dim() == (size_t)Ab_.rows());
864 
865  return conditional;
866 }
867 
868 } // namespace gtsam
gttoc
#define gttoc(label)
Definition: timing.h:296
Dims
std::vector< Eigen::Index > Dims
Definition: testGaussianConditional.cpp:46
timing.h
Timing utilities.
gtsam::VerticalBlockMatrix::rowEnd
const DenseIndex & rowEnd() const
Definition: VerticalBlockMatrix.h:176
gtsam::VariableSlots::Empty
static const GTSAM_EXPORT size_t Empty
Definition: VariableSlots.h:56
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::JacobianFactor::splitConditional
std::shared_ptr< GaussianConditional > splitConditional(size_t nrFrontals)
Definition: JacobianFactor.cpp:819
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::JacobianFactor::unweighted_error
Vector unweighted_error(const VectorValues &c) const
Definition: JacobianFactor.cpp:471
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
A3
static const double A3[]
Definition: expn.h:8
gtsam::VariableSlots
Definition: VariableSlots.h:51
RowsBlockXpr
Block< Derived, Dynamic, internal::traits< Derived >::ColsAtCompileTime, IsRowMajor > RowsBlockXpr
Definition: BlockMethods.h:23
gtsam::JacobianFactor::Ab_
VerticalBlockMatrix Ab_
Definition: JacobianFactor.h:106
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::noiseModel::Constrained::MixedSigmas
static shared_ptr MixedSigmas(const Vector &mu, const Vector &sigmas)
Definition: NoiseModel.cpp:376
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
Definition: JacobianFactor.h:270
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
Matrix.h
typedef and functions to augment Eigen's MatrixXd
pybind_wrapper_test_script.this
this
Definition: pybind_wrapper_test_script.py:38
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::FastMap< Key, size_t >
gtsam::GaussianFactor
Definition: GaussianFactor.h:38
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
Definition: VerticalBlockMatrix.h:130
cholesky.h
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition: Factor.h:131
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:158
gtsam::VerticalBlockMatrix::full
Block full()
Definition: VerticalBlockMatrix.h:157
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::InvalidNoiseModel
Definition: linearExceptions.h:106
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
Ordering.h
Variable ordering for the elimination algorithm.
gtsam::Factor
Definition: Factor.h:70
gtsam::matlabFormat
const Eigen::IOFormat & matlabFormat()
Definition: Matrix.cpp:139
gtsam::JacobianFactor::rows
size_t rows() const
Definition: JacobianFactor.h:284
gtsam::HessianFactor::info
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
Definition: HessianFactor.h:264
gtsam::HessianFactor::negate
GaussianFactor::shared_ptr negate() const override
Definition: HessianFactor.cpp:377
gtsam::JacobianFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Returns (dense) A,b pair associated with factor, bakes in the weights.
Definition: JacobianFactor.cpp:714
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:146
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::JacobianFactor::negate
GaussianFactor::shared_ptr negate() const override
Definition: JacobianFactor.cpp:753
gtsam::JacobianFactor::hessianDiagonalAdd
void hessianDiagonalAdd(VectorValues &d) const override
Add the current diagonal to a VectorValues instance.
Definition: JacobianFactor.cpp:516
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
gtsam::choleskyCareful
pair< size_t, bool > choleskyCareful(Matrix &ATA, int order)
Definition: base/cholesky.cpp:75
gtsam::JacobianFactor::getb
const constBVector getb() const
Definition: JacobianFactor.h:298
gtsam::JacobianFactor::operator*
Vector operator*(const VectorValues &x) const
Definition: JacobianFactor.cpp:601
gtsam::JacobianFactor::eliminate
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > eliminate(const Ordering &keys)
Definition: JacobianFactor.cpp:760
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition: GaussianFactor.cpp:35
ss
static std::stringstream ss
Definition: testBTree.cpp:31
gtsam::JacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: JacobianFactor.cpp:638
gtsam::JacobianFactor::model_
noiseModel::Diagonal::shared_ptr model_
Definition: JacobianFactor.h:107
gtsam::VerticalBlockMatrix::firstBlock
const DenseIndex & firstBlock() const
Definition: VerticalBlockMatrix.h:182
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::Dims
std::vector< Key > Dims
Definition: HessianFactor.cpp:41
linearExceptions.h
Exceptions that may be thrown by linear solver components.
gtsam::VerticalBlockMatrix::rowStart
const DenseIndex & rowStart() const
Definition: VerticalBlockMatrix.h:170
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition: JacobianFactor-inl.h:60
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
gtsam::JacobianFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Compute the gradient wrt a key at any values.
Definition: JacobianFactor.cpp:707
gtsam::JacobianFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Definition: JacobianFactor.h:277
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::VectorValues
Definition: VectorValues.h:74
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
make_tuple
tuple make_tuple()
Definition: cast.h:1383
I
#define I
Definition: main.h:112
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Definition: base/Matrix.h:221
gtsam::JacobianFactor::information
Matrix information() const override
Definition: JacobianFactor.cpp:505
gtsam::JacobianFactor::jacobianUnweighted
std::pair< Matrix, Vector > jacobianUnweighted() const
Returns (dense) A,b pair associated with factor, does not bake in weights.
Definition: JacobianFactor.cpp:723
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition: JacobianFactor.cpp:690
gtsam::JacobianFactor::EliminateQR
friend GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778
gtsam::KeyFormatter
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
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::InvalidDenseElimination
Definition: linearExceptions.h:134
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
Eigen::PlainObjectBase::rows
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:143
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
gtsam::HessianFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Definition: HessianFactor.cpp:438
gtsam::JacobianFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Definition: JacobianFactor.cpp:730
info
else if n * info
Definition: 3rdparty/Eigen/lapack/cholesky.cpp:18
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition: VerticalBlockMatrix.h:121
Eigen::StrictlyLower
@ StrictlyLower
Definition: Constants.h:221
VectorValues.h
Factor Graph Values.
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
gtsam::FastList
Definition: FastList.h:43
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
size_t
std::size_t size_t
Definition: wrap/pybind11/include/pybind11/detail/common.h:490
gtsam::JacobianFactor::cols
size_t cols() const
Definition: JacobianFactor.h:289
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
ordering
static enum @1096 ordering
gtsam::JacobianFactor::error_vector
Vector error_vector(const VectorValues &c) const
Definition: JacobianFactor.cpp:479
y
Scalar * y
Definition: level1_cplx_impl.h:124
offset
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
Definition: gnuplot_common_settings.hh:64
key
const gtsam::Symbol key('X', 0)
E
DiscreteKey E(5, 2)
JacobianFactor.h
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Pairs
std::vector< std::pair< Key, Matrix > > Pairs
Definition: JacobianFactor.cpp:44
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
gtsam::inplace_QR
void inplace_QR(Matrix &A)
Definition: Matrix.cpp:636
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam::orderedSlotsHelper
FastVector< VariableSlots::const_iterator > orderedSlotsHelper(const Ordering &ordering, const VariableSlots &variableSlots)
Definition: JacobianFactor.cpp:285
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
gtsam
traits
Definition: SFMdata.h:40
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition: VerticalBlockMatrix.h:118
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::Factor::front
Key front() const
First key.
Definition: Factor.h:134
i1
double i1(double x)
Definition: i1.c:150
gtsam::JacobianFactor::equals
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
assert equality up to a tolerance
Definition: JacobianFactor.cpp:420
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::JacobianFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: JacobianFactor.cpp:550
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Definition: VerticalBlockMatrix.h:188
std
Definition: BFloat16.h:88
A1
static const double A1[]
Definition: expn.h:6
gtsam::JacobianFactor::JacobianFactorHelper
void JacobianFactorHelper(const GaussianFactorGraph &graph, const FastVector< VariableSlots::const_iterator > &orderedSlots)
Definition: JacobianFactor.cpp:206
gtsam::GaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:46
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
JacobianFactor
gtsam::HessianFactor::rows
size_t rows() const
Definition: HessianFactor.h:217
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:115
gtsam::GaussianFactor::Slot
static DenseIndex Slot(const CONTAINER &keys, Key key)
Definition: GaussianFactor.h:175
gtsam::JacobianFactor::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print with optional string
Definition: JacobianFactor.cpp:403
gtsam::JacobianFactor::error
double error(const VectorValues &c) const override
Definition: JacobianFactor.cpp:486
gtsam::JacobianFactor::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, VectorValues &x) const
Definition: JacobianFactor.cpp:618
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
setZero
v setZero(3)
gtsam::JacobianFactor::JacobianFactor
JacobianFactor()
Definition: JacobianFactor.cpp:47
N
#define N
Definition: igam.h:9
gtsam::IndeterminantLinearSystemException
Definition: linearExceptions.h:94
gtsam::JacobianFactor::getA
constABlock getA() const
Definition: JacobianFactor.h:304
pos
Definition: example-NearestNeighbor.cpp:32
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::JacobianFactor::whiten
JacobianFactor whiten() const
Definition: JacobianFactor.cpp:743
gtsam::JacobianFactor::setModel
void setModel(bool anyConstrained, const Vector &sigmas)
Definition: JacobianFactor.cpp:768
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::JacobianFactor::augmentedJacobianUnweighted
Matrix augmentedJacobianUnweighted() const
Definition: JacobianFactor.cpp:738
Scatter.h
Maps global variable indices to slot indices.
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::JacobianFactor::augmentedInformation
Matrix augmentedInformation() const override
Definition: JacobianFactor.cpp:494
gttic
#define gttic(label)
Definition: timing.h:295
debug.h
Global debugging flags.
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
gtsam::formatMatrixIndented
std::string formatMatrixIndented(const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
Definition: Matrix.cpp:597
gtsam::JacobianFactor::updateHessian
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Definition: JacobianFactor.cpp:563
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778


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