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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:23