NoiseModel.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/base/timing.h>
21 
22 #include <cmath>
23 #include <iostream>
24 #include <limits>
25 #include <stdexcept>
26 #include <typeinfo>
27 
28 using namespace std;
29 
30 namespace gtsam {
31 namespace noiseModel {
32 
33 /* ************************************************************************* */
34 // update A, b
35 // A' \define A_{S}-ar and b'\define b-ad
36 // Linear algebra: takes away projection on latest orthogonal
37 // Graph: make a new factor on the separator S
38 // __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
39 template<class MATRIX>
40 void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
41  size_t n = Ab.cols()-1;
42  Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
43 }
44 
45 /* ************************************************************************* */
46 // check *above the diagonal* for non-zero entries
47 std::optional<Vector> checkIfDiagonal(const Matrix& M) {
48  size_t m = M.rows(), n = M.cols();
49  assert(m > 0);
50  // check all non-diagonal entries
51  bool full = false;
52  size_t i, j;
53  for (i = 0; i < m; i++)
54  if (!full)
55  for (j = i + 1; j < n; j++)
56  if (std::abs(M(i, j)) > 1e-9) {
57  full = true;
58  break;
59  }
60  if (full) {
61  return {};
62  } else {
63  Vector diagonal(n);
64  for (j = 0; j < n; j++)
65  diagonal(j) = M(j, j);
66  return std::move(diagonal);
67  }
68 }
69 
70 /* ************************************************************************* */
72  throw("Base::sigmas: sigmas() not implemented for this noise model");
73 }
74 
75 /* ************************************************************************* */
76 double Base::squaredMahalanobisDistance(const Vector& v) const {
77  // Note: for Diagonal, which does ediv_, will be correct for constraints
78  Vector w = whiten(v);
79  return w.dot(w);
80 }
81 
82 /* ************************************************************************* */
83 Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
84  size_t m = R.rows(), n = R.cols();
85  if (m != n)
86  throw invalid_argument("Gaussian::SqrtInformation: R not square");
87  if (smart) {
88  std::optional<Vector> diagonal = checkIfDiagonal(R);
89  if (diagonal)
90  return Diagonal::Sigmas(diagonal->array().inverse(), true);
91  }
92  // NOTE(frank): only reaches here if !(smart && diagonal)
93  return std::make_shared<Gaussian>(R.rows(), R);
94 }
95 
96 /* ************************************************************************* */
97 Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) {
98  size_t m = information.rows(), n = information.cols();
99  if (m != n)
100  throw invalid_argument("Gaussian::Information: R not square");
101  std::optional<Vector> diagonal = {};
102  if (smart)
103  diagonal = checkIfDiagonal(information);
104  if (diagonal)
105  return Diagonal::Precisions(*diagonal, true);
106  else {
107  Eigen::LLT<Matrix> llt(information);
108  Matrix R = llt.matrixU();
109  return std::make_shared<Gaussian>(n, R);
110  }
111 }
112 
113 /* ************************************************************************* */
114 Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
115  bool smart) {
116  size_t m = covariance.rows(), n = covariance.cols();
117  if (m != n)
118  throw invalid_argument("Gaussian::Covariance: covariance not square");
119  std::optional<Vector> variances = {};
120  if (smart)
121  variances = checkIfDiagonal(covariance);
122  if (variances)
123  return Diagonal::Variances(*variances, true);
124  else {
125  // NOTE: if cov = L'*L, then the square root information R can be found by
126  // QR, as L.inverse() = Q*R, with Q some rotation matrix. However, R has
127  // annoying sign flips with respect the simpler Information(inv(cov)),
128  // hence we choose the simpler path here:
129  return Information(covariance.inverse(), false);
130  }
131 }
132 
133 /* ************************************************************************* */
134 void Gaussian::print(const string& name) const {
135  gtsam::print(thisR(), name + "Gaussian ");
136 }
137 
138 /* ************************************************************************* */
139 bool Gaussian::equals(const Base& expected, double tol) const {
140  const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
141  if (p == nullptr) return false;
142  if (typeid(*this) != typeid(*p)) return false;
143  return equal_with_abs_tol(R(), p->R(), sqrt(tol));
144 }
145 
146 /* ************************************************************************* */
147 Matrix Gaussian::covariance() const {
148  // Uses a fast version of `covariance = information().inverse();`
149  const Matrix& R = this->R();
150  Matrix I = Matrix::Identity(R.rows(), R.cols());
151  // Fast inverse of upper-triangular matrix R using forward-substitution
152  Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I);
153  // (R' * R)^{-1} = R^{-1} * R^{-1}'
154  return Rinv * Rinv.transpose();
155 }
156 
157 /* ************************************************************************* */
159  return Vector(covariance().diagonal()).cwiseSqrt();
160 }
161 
162 /* ************************************************************************* */
163 Vector Gaussian::whiten(const Vector& v) const {
164  return thisR() * v;
165 }
166 
167 /* ************************************************************************* */
168 Vector Gaussian::unwhiten(const Vector& v) const {
169  return backSubstituteUpper(thisR(), v);
170 }
171 
172 /* ************************************************************************* */
173 Matrix Gaussian::Whiten(const Matrix& H) const {
174  return thisR() * H;
175 }
176 
177 /* ************************************************************************* */
178 void Gaussian::WhitenInPlace(Matrix& H) const {
179  H = thisR() * H;
180 }
181 
182 /* ************************************************************************* */
183 void Gaussian::WhitenInPlace(Eigen::Block<Matrix> H) const {
184  H = thisR() * H;
185 }
186 
187 /* ************************************************************************* */
188 // General QR, see also special version in Constrained
189 SharedDiagonal Gaussian::QR(Matrix& Ab) const {
190 
191  gttic(Gaussian_noise_model_QR);
192 
193  static const bool debug = false;
194 
195  // get size(A) and maxRank
196  // TODO: really no rank problems ?
197  size_t m = Ab.rows(), n = Ab.cols()-1;
198  size_t maxRank = min(m,n);
199 
200  // pre-whiten everything (cheaply if possible)
201  WhitenInPlace(Ab);
202 
203  if(debug) gtsam::print(Ab, "Whitened Ab: ");
204 
205  // Eigen QR - much faster than older householder approach
206  inplace_QR(Ab);
207  Ab.triangularView<Eigen::StrictlyLower>().setZero();
208 
209  // hand-coded householder implementation
210  // TODO: necessary to isolate last column?
211  // householder(Ab, maxRank);
212 
213  return noiseModel::Unit::Create(maxRank);
214 }
215 
216 void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
217  for(Matrix& Aj: A) { WhitenInPlace(Aj); }
218  whitenInPlace(b);
219 }
220 
221 void Gaussian::WhitenSystem(Matrix& A, Vector& b) const {
222  WhitenInPlace(A);
223  whitenInPlace(b);
224 }
225 
226 void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
227  WhitenInPlace(A1);
228  WhitenInPlace(A2);
229  whitenInPlace(b);
230 }
231 
232 void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
233  WhitenInPlace(A1);
234  WhitenInPlace(A2);
235  WhitenInPlace(A3);
236  whitenInPlace(b);
237 }
238 
239 Matrix Gaussian::information() const { return R().transpose() * R(); }
240 
241 /* *******************************************************************************/
242 double Gaussian::logDetR() const {
243  double logDetR =
244  R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
245  return logDetR;
246 }
247 
248 /* *******************************************************************************/
249 double Gaussian::logDeterminant() const {
250  // Since noise models are Gaussian, we can get the logDeterminant easily
251  // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
252  // log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
253  // Hence, log det(Sigma)) = -2.0 * logDetR()
254  return -2.0 * logDetR();
255 }
256 
257 /* *******************************************************************************/
258 double Gaussian::negLogConstant() const {
259  // log(det(Sigma)) = -2.0 * logDetR
260  // which gives neg-log = 0.5*n*log(2*pi) + 0.5*(-2.0 * logDetR())
261  // = 0.5*n*log(2*pi) - (0.5*2.0 * logDetR())
262  // = 0.5*n*log(2*pi) - logDetR()
263  size_t n = dim();
264  constexpr double log2pi = 1.8378770664093454835606594728112;
265  // Get -log(1/\sqrt(|2pi Sigma|)) = 0.5*log(|2pi Sigma|)
266  return 0.5 * n * log2pi - logDetR();
267 }
268 
269 /* ************************************************************************* */
270 // Diagonal
271 /* ************************************************************************* */
273 
274 /* ************************************************************************* */
276  : Gaussian(sigmas.size()),
277  sigmas_(sigmas),
278  invsigmas_(sigmas.array().inverse()),
279  precisions_(invsigmas_.array().square()) {
280 }
281 
282 /* ************************************************************************* */
283 Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
284  // check whether all the same entry
285  return (smart && (variances.array() == variances(0)).all())
286  ? Isotropic::Variance(variances.size(), variances(0), true)
287  : shared_ptr(new Diagonal(variances.cwiseSqrt()));
288 }
289 
290 /* ************************************************************************* */
292  if (smart) {
293  size_t n = sigmas.size();
294  if (n == 0) goto full;
295 
296  // look for zeros to make a constraint
297  if ((sigmas.array() < 1e-8).any()) {
299  }
300 
301  // check whether all the same entry
302  if ((sigmas.array() == sigmas(0)).all()) {
303  return Isotropic::Sigma(n, sigmas(0), true);
304  }
305  }
306 full:
307  return Diagonal::shared_ptr(new Diagonal(sigmas));
308 }
309 
310 /* ************************************************************************* */
312  bool smart) {
313  return Variances(precisions.array().inverse(), smart);
314 }
315 
316 /* ************************************************************************* */
317 void Diagonal::print(const string& name) const {
318  gtsam::print(sigmas_, name + "diagonal sigmas ");
319 }
320 
321 /* ************************************************************************* */
323  return v.cwiseProduct(invsigmas_);
324 }
325 
327  return v.cwiseProduct(sigmas_);
328 }
329 
331  return vector_scale(invsigmas(), H);
332 }
333 
336 }
337 
339  H = invsigmas().asDiagonal() * H;
340 }
341 
342 /* *******************************************************************************/
343 double Diagonal::logDetR() const {
344  return invsigmas_.unaryExpr([](double x) { return log(x); }).sum();
345 }
346 
347 /* ************************************************************************* */
348 // Constrained
349 /* ************************************************************************* */
350 
351 namespace internal {
352 // switch precisions and invsigmas to finite value
353 // TODO: why?? And, why not just ask s==0.0 below ?
354 static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
355  for (Vector::Index i = 0; i < sigmas.size(); ++i)
356  if (!std::isfinite(1. / sigmas[i])) {
357  precisions[i] = 0.0;
358  invsigmas[i] = 0.0;
359  }
360 }
361 }
362 
363 /* ************************************************************************* */
365  : Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) {
367 }
368 
369 /* ************************************************************************* */
371  : Diagonal(sigmas), mu_(mu) {
373 }
374 
375 /* ************************************************************************* */
377  const Vector& sigmas) {
378  return shared_ptr(new Constrained(mu, sigmas));
379 }
380 
381 /* ************************************************************************* */
382 bool Constrained::constrained(size_t i) const {
383  // TODO why not just check sigmas_[i]==0.0 ?
384  return !std::isfinite(1./sigmas_[i]);
385 }
386 
387 /* ************************************************************************* */
388 void Constrained::print(const std::string& name) const {
389  gtsam::print(sigmas_, name + "constrained sigmas ");
390  gtsam::print(mu_, name + "constrained mu ");
391 }
392 
393 /* ************************************************************************* */
395  // If sigmas[i] is not 0 then divide v[i] by sigmas[i], as usually done in
396  // other normal Gaussian noise model. Otherwise, sigmas[i] = 0 indicating
397  // a hard constraint, we don't do anything.
398  const Vector& a = v;
399  const Vector& b = sigmas_;
400  size_t n = a.size();
401  assert (b.size()==a.size());
402  Vector c(n);
403  for( size_t i = 0; i < n; i++ ) {
404  const double& ai = a(i), bi = b(i);
405  c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
406  }
407  return c;
408 }
409 
410 /* ************************************************************************* */
412  return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
413 }
414 
416  const Vector& sigmas) {
417  return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
418 }
419 
421  const Vector& variances) {
422  return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
423 }
425  return shared_ptr(new Constrained(variances.cwiseSqrt()));
426 }
427 
429  const Vector& precisions) {
430  return MixedVariances(mu, precisions.array().inverse());
431 }
433  return MixedVariances(precisions.array().inverse());
434 }
435 
436 /* ************************************************************************* */
438  Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
439  for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
440  if (constrained(i)) // whiten makes constrained variables zero
441  w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
442  return w.dot(w);
443 }
444 
445 /* ************************************************************************* */
447  Matrix A = H;
448  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
449  if (!constrained(i)) // if constrained, leave row of A as is
450  A.row(i) *= invsigmas_(i);
451  return A;
452 }
453 
454 /* ************************************************************************* */
456  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
457  if (!constrained(i)) // if constrained, leave row of H as is
458  H.row(i) *= invsigmas_(i);
459 }
460 
461 /* ************************************************************************* */
463  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
464  if (!constrained(i)) // if constrained, leave row of H as is
465  H.row(i) *= invsigmas_(i);
466 }
467 
468 /* ************************************************************************* */
470  Vector sigmas = Vector::Ones(dim());
471  for (size_t i=0; i<dim(); ++i)
472  if (constrained(i))
473  sigmas(i) = 0.0;
474  return MixedSigmas(mu_, sigmas);
475 }
476 
477 /* ************************************************************************* */
478 // Special version of QR for Constrained calls slower but smarter code
479 // that deals with possibly zero sigmas
480 // It is Gram-Schmidt orthogonalization rather than Householder
481 
482 // Check whether column a triggers a constraint and corresponding variable is deterministic
483 // Return constraint_row with maximum element in case variable plays in multiple constraints
484 template <typename VECTOR>
485 std::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
486  std::optional<size_t> constraint_row;
487  // not zero, so roundoff errors will not be counted
488  // TODO(frank): that's a fairly crude way of dealing with roundoff errors :-(
489  double max_element = 1e-9;
490  for (size_t i = 0; i < m; i++) {
491  if (!std::isinf(invsigmas[i]))
492  continue;
493  double abs_ai = std::abs(a(i,0));
494  if (abs_ai > max_element) {
495  max_element = abs_ai;
496  constraint_row = i;
497  }
498  }
499  return constraint_row;
500 }
501 
503  static const double kInfinity = std::numeric_limits<double>::infinity();
504 
505  // get size(A) and maxRank
506  size_t m = Ab.rows();
507  const size_t n = Ab.cols() - 1;
508  const size_t maxRank = min(m, n);
509 
510  // create storage for [R d]
511  typedef std::tuple<size_t, Matrix, double> Triple;
512  list<Triple> Rd;
513 
514  Matrix rd(1, n + 1); // and for row of R
515  Vector invsigmas = sigmas_.array().inverse();
516  Vector weights = invsigmas.array().square(); // calculate weights once
517 
518  // We loop over all columns, because the columns that can be eliminated
519  // are not necessarily contiguous. For each one, estimate the corresponding
520  // scalar variable x as d-rS, with S the separator (remaining columns).
521  // Then update A and b by substituting x with d-rS, zero-ing out x's column.
522  for (size_t j = 0; j < n; ++j) {
523  // extract the first column of A
524  Eigen::Block<Matrix> a = Ab.block(0, j, m, 1);
525 
526  // Check whether we need to handle as a constraint
527  std::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);
528 
529  if (constraint_row) {
530  // Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j)
531 
532  // In this case, the row in [R|d] is simply the row in [A|b]
533  // NOTE(frank): we used to divide by a[i] but there is no need with a constraint
534  rd = Ab.row(*constraint_row);
535 
536  // Construct solution (r, d, sigma)
537  Rd.push_back(std::make_tuple(j, rd, kInfinity));
538 
539  // exit after rank exhausted
540  if (Rd.size() >= maxRank)
541  break;
542 
543  // The constraint row will be zeroed out, so we can save work by swapping in the
544  // last valid row and decreasing m. This will save work on subsequent down-dates, too.
545  m -= 1;
546  if (*constraint_row != m) {
547  Ab.row(*constraint_row) = Ab.row(m);
548  weights(*constraint_row) = weights(m);
549  invsigmas(*constraint_row) = invsigmas(m);
550  }
551 
552  // get a reduced a-column which is now shorter
553  Eigen::Block<Matrix> a_reduced = Ab.block(0, j, m, 1);
554  a_reduced *= (1.0/rd(0, j)); // NOTE(frank): this is the 1/a[i] = 1/rd(0,j) factor we need!
555 
556  // Rank-1 down-date of Ab, expensive, using outer product
557  Ab.block(0, j + 1, m, n - j).noalias() -= a_reduced * rd.middleCols(j + 1, n - j);
558  } else {
559  // Treat in normal Gram-Schmidt way
560  // Calculate weighted pseudo-inverse and corresponding precision
561 
562  // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
563  // For diagonal Sigma, inv(Sigma) = diag(precisions)
564  double precision = 0;
565  Vector pseudo(m); // allocate storage for pseudo-inverse
566  for (size_t i = 0; i < m; i++) {
567  double ai = a(i, 0);
568  if (std::abs(ai) > 1e-9) { // also catches remaining sigma==0 rows
569  pseudo[i] = weights[i] * ai;
570  precision += pseudo[i] * ai;
571  } else
572  pseudo[i] = 0;
573  }
574 
575  if (precision > 1e-8) {
576  pseudo /= precision;
577 
578  // create solution [r d], rhs is automatically r(n)
579  rd(0, j) = 1.0; // put 1 on diagonal
580  rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j);
581 
582  // construct solution (r, d, sigma)
583  Rd.push_back(std::make_tuple(j, rd, precision));
584  } else {
585  // If precision is zero, no information on this column
586  // This is actually not limited to constraints, could happen in Gaussian::QR
587  // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
588  continue; // but even if not, no need to update if a==zeros
589  }
590 
591  // exit after rank exhausted
592  if (Rd.size() >= maxRank)
593  break;
594 
595  // Rank-1 down-date of Ab, expensive, using outer product
596  Ab.block(0, j + 1, m, n - j).noalias() -= a * rd.middleCols(j + 1, n - j);
597  }
598  }
599 
600  // Create storage for precisions
601  Vector precisions(Rd.size());
602 
603  // Write back result in Ab, imperative as we are
604  size_t i = 0; // start with first row
605  bool mixed = false;
606  Ab.setZero(); // make sure we don't look below
607  for (const Triple& t: Rd) {
608  const size_t& j = std::get<0>(t);
609  const Matrix& rd = std::get<1>(t);
610  precisions(i) = std::get<2>(t);
611  if (std::isinf(precisions(i)))
612  mixed = true;
613  Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j);
614  i += 1;
615  }
616 
617  // Must include mu, as the defaults might be higher, resulting in non-convergence
619 }
620 
621 /* ************************************************************************* */
622 // Isotropic
623 /* ************************************************************************* */
624 Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart) {
625  if (smart && std::abs(sigma-1.0)<1e-9) return Unit::Create(dim);
626  return shared_ptr(new Isotropic(dim, sigma));
627 }
628 
629 /* ************************************************************************* */
630 Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) {
631  if (smart && std::abs(variance-1.0)<1e-9) return Unit::Create(dim);
632  return shared_ptr(new Isotropic(dim, sqrt(variance)));
633 }
634 
635 /* ************************************************************************* */
636 void Isotropic::print(const string& name) const {
637  cout << "isotropic dim=" << dim() << " sigma=" << sigma_ << endl;
638 }
639 
640 /* ************************************************************************* */
642  return v.dot(v) * invsigma_ * invsigma_;
643 }
644 
645 /* ************************************************************************* */
647  return v * invsigma_;
648 }
649 
650 /* ************************************************************************* */
652  return v * sigma_;
653 }
654 
655 /* ************************************************************************* */
657  return invsigma_ * H;
658 }
659 
660 /* ************************************************************************* */
662  H *= invsigma_;
663 }
664 
665 /* ************************************************************************* */
667  v *= invsigma_;
668 }
669 
670 /* ************************************************************************* */
672  H *= invsigma_;
673 }
674 
675 /* *******************************************************************************/
676 double Isotropic::logDetR() const { return log(invsigma_) * dim(); }
677 
678 /* ************************************************************************* */
679 // Unit
680 /* ************************************************************************* */
681 void Unit::print(const std::string& name) const {
682  cout << name << "unit (" << dim_ << ") " << endl;
683 }
684 
685 /* ************************************************************************* */
687  return v.dot(v);
688 }
689 
690 /* *******************************************************************************/
691 double Unit::logDetR() const { return 0.0; }
692 
693 /* ************************************************************************* */
694 // Robust
695 /* ************************************************************************* */
696 
697 void Robust::print(const std::string& name) const {
698  robust_->print(name);
699  noise_->print(name);
700 }
701 
702 bool Robust::equals(const Base& expected, double tol) const {
703  const Robust* p = dynamic_cast<const Robust*> (&expected);
704  if (p == nullptr) return false;
705  return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
706 }
707 
709  noise_->whitenInPlace(b);
710  robust_->reweight(b);
711 }
712 
713 void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
714  noise_->WhitenSystem(A,b);
715  robust_->reweight(A,b);
716 }
717 
719  noise_->WhitenSystem(A,b);
720  robust_->reweight(A,b);
721 }
722 
724  noise_->WhitenSystem(A1,A2,b);
725  robust_->reweight(A1,A2,b);
726 }
727 
729  noise_->WhitenSystem(A1,A2,A3,b);
730  robust_->reweight(A1,A2,A3,b);
731 }
732 
734  return noise_->unweightedWhiten(v);
735 }
736 double Robust::weight(const Vector& v) const {
737  return robust_->weight(v.norm());
738 }
739 
741 const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
742  return shared_ptr(new Robust(robust,noise));
743 }
744 
745 /* ************************************************************************* */
746 } // namespace noiseModel
747 } // gtsam
gtsam::noiseModel::Isotropic::Isotropic
Isotropic()
Definition: NoiseModel.h:555
timing.h
Timing utilities.
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam::noiseModel::Constrained::MixedVariances
static shared_ptr MixedVariances(const Vector &mu, const Vector &variances)
Definition: NoiseModel.cpp:420
gtsam::noiseModel::Diagonal::unwhiten
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition: NoiseModel.cpp:326
H
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 set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::noiseModel::internal::fix
static void fix(const Vector &sigmas, Vector &precisions, Vector &invsigmas)
Definition: NoiseModel.cpp:354
gtsam::noiseModel::Diagonal::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.cpp:322
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
name
Annotation for function names.
Definition: attr.h:51
llt
EIGEN_DONT_INLINE void llt(const Mat &A, const Mat &B, Mat &C)
Definition: llt.cpp:5
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
A3
static const double A3[]
Definition: expn.h:8
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::noiseModel::Robust::robust
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition: NoiseModel.h:705
gtsam::noiseModel::Diagonal::Whiten
Matrix Whiten(const Matrix &H) const override
Definition: NoiseModel.cpp:330
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::noiseModel::Base
Definition: NoiseModel.h:57
gtsam::noiseModel::Constrained::MixedSigmas
static shared_ptr MixedSigmas(const Vector &mu, const Vector &sigmas)
Definition: NoiseModel.cpp:376
gtsam::noiseModel::Base::shared_ptr
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
gtsam::noiseModel::Constrained::unit
shared_ptr unit() const
Definition: NoiseModel.cpp:469
gtsam::noiseModel::Robust::unweightedWhiten
Vector unweightedWhiten(const Vector &v) const override
Definition: NoiseModel.cpp:733
gtsam::noiseModel::Diagonal::Precisions
static shared_ptr Precisions(const Vector &precisions, bool smart=true)
Definition: NoiseModel.cpp:311
gtsam::noiseModel::check_if_constraint
std::optional< size_t > check_if_constraint(VECTOR a, const Vector &invsigmas, size_t m)
Definition: NoiseModel.cpp:485
mu
double mu
Definition: testBoundingConstraint.cpp:37
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::noiseModel::Robust::Create
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:740
gtsam::noiseModel::Diagonal
Definition: NoiseModel.h:301
gtsam::noiseModel::Diagonal::Diagonal
Diagonal()
Definition: NoiseModel.cpp:272
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::noiseModel::Robust::print
void print(const std::string &name) const override
Definition: NoiseModel.cpp:697
gtsam::noiseModel::Isotropic::sigma_
double sigma_
Definition: NoiseModel.h:543
gtsam::noiseModel::Diagonal::invsigmas
const Vector & invsigmas() const
Definition: NoiseModel.h:361
Eigen::Upper
@ Upper
Definition: Constants.h:211
gtsam::noiseModel::Gaussian
Definition: NoiseModel.h:168
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::noiseModel::Diagonal::print
void print(const std::string &name) const override
Definition: NoiseModel.cpp:317
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::noiseModel::Isotropic::print
void print(const std::string &name) const override
Definition: NoiseModel.cpp:636
gtsam::noiseModel::Isotropic::Variance
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
Definition: NoiseModel.cpp:630
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
gtsam::noiseModel::updateAb
void updateAb(MATRIX &Ab, int j, const Vector &a, const Vector &rd)
Definition: NoiseModel.cpp:40
gtsam::noiseModel::Diagonal::WhitenInPlace
void WhitenInPlace(Matrix &H) const override
Definition: NoiseModel.cpp:334
gtsam::noiseModel::Base::dim
size_t dim() const
Dimensionality.
Definition: NoiseModel.h:79
gtsam::noiseModel::Constrained::Constrained
Constrained(const Vector &mu, const Vector &sigmas)
Definition: NoiseModel.cpp:370
gtsam::noiseModel::Robust::WhitenSystem
virtual void WhitenSystem(Vector &b) const
Definition: NoiseModel.cpp:708
gtsam::noiseModel::Gaussian::shared_ptr
std::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:191
gtsam::noiseModel::Constrained::QR
Diagonal::shared_ptr QR(Matrix &Ab) const override
Definition: NoiseModel.cpp:502
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
diagonal
void diagonal(const MatrixType &m)
Definition: diagonal.cpp:12
gtsam::noiseModel::Isotropic::logDetR
virtual double logDetR() const override
Compute the log of |R|. Used for computing log(|Σ|)
Definition: NoiseModel.cpp:676
gtsam::noiseModel::Isotropic::sigma
double sigma() const
Definition: NoiseModel.h:593
gtsam::noiseModel::Isotropic::whitenInPlace
void whitenInPlace(Vector &v) const override
Definition: NoiseModel.cpp:666
gtsam::noiseModel::Constrained::print
void print(const std::string &name) const override
Definition: NoiseModel.cpp:388
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::noiseModel::Isotropic::squaredMahalanobisDistance
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Definition: NoiseModel.cpp:641
gtsam::noiseModel::Diagonal::invsigmas_
Vector invsigmas_
Definition: NoiseModel.h:309
gtsam::noiseModel::Diagonal::sigmas
Vector sigmas() const override
Calculate standard deviations.
Definition: NoiseModel.h:346
gtsam::noiseModel::Diagonal::precisions_
Vector precisions_
Definition: NoiseModel.h:309
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::noiseModel::Constrained::mu_
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition: NoiseModel.h:408
gtsam::noiseModel::Constrained::mu
const Vector & mu() const
Access mu as a vector.
Definition: NoiseModel.h:438
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::noiseModel::Robust::Robust
Robust()
Default Constructor for serialization.
Definition: NoiseModel.h:692
gtsam::noiseModel::mEstimator::Base::shared_ptr
std::shared_ptr< Base > shared_ptr
Definition: LossFunctions.h:71
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
make_tuple
tuple make_tuple()
Definition: cast.h:1383
gtsam::vector_scale_inplace
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
Definition: Matrix.cpp:480
I
#define I
Definition: main.h:112
gtsam::noiseModel::Diagonal::precision
double precision(size_t i) const
Definition: NoiseModel.h:368
square
const EIGEN_DEVICE_FUNC SquareReturnType square() const
Definition: ArrayCwiseUnaryOps.h:425
gtsam::noiseModel::Isotropic::invsigma_
double invsigma_
Definition: NoiseModel.h:543
Eigen::all
static const Eigen::internal::all_t all
Definition: IndexedViewHelper.h:171
gtsam::internal::logDeterminant
double logDeterminant(const typename BAYESTREE::sharedClique &clique)
Definition: GaussianBayesTree-inl.h:42
debug
static constexpr bool debug
Definition: testDiscreteBayesTree.cpp:31
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::noiseModel::Unit::logDetR
virtual double logDetR() const override
Compute the log of |R|. Used for computing log(|Σ|)
Definition: NoiseModel.cpp:691
gtsam::noiseModel::Diagonal::sigmas_
Vector sigmas_
Definition: NoiseModel.h:309
isfinite
#define isfinite(X)
Definition: main.h:95
gtsam::noiseModel::Robust
Definition: NoiseModel.h:678
gtsam::noiseModel::Unit::squaredMahalanobisDistance
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Definition: NoiseModel.cpp:686
Eigen::StrictlyLower
@ StrictlyLower
Definition: Constants.h:221
gtsam::noiseModel::Isotropic::Whiten
Matrix Whiten(const Matrix &H) const override
Definition: NoiseModel.cpp:656
gtsam::noiseModel::Constrained::whiten
Vector whiten(const Vector &v) const override
Calculates error vector with weights applied.
Definition: NoiseModel.cpp:394
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::noiseModel::Robust::robust_
const RobustModel::shared_ptr robust_
robust error function used
Definition: NoiseModel.h:686
gtsam::noiseModel::Constrained::squaredMahalanobisDistance
double squaredMahalanobisDistance(const Vector &v) const override
Definition: NoiseModel.cpp:437
gtsam::noiseModel::Robust::weight
double weight(const Vector &v) const override
Definition: NoiseModel.cpp:736
gtsam::noiseModel::checkIfDiagonal
std::optional< Vector > checkIfDiagonal(const Matrix &M)
Definition: NoiseModel.cpp:47
gtsam::noiseModel::Diagonal::precisions
const Vector & precisions() const
Definition: NoiseModel.h:367
gtsam::noiseModel::Robust::noise_
const NoiseModel::shared_ptr noise_
noise model used
Definition: NoiseModel.h:687
gtsam::b
const G & b
Definition: Group.h:79
Eigen::LLT
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
Definition: LLT.h:66
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::inplace_QR
void inplace_QR(Matrix &A)
Definition: Matrix.cpp:636
gtsam::noiseModel::Isotropic::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.cpp:646
array
Definition: numpy.h:821
gtsam::backSubstituteUpper
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
Definition: Matrix.cpp:375
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
gtsam::noiseModel::Isotropic::WhitenInPlace
void WhitenInPlace(Matrix &H) const override
Definition: NoiseModel.cpp:661
gtsam::vector_scale
Matrix vector_scale(const Vector &v, const Matrix &A, bool inf_mask)
Definition: Matrix.cpp:496
gtsam::noiseModel::Diagonal::logDetR
virtual double logDetR() const override
Compute the log of |R|. Used for computing log(|Σ|)
Definition: NoiseModel.cpp:343
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
std
Definition: BFloat16.h:88
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
A1
static const double A1[]
Definition: expn.h:6
exampleQR::Rd
Matrix Rd
Definition: testNoiseModel.cpp:215
gtsam::noiseModel::Constrained::Whiten
Matrix Whiten(const Matrix &H) const override
Definition: NoiseModel.cpp:446
gtsam::noiseModel::Robust::shared_ptr
std::shared_ptr< Robust > shared_ptr
Definition: NoiseModel.h:680
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
gtsam::noiseModel::Constrained::MixedPrecisions
static shared_ptr MixedPrecisions(const Vector &mu, const Vector &precisions)
Definition: NoiseModel.cpp:428
gtsam::noiseModel::Unit::print
void print(const std::string &name) const override
Definition: NoiseModel.cpp:681
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::noiseModel::Constrained::constrained
bool constrained(size_t i) const
Return true if a particular dimension is free or constrained.
Definition: NoiseModel.cpp:382
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
setZero
v setZero(3)
abs
#define abs(x)
Definition: datatypes.h:17
internal
Definition: BandTriangularSolver.h:13
gtsam::noiseModel::Isotropic::shared_ptr
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:559
gtsam::noiseModel::Base::dim_
size_t dim_
Definition: NoiseModel.h:64
gtsam::noiseModel::Robust::noise
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition: NoiseModel.h:708
align_3::t
Point2 t(10, 10)
gtsam::noiseModel::Constrained::WhitenInPlace
void WhitenInPlace(Matrix &H) const override
Definition: NoiseModel.cpp:455
gtsam::noiseModel::Constrained::shared_ptr
std::shared_ptr< Constrained > shared_ptr
Definition: NoiseModel.h:419
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::noiseModel::Robust::equals
bool equals(const Base &expected, double tol=1e-9) const override
Definition: NoiseModel.cpp:702
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::noiseModel::Isotropic::unwhiten
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition: NoiseModel.cpp:651
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::noiseModel::Diagonal::Variances
static shared_ptr Variances(const Vector &variances, bool smart=true)
Definition: NoiseModel.cpp:283
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
isinf
#define isinf(X)
Definition: main.h:94
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:50