ShonanAveraging.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, 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 
19 #include <SymEigsSolver.h>
20 #include <cmath>
21 #include <gtsam/linear/PCGSolver.h>
27 #include <gtsam/sfm/ShonanFactor.h>
31 
32 #include <Eigen/Eigenvalues>
33 #include <algorithm>
34 #include <complex>
35 #include <iostream>
36 #include <map>
37 #include <random>
38 #include <set>
39 #include <vector>
40 
41 namespace gtsam {
42 
43 // In Wrappers we have no access to this so have a default ready
44 static std::mt19937 kRandomNumberGenerator(42);
45 
47 
48 /* ************************************************************************* */
49 template <size_t d>
51  const LevenbergMarquardtParams &_lm, const std::string &method,
52  double optimalityThreshold, double alpha, double beta, double gamma)
53  : lm(_lm),
54  optimalityThreshold(optimalityThreshold),
55  alpha(alpha),
56  beta(beta),
57  gamma(gamma),
58  useHuber(false),
59  certifyOptimality(true) {
60  // By default, we will do conjugate gradient
62 
63  // Create subgraph builder parameters
64  SubgraphBuilderParameters builderParameters;
68  builderParameters.augmentationFactor = 0.0;
69 
70  auto pcg = std::make_shared<PCGSolverParameters>();
71 
72  // Choose optimization method
73  if (method == "SUBGRAPH") {
75  std::make_shared<SubgraphSolverParameters>(builderParameters);
76  } else if (method == "SGPC") {
77  pcg->preconditioner_ =
78  std::make_shared<SubgraphPreconditionerParameters>(builderParameters);
79  lm.iterativeParams = pcg;
80  } else if (method == "JACOBI") {
81  pcg->preconditioner_ =
82  std::make_shared<BlockJacobiPreconditionerParameters>();
83  lm.iterativeParams = pcg;
84  } else if (method == "QR") {
85  lm.setLinearSolverType("MULTIFRONTAL_QR");
86  } else if (method == "CHOLESKY") {
87  lm.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
88  } else {
89  throw std::invalid_argument("ShonanAveragingParameters: unknown method");
90  }
91 }
92 
93 /* ************************************************************************* */
94 // Explicit instantiation for d=2 and d=3
95 template struct ShonanAveragingParameters<2>;
96 template struct ShonanAveragingParameters<3>;
97 
98 /* ************************************************************************* */
99 // Calculate number of unknown rotations referenced by measurements
100 // Throws exception of keys are not numbered as expected (may change in future).
101 template <size_t d>
102 static size_t NrUnknowns(
103  const typename ShonanAveraging<d>::Measurements &measurements) {
104  Key maxKey = 0;
105  std::set<Key> keys;
106  for (const auto &measurement : measurements) {
107  for (const Key &key : measurement.keys()) {
108  maxKey = std::max(key, maxKey);
109  keys.insert(key);
110  }
111  }
112  size_t N = keys.size();
113  if (maxKey != N - 1) {
114  throw std::invalid_argument(
115  "As of now, ShonanAveraging expects keys numbered 0..N-1");
116  }
117  return N;
118 }
119 
120 /* ************************************************************************* */
121 template <size_t d>
123  const Parameters &parameters)
124  : parameters_(parameters),
125  measurements_(measurements),
126  nrUnknowns_(NrUnknowns<d>(measurements)) {
127  for (const auto &measurement : measurements_) {
128  const auto &model = measurement.noiseModel();
129  if (model && model->dim() != SO<d>::dimension) {
130  measurement.print("Factor with incorrect noise model:\n");
131  throw std::invalid_argument(
132  "ShonanAveraging: measurements passed to "
133  "constructor have incorrect dimension.");
134  }
135  }
136  Q_ = buildQ();
137  D_ = buildD();
138  L_ = D_ - Q_;
139 }
140 
141 /* ************************************************************************* */
142 template <size_t d>
145  auto G = std::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));
146 
147  for (const auto &measurement : measurements_) {
148  const auto &keys = measurement.keys();
149  const auto &Rij = measurement.measured();
150  const auto &model = measurement.noiseModel();
151  graph.emplace_shared<ShonanFactor<d>>(keys[0], keys[1], Rij, p, model, G);
152  }
153  // Possibly add Karcher prior
154  if (parameters_.beta > 0) {
155  const size_t dim = SOn::Dimension(p);
156  graph.emplace_shared<KarcherMeanFactor<SOn>>(graph.keys(), dim);
157  }
158  // Possibly add gauge factors - they are probably useless as gradient is zero
159  if (parameters_.gamma > 0 && p > d + 1) {
160  for (auto key : graph.keys())
161  graph.emplace_shared<ShonanGaugeFactor>(key, p, d, parameters_.gamma);
162  }
163  return graph;
164 }
165 
166 /* ************************************************************************* */
167 template <size_t d>
168 double ShonanAveraging<d>::costAt(size_t p, const Values &values) const {
169  const NonlinearFactorGraph graph = buildGraphAt(p);
170  return graph.error(values);
171 }
172 
173 /* ************************************************************************* */
174 template <size_t d>
175 std::shared_ptr<LevenbergMarquardtOptimizer>
177  // Build graph
178  NonlinearFactorGraph graph = buildGraphAt(p);
179 
180  // Anchor prior is added here as depends on initial value (and cost is zero)
181  if (parameters_.alpha > 0) {
182  const size_t dim = SOn::Dimension(p);
183  const auto [i, value] = parameters_.anchor;
184  auto model = noiseModel::Isotropic::Precision(dim, parameters_.alpha);
185  graph.emplace_shared<PriorFactor<SOn>>(i, SOn::Lift(p, value.matrix()),
186  model);
187  }
188  // Optimize
189  return std::make_shared<LevenbergMarquardtOptimizer>(graph, initial,
190  parameters_.lm);
191 }
192 
193 /* ************************************************************************* */
194 template <size_t d>
196  const Values &initial) const {
197  auto lm = createOptimizerAt(p, initial);
198  return lm->optimize();
199 }
200 
201 /* ************************************************************************* */
202 // Project to pxdN Stiefel manifold
203 template <size_t d>
205  const size_t N = values.size();
206  const size_t p = values.at<SOn>(0).rows();
207  Matrix S(p, N * d);
208  for (const auto& it : values.extract<SOn>()) {
209  S.middleCols<d>(it.first * d) =
210  it.second.matrix().leftCols<d>(); // project Qj to Stiefel
211  }
212  return S;
213 }
214 
215 /* ************************************************************************* */
216 template <>
218  Values result;
219  for (const auto& it : values.extract<SOn>()) {
220  assert(it.second.rows() == p);
221  const auto &M = it.second.matrix();
222  const Rot2 R = Rot2::atan2(M(1, 0), M(0, 0));
223  result.insert(it.first, R);
224  }
225  return result;
226 }
227 
228 template <>
230  Values result;
231  for (const auto& it : values.extract<SOn>()) {
232  assert(it.second.rows() == p);
233  const auto &M = it.second.matrix();
234  const Rot3 R = Rot3::ClosestTo(M.topLeftCorner<3, 3>());
235  result.insert(it.first, R);
236  }
237  return result;
238 }
239 
240 /* ************************************************************************* */
241 template <size_t d>
242 static Matrix RoundSolutionS(const Matrix &S) {
243  const size_t N = S.cols() / d;
244  // First, compute a thin SVD of S
246  const Vector sigmas = svd.singularValues();
247 
248  // Construct a diagonal matrix comprised of the first d singular values
249  using DiagonalMatrix = Eigen::DiagonalMatrix<double, d>;
250  DiagonalMatrix Sigma_d;
251  Sigma_d.diagonal() = sigmas.head<d>();
252 
253  // Now, construct a rank-d truncated singular value decomposition for S
254  Matrix R = Sigma_d * svd.matrixV().leftCols<d>().transpose();
255 
256  // Count the number of blocks whose determinants have positive sign
257  size_t numPositiveBlocks = 0;
258  for (size_t i = 0; i < N; ++i) {
259  // Compute the determinant of the ith dxd block of R
260  double determinant = R.middleCols<d>(d * i).determinant();
261  if (determinant > 0) ++numPositiveBlocks;
262  }
263 
264  if (numPositiveBlocks < N / 2) {
265  // Less than half of the total number of blocks have the correct sign.
266  // To reverse their orientations, multiply with a reflection matrix.
267  DiagonalMatrix reflector;
268  reflector.setIdentity();
269  reflector.diagonal()(d - 1) = -1;
270  R = reflector * R;
271  }
272 
273  return R;
274 }
275 
276 /* ************************************************************************* */
277 template <>
279  // Round to a 2*2N matrix
280  Matrix R = RoundSolutionS<2>(S);
281 
282  // Finally, project each dxd rotation block to SO(2)
283  Values values;
284  for (size_t j = 0; j < nrUnknowns(); ++j) {
285  const Rot2 Ri = Rot2::atan2(R(1, 2 * j), R(0, 2 * j));
286  values.insert(j, Ri);
287  }
288  return values;
289 }
290 
291 template <>
293  // Round to a 3*3N matrix
294  Matrix R = RoundSolutionS<3>(S);
295 
296  // Finally, project each dxd rotation block to SO(3)
297  Values values;
298  for (size_t j = 0; j < nrUnknowns(); ++j) {
299  const Rot3 Ri = Rot3::ClosestTo(R.middleCols<3>(3 * j));
300  values.insert(j, Ri);
301  }
302  return values;
303 }
304 
305 /* ************************************************************************* */
306 template <size_t d>
308  // Project to pxdN Stiefel manifold...
309  Matrix S = StiefelElementMatrix(values);
310  // ...and call version above.
311  return roundSolutionS(S);
312 }
313 
314 /* ************************************************************************* */
315 template <size_t d>
316 double ShonanAveraging<d>::cost(const Values &values) const {
318  for (const auto &measurement : measurements_) {
319  const auto &keys = measurement.keys();
320  const auto &Rij = measurement.measured();
321  const auto &model = measurement.noiseModel();
323  keys[0], keys[1], SO<d>(Rij.matrix()), model);
324  }
325  // Finally, project each dxd rotation block to SO(d)
326  Values result;
327  for (const auto& it : values.extract<Rot>()) {
328  result.insert(it.first, SO<d>(it.second.matrix()));
329  }
330  return graph.error(result);
331 }
332 
333 /* ************************************************************************* */
334 // Get kappa from noise model
335 template <typename T, size_t d>
338  const auto &isotropic = std::dynamic_pointer_cast<noiseModel::Isotropic>(
339  measurement.noiseModel());
340  double sigma;
341  if (isotropic) {
342  sigma = isotropic->sigma();
343  } else {
344  const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
345  measurement.noiseModel());
346  // Check if noise model is robust
347  if (robust) {
348  // If robust, check if optimality certificate is expected
349  if (parameters.getCertifyOptimality()) {
350  throw std::invalid_argument(
351  "Certification of optimality does not work with robust cost.");
352  } else {
353  // Optimality certificate not required, so setting default sigma
354  sigma = 1;
355  }
356  } else {
357  throw std::invalid_argument(
358  "Shonan averaging noise models must be isotropic (but robust losses "
359  "are allowed).");
360  }
361  }
362  return 1.0 / (sigma * sigma);
363 }
364 
365 /* ************************************************************************* */
366 template <size_t d>
368  // Each measurement contributes 2*d elements along the diagonal of the
369  // degree matrix.
370  static constexpr size_t stride = 2 * d;
371 
372  // Reserve space for triplets
373  std::vector<Eigen::Triplet<double>> triplets;
374  triplets.reserve(stride * measurements_.size());
375 
376  for (const auto &measurement : measurements_) {
377  // Get pose keys
378  const auto &keys = measurement.keys();
379 
380  // Get kappa from noise model
381  double kappa = Kappa<Rot, d>(measurement, parameters_);
382 
383  const size_t di = d * keys[0], dj = d * keys[1];
384  for (size_t k = 0; k < d; k++) {
385  // Elements of ith block-diagonal
386  triplets.emplace_back(di + k, di + k, kappa);
387  // Elements of jth block-diagonal
388  triplets.emplace_back(dj + k, dj + k, kappa);
389  }
390  }
391 
392  // Construct and return a sparse matrix from these triplets
393  const size_t dN = d * nrUnknowns();
394  Sparse D(dN, dN);
395  D.setFromTriplets(triplets.begin(), triplets.end());
396 
397  return D;
398 }
399 
400 /* ************************************************************************* */
401 template <size_t d>
403  // Each measurement contributes 2*d^2 elements on a pair of symmetric
404  // off-diagonal blocks
405  static constexpr size_t stride = 2 * d * d;
406 
407  // Reserve space for triplets
408  std::vector<Eigen::Triplet<double>> triplets;
409  triplets.reserve(stride * measurements_.size());
410 
411  for (const auto &measurement : measurements_) {
412  // Get pose keys
413  const auto &keys = measurement.keys();
414 
415  // Extract rotation measurement
416  const auto Rij = measurement.measured().matrix();
417 
418  // Get kappa from noise model
419  double kappa = Kappa<Rot, d>(measurement, parameters_);
420 
421  const size_t di = d * keys[0], dj = d * keys[1];
422  for (size_t r = 0; r < d; r++) {
423  for (size_t c = 0; c < d; c++) {
424  // Elements of ij block
425  triplets.emplace_back(di + r, dj + c, kappa * Rij(r, c));
426  // Elements of ji block
427  triplets.emplace_back(dj + r, di + c, kappa * Rij(c, r));
428  }
429  }
430  }
431 
432  // Construct and return a sparse matrix from these triplets
433  const size_t dN = d * nrUnknowns();
434  Sparse Q(dN, dN);
435  Q.setFromTriplets(triplets.begin(), triplets.end());
436 
437  return Q;
438 }
439 
440 /* ************************************************************************* */
441 template <size_t d>
443  // Each pose contributes 2*d elements along the diagonal of Lambda
444  static constexpr size_t stride = d * d;
445 
446  // Reserve space for triplets
447  const size_t N = nrUnknowns();
448  std::vector<Eigen::Triplet<double>> triplets;
449  triplets.reserve(stride * N);
450 
451  // Do sparse-dense multiply to get Q*S'
452  auto QSt = Q_ * S.transpose();
453 
454  for (size_t j = 0; j < N; j++) {
455  // Compute B, the building block for the j^th diagonal block of Lambda
456  const size_t dj = d * j;
457  Matrix B = QSt.middleRows(dj, d) * S.middleCols<d>(dj);
458 
459  // Elements of jth block-diagonal
460  for (size_t r = 0; r < d; r++)
461  for (size_t c = 0; c < d; c++)
462  triplets.emplace_back(dj + r, dj + c, 0.5 * (B(r, c) + B(c, r)));
463  }
464 
465  // Construct and return a sparse matrix from these triplets
466  Sparse Lambda(d * N, d * N);
467  Lambda.setFromTriplets(triplets.begin(), triplets.end());
468  return Lambda;
469 }
470 
471 /* ************************************************************************* */
472 template <size_t d>
474  // Project to pxdN Stiefel manifold...
475  Matrix S = StiefelElementMatrix(values);
476  // ...and call version above.
477  return computeLambda(S);
478 }
479 
480 /* ************************************************************************* */
481 template <size_t d>
483  assert(values.size() == nrUnknowns());
484  const Matrix S = StiefelElementMatrix(values);
485  auto Lambda = computeLambda(S);
486  return Lambda - Q_;
487 }
488 
489 /* ************************************************************************* */
490 template <size_t d>
492  auto Lambda = computeLambda(S);
493  return Lambda - Q_;
494 }
495 
496 /* ************************************************************************* */
497 // Perturb the initial initialVector by adding a spherically-uniformly
498 // distributed random vector with 0.03*||initialVector||_2 magnitude to
499 // initialVector
500 // ref : Part III. C, Rosen, D. and Carlone, L., 2017, September. Computational
501 // enhancements for certifiably correct SLAM. In Proceedings of the
502 // International Conference on Intelligent Robots and Systems.
503 static Vector perturb(const Vector &initialVector) {
504  // generate a 0.03*||x_0||_2 as stated in David's paper
505  int n = initialVector.rows();
506  Vector disturb = Vector::Random(n);
507  disturb.normalize();
508 
509  double magnitude = initialVector.norm();
510  Vector perturbedVector = initialVector + 0.03 * magnitude * disturb;
511  perturbedVector.normalize();
512  return perturbedVector;
513 }
514 
515 /* ************************************************************************* */
517 // Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization,
518 // it takes in the certificate matrix A as input, the maxIterations and the
519 // minEigenvalueNonnegativityTolerance is set to 1000 and 10e-4 ad default,
520 // there are two parts
521 // in this algorithm:
522 // (1) compute the maximum eigenpair (\lamda_dom, \vect{v}_dom) of A by power
523 // method. if \lamda_dom is less than zero, then return the eigenpair. (2)
524 // compute the maximum eigenpair (\theta, \vect{v}) of C = \lamda_dom * I - A by
525 // accelerated power method. Then return (\lamda_dom - \theta, \vect{v}).
527  const Sparse &A, const Matrix &S, double &minEigenValue,
528  Vector *minEigenVector = 0, size_t *numIterations = 0,
529  size_t maxIterations = 1000,
530  double minEigenvalueNonnegativityTolerance = 10e-4) {
531 
532  // a. Compute dominant eigenpair of S using power method
533  PowerMethod<Sparse> pmOperator(A);
534 
535  const bool pmConverged = pmOperator.compute(
536  maxIterations, 1e-5);
537 
538  // Check convergence and bail out if necessary
539  if (!pmConverged) return false;
540 
541  const double pmEigenValue = pmOperator.eigenvalue();
542 
543  if (pmEigenValue < 0) {
544  // The largest-magnitude eigenvalue is negative, and therefore also the
545  // minimum eigenvalue, so just return this solution
546  minEigenValue = pmEigenValue;
547  if (minEigenVector) {
548  *minEigenVector = pmOperator.eigenvector();
549  minEigenVector->normalize(); // Ensure that this is a unit vector
550  }
551  return true;
552  }
553 
554  const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()).sparseView() - A;
555  const std::optional<Vector> initial = perturb(S.row(0));
556  AcceleratedPowerMethod<Sparse> apmShiftedOperator(C, initial);
557 
558  const bool minConverged = apmShiftedOperator.compute(
559  maxIterations, minEigenvalueNonnegativityTolerance / pmEigenValue);
560 
561  if (!minConverged) return false;
562 
563  minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue();
564  if (minEigenVector) {
565  *minEigenVector = apmShiftedOperator.eigenvector();
566  minEigenVector->normalize(); // Ensure that this is a unit vector
567  }
568  if (numIterations) *numIterations = apmShiftedOperator.nrIterations();
569  return true;
570 }
571 
577  // Const reference to an externally-held matrix whose minimum-eigenvalue we
578  // want to compute
579  const Sparse &A_;
580 
581  // Spectral shift
582  double sigma_;
583 
584  // Constructor
585  explicit MatrixProdFunctor(const Sparse &A, double sigma = 0)
586  : A_(A), sigma_(sigma) {}
587 
588  int rows() const { return A_.rows(); }
589  int cols() const { return A_.cols(); }
590 
591  // Matrix-vector multiplication operation
592  void perform_op(const double *x, double *y) const {
593  // Wrap the raw arrays as Eigen Vector types
595  Eigen::Map<Vector> Y(y, rows());
596 
597  // Do the multiplication using wrapped Eigen vectors
598  Y = A_ * X + sigma_ * X;
599  }
600 };
601 
613 
622 
623 // For the defaults, David Rosen says:
624 // - maxIterations refers to the max number of Lanczos iterations to run;
625 // ~1000 should be sufficiently large
626 // - We've been using 10^-4 for the nonnegativity tolerance
627 // - for numLanczosVectors, 20 is a good default value
628 
630  const Sparse &A, const Matrix &S, double *minEigenValue,
631  Vector *minEigenVector = 0, size_t *numIterations = 0,
632  size_t maxIterations = 1000,
633  double minEigenvalueNonnegativityTolerance = 10e-4,
634  Eigen::Index numLanczosVectors = 20) {
635  // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos
636  MatrixProdFunctor lmOperator(A);
639  lmEigenValueSolver(&lmOperator, 1, std::min(numLanczosVectors, A.rows()));
640  lmEigenValueSolver.init();
641 
642  const int lmConverged = lmEigenValueSolver.compute(
643  maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
644 
645  // Check convergence and bail out if necessary
646  if (lmConverged != 1) return false;
647 
648  const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
649 
650  if (lmEigenValue < 0) {
651  // The largest-magnitude eigenvalue is negative, and therefore also the
652  // minimum eigenvalue, so just return this solution
653  *minEigenValue = lmEigenValue;
654  if (minEigenVector) {
655  *minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
656  minEigenVector->normalize(); // Ensure that this is a unit vector
657  }
658  return true;
659  }
660 
661  // The largest-magnitude eigenvalue is positive, and is therefore the
662  // maximum eigenvalue. Therefore, after shifting the spectrum of A
663  // by -2*lmEigenValue (by forming A - 2*lambda_max*I), the shifted
664  // spectrum will lie in the interval [minEigenValue(A) - 2* lambda_max(A),
665  // -lambda_max*A]; in particular, the largest-magnitude eigenvalue of
666  // A - 2*lambda_max*I is minEigenValue - 2*lambda_max, with corresponding
667  // eigenvector v_min
668 
669  MatrixProdFunctor minShiftedOperator(A, -2 * lmEigenValue);
670 
673  minEigenValueSolver(&minShiftedOperator, 1,
674  std::min(numLanczosVectors, A.rows()));
675 
676  // If S is a critical point of F, then S^T is also in the null space of S -
677  // Lambda(S) (cf. Lemma 6 of the tech report), and therefore its rows are
678  // eigenvectors corresponding to the eigenvalue 0. In the case that the
679  // relaxation is exact, this is the *minimum* eigenvalue, and therefore the
680  // rows of S are exactly the eigenvectors that we're looking for. On the
681  // other hand, if the relaxation is *not* exact, then S - Lambda(S) has at
682  // least one strictly negative eigenvalue, and the rows of S are *unstable
683  // fixed points* for the Lanczos iterations. Thus, we will take a slightly
684  // "fuzzed" version of the first row of S as an initialization for the
685  // Lanczos iterations; this allows for rapid convergence in the case that
686  // the relaxation is exact (since are starting close to a solution), while
687  // simultaneously allowing the iterations to escape from this fixed point in
688  // the case that the relaxation is not exact.
689  Vector v0 = S.row(0).transpose();
690  Vector perturbation(v0.size());
691  perturbation.setRandom();
692  perturbation.normalize();
693  Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3%
694 
695  // Use this to initialize the eigensolver
696  minEigenValueSolver.init(xinit.data());
697 
698  // Now determine the relative precision required in the Lanczos method in
699  // order to be able to estimate the smallest eigenvalue within an *absolute*
700  // tolerance of 'minEigenvalueNonnegativityTolerance'
701  const int minConverged = minEigenValueSolver.compute(
702  maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
703  Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
704 
705  if (minConverged != 1) return false;
706 
707  *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
708  if (minEigenVector) {
709  *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
710  minEigenVector->normalize(); // Ensure that this is a unit vector
711  }
712  if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
713  return true;
714 }
715 
716 /* ************************************************************************* */
717 template <size_t d>
719  Vector *minEigenVector) const {
720  assert(values.size() == nrUnknowns());
721  const Matrix S = StiefelElementMatrix(values);
722  auto A = computeA(S);
723 
724  double minEigenValue;
725  bool success = SparseMinimumEigenValue(A, S, &minEigenValue, minEigenVector);
726  if (!success) {
727  throw std::runtime_error(
728  "SparseMinimumEigenValue failed to compute minimum eigenvalue.");
729  }
730  return minEigenValue;
731 }
732 
733 /* ************************************************************************* */
734 template <size_t d>
736  Vector *minEigenVector) const {
737  assert(values.size() == nrUnknowns());
738  const Matrix S = StiefelElementMatrix(values);
739  auto A = computeA(S);
740 
741  double minEigenValue = 0;
742  bool success = PowerMinimumEigenValue(A, S, minEigenValue, minEigenVector);
743  if (!success) {
744  throw std::runtime_error(
745  "PowerMinimumEigenValue failed to compute minimum eigenvalue.");
746  }
747  return minEigenValue;
748 }
749 
750 /* ************************************************************************* */
751 template <size_t d>
753  const Values &values) const {
754  Vector minEigenVector;
755  double minEigenValue = computeMinEigenValue(values, &minEigenVector);
756  return {minEigenValue, minEigenVector};
757 }
758 
759 /* ************************************************************************* */
760 template <size_t d>
762  double minEigenValue = computeMinEigenValue(values);
763  return minEigenValue > parameters_.optimalityThreshold;
764 }
765 
766 /* ************************************************************************* */
767 template <size_t d>
769  const Vector &v) {
771  // Create a tangent direction xi with eigenvector segment v_i
772  const size_t dimension = SOn::Dimension(p);
773  double sign0 = pow(-1.0, round((p + 1) / 2) + 1);
774  for (size_t i = 0; i < v.size() / d; i++) {
775  // Assumes key is 0-based integer
776  const auto v_i = v.segment<d>(d * i);
777  Vector xi = Vector::Zero(dimension);
778  double sign = sign0;
779  for (size_t j = 0; j < d; j++) {
780  xi(j + p - d - 1) = sign * v_i(d - j - 1);
781  sign = -sign;
782  }
783  delta.insert(i, xi);
784  }
785  return delta;
786 }
787 
788 /* ************************************************************************* */
789 template <size_t d>
791  const Values &values) const {
792  Matrix S_dot = StiefelElementMatrix(values);
793  // calculate the gradient of F(Q_dot) at Q_dot
794  Matrix euclideanGradient = 2 * (L_ * (S_dot.transpose())).transpose();
795  // cout << "euclidean gradient rows and cols" << euclideanGradient.rows() <<
796  // "\t" << euclideanGradient.cols() << endl;
797 
798  // project the gradient onto the entire euclidean space
799  Matrix symBlockDiagProduct(p, d * nrUnknowns());
800  for (size_t i = 0; i < nrUnknowns(); i++) {
801  // Compute block product
802  const size_t di = d * i;
803  const Matrix P = S_dot.middleCols<d>(di).transpose() *
804  euclideanGradient.middleCols<d>(di);
805  // Symmetrize this block
806  Matrix S = .5 * (P + P.transpose());
807  // Compute S_dot * S and set corresponding block
808  symBlockDiagProduct.middleCols<d>(di) = S_dot.middleCols<d>(di) * S;
809  }
810  Matrix riemannianGradient = euclideanGradient - symBlockDiagProduct;
811  return riemannianGradient;
812 }
813 
814 /* ************************************************************************* */
815 template <size_t d>
817  const Vector &minEigenVector) {
818  Values lifted = LiftTo<SOn>(p, values);
819  VectorValues delta = TangentVectorValues(p, minEigenVector);
820  return lifted.retract(delta);
821 }
822 
823 /* ************************************************************************* */
824 template <size_t d>
826  size_t p, const Values &values, const Vector &minEigenVector,
827  double minEigenValue, double gradienTolerance,
828  double preconditionedGradNormTolerance) const {
829  double funcVal = costAt(p - 1, values);
830  double alphaMin = 1e-2;
831  double alpha =
832  std::max(1024 * alphaMin, 10 * gradienTolerance / fabs(minEigenValue));
833  vector<double> alphas;
834  vector<double> fvals;
835  // line search
836  while ((alpha >= alphaMin)) {
837  Values Qplus = LiftwithDescent(p, values, alpha * minEigenVector);
838  double funcValTest = costAt(p, Qplus);
839  Matrix gradTest = riemannianGradient(p, Qplus);
840  double gradTestNorm = gradTest.norm();
841  // Record alpha and funcVal
842  alphas.push_back(alpha);
843  fvals.push_back(funcValTest);
844  if ((funcVal > funcValTest) && (gradTestNorm > gradienTolerance)) {
845  return Qplus;
846  }
847  alpha /= 2;
848  }
849 
850  auto fminIter = min_element(fvals.begin(), fvals.end());
851  auto minIdx = distance(fvals.begin(), fminIter);
852  double fMin = fvals[minIdx];
853  double aMin = alphas[minIdx];
854  if (fMin < funcVal) {
855  Values Qplus = LiftwithDescent(p, values, aMin * minEigenVector);
856  return Qplus;
857  }
858 
859  return LiftwithDescent(p, values, alpha * minEigenVector);
860 }
861 
862 /* ************************************************************************* */
863 template <size_t d>
865  Values initial;
866  for (size_t j = 0; j < nrUnknowns(); j++) {
867  initial.insert(j, Rot::Random(rng));
868  }
869  return initial;
870 }
871 
872 /* ************************************************************************* */
873 template <size_t d>
875  return initializeRandomly(kRandomNumberGenerator);
876 }
877 
878 /* ************************************************************************* */
879 template <size_t d>
881  std::mt19937 &rng) const {
882  const Values randomRotations = initializeRandomly(rng);
883  return LiftTo<Rot3>(p, randomRotations); // lift to p!
884 }
885 
886 /* ************************************************************************* */
887 template <size_t d>
889  return initializeRandomlyAt(p, kRandomNumberGenerator);
890 }
891 
892 /* ************************************************************************* */
893 template <size_t d>
894 std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
895  size_t pMin,
896  size_t pMax) const {
897  Values Qstar;
898  Values initialSOp = LiftTo<Rot>(pMin, initialEstimate); // lift to pMin!
899  for (size_t p = pMin; p <= pMax; p++) {
900  // Optimize until convergence at this level
901  Qstar = tryOptimizingAt(p, initialSOp);
902  if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) {
903  // in this case, there is no optimality certification
904  if (pMin != pMax) {
905  throw std::runtime_error(
906  "When using robust norm, Shonan only tests a single rank. Set pMin = pMax");
907  }
908  const Values SO3Values = roundSolution(Qstar);
909  return {SO3Values, 0};
910  } else {
911  // Check certificate of global optimality
912  Vector minEigenVector;
913  double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
914  if (minEigenValue > parameters_.optimalityThreshold) {
915  // If at global optimum, round and return solution
916  const Values SO3Values = roundSolution(Qstar);
917  return {SO3Values, minEigenValue};
918  }
919 
920  // Not at global optimimum yet, so check whether we will go to next level
921  if (p != pMax) {
922  // Calculate initial estimate for next level by following minEigenVector
923  initialSOp =
924  initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
925  }
926  }
927  }
928  throw std::runtime_error("Shonan::run did not converge for given pMax");
929 }
930 
931 /* ************************************************************************* */
932 // Explicit instantiation for d=2
933 template class ShonanAveraging<2>;
934 
935 ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
936  const Parameters &parameters)
937  : ShonanAveraging<2>(maybeRobust(measurements, parameters.getUseHuber()),
938  parameters) {}
939 
942  parameters.getUseHuber()),
943  parameters) {}
944 
945 // Extract Rot2 measurement from Pose2 betweenfactors
946 // Modeled after similar function in dataset.cpp
949  auto gaussian =
950  std::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
951  if (!gaussian)
952  throw std::invalid_argument(
953  "parseMeasurements<Rot2> can only convert Pose2 measurements "
954  "with Gaussian noise models.");
955  const Matrix3 M = gaussian->covariance();
956  // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance
957  // because the tangent space of Pose2 is ordered as (vx, vy, w)
958  auto model = noiseModel::Isotropic::Variance(1, M(2, 2));
959  return BinaryMeasurement<Rot2>(f->key<1>(), f->key<2>(), f->measured().rotation(),
960  model);
961 }
962 
964  const BetweenFactorPose2s &factors) {
966  result.reserve(factors.size());
967  for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f));
968  return result;
969 }
970 
972  const Parameters &parameters)
974  parameters.getUseHuber()),
975  parameters) {}
976 
977 /* ************************************************************************* */
978 // Explicit instantiation for d=3
979 template class ShonanAveraging<3>;
980 
982  const Parameters &parameters)
983  : ShonanAveraging<3>(maybeRobust(measurements, parameters.getUseHuber()),
984  parameters) {}
985 
988  parameters.getUseHuber()),
989  parameters) {}
990 
991 // TODO(frank): Deprecate after we land pybind wrapper
992 
993 // Extract Rot3 measurement from Pose3 betweenfactors
994 // Modeled after similar function in dataset.cpp
997  auto gaussian =
998  std::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
999  if (!gaussian)
1000  throw std::invalid_argument(
1001  "parseMeasurements<Rot3> can only convert Pose3 measurements "
1002  "with Gaussian noise models.");
1003  const Matrix6 M = gaussian->covariance();
1004  // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance
1005  // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's
1006  auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0));
1007  return BinaryMeasurement<Rot3>(f->key<1>(), f->key<2>(), f->measured().rotation(),
1008  model);
1009 }
1010 
1012  const BetweenFactorPose3s &factors) {
1014  result.reserve(factors.size());
1015  for (auto f : factors) result.push_back(convert(f));
1016  return result;
1017 }
1018 
1020  const Parameters &parameters)
1022  parameters.getUseHuber()),
1023  parameters) {}
1024 
1025 /* ************************************************************************* */
1026 } // namespace gtsam
const gtsam::Symbol key('X', 0)
enum gtsam::SubgraphBuilderParameters::Skeleton skeletonType
const SharedNoiseModel & noiseModel() const
Index cols() const
Definition: SparseMatrix.h:140
const char Y
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
#define max(a, b)
Definition: datatypes.h:20
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
static std::mt19937 kRandomNumberGenerator(42)
Main factor type in Shonan averaging, on SO(n) pairs.
KeySet keys() const
void setLinearSolverType(const std::string &solver)
Quaternion Q
Scalar * y
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:114
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
Definition: dataset.h:218
Factor Graph consisting of non-linear factors.
JacobiRotation< float > G
EIGEN_DEVICE_FUNC const DiagonalVectorType & diagonal() const
noiseModel::Diagonal::shared_ptr model
const ValueType at(Key j) const
Definition: Values-inl.h:204
Index rows() const
Definition: SparseMatrix.h:138
#define min(a, b)
Definition: datatypes.h:19
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
Compute maximum Eigenpair with accelerated power method.
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
static bool SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, Vector *minEigenVector=0, size_t *numIterations=0, size_t maxIterations=1000, double minEigenvalueNonnegativityTolerance=10e-4, Eigen::Index numLanczosVectors=20)
static std::mt19937 rng
MatrixProdFunctor(const Sparse &A, double sigma=0)
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Rot2 R(Rot2::fromAngle(0.1))
leaf::MyValues values
Represents a diagonal matrix with its storage.
bool compute(size_t maxIterations, double tol)
Definition: PowerMethod.h:130
const GaussianFactorGraph factors
Double_ distance(const OrientedPlane3_ &p)
Values initial
LevenbergMarquardtParams lm
LM parameters.
iterator insert(const std::pair< Key, Vector > &key_value)
static Matrix RoundSolutionS(const Matrix &S)
#define N
Definition: gksort.c:12
typename Parameters::Rot Rot
DiscreteKey S(1, 2)
Real fabs(const Real &a)
NonlinearFactorGraph graph
Eigen::SparseMatrix< double > Sparse
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Key maxKey(const PROBLEM &problem)
Definition: SOn.h:54
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
Definition: NoiseModel.cpp:600
ShonanAveraging2(const Measurements &measurements, const Parameters &parameters=Parameters())
ShonanAveragingParameters(const LevenbergMarquardtParams &lm=LevenbergMarquardtParams::CeresDefaults(), const std::string &method="JACOBI", double optimalityThreshold=-1e-4, double alpha=0.0, double beta=1.0, double gamma=0.0)
static Point2 measurement(323.0, 240.0)
ShonanAveraging(const Measurements &measurements, const Parameters &parameters=Parameters())
Parameters governing optimization etc.
Factor used in Shonan Averaging to clamp down gauge freedom.
void perform_op(const double *x, double *y) const
double augmentationFactor
factor multiplied with n, yields number of extra edges.
const SharedNoiseModel & noiseModel() const
access to the noise model
std::shared_ptr< BetweenFactor > shared_ptr
Definition: BetweenFactor.h:63
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
static BinaryMeasurement< Rot2 > convertPose2ToBinaryMeasurementRot2(const BetweenFactor< Pose2 >::shared_ptr &f)
static double Kappa(const BinaryMeasurement< T > &measurement, const ShonanAveragingParameters< d > &parameters)
Compute maximum Eigenpair with power method.
Definition: PowerMethod.h:58
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
Array< int, Dynamic, 1 > v
EIGEN_DEVICE_FUNC const SignReturnType sign() const
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T >> &measurements, bool useRobustModel=false) const
RealScalar alpha
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static size_t NrUnknowns(const typename ShonanAveraging< d >::Measurements &measurements)
bool perturb
static bool PowerMinimumEigenValue(const Sparse &A, const Matrix &S, double &minEigenValue, Vector *minEigenVector=0, size_t *numIterations=0, size_t maxIterations=1000, double minEigenvalueNonnegativityTolerance=10e-4)
MINIMUM EIGENVALUE COMPUTATIONS.
size_t size() const
Definition: Values.h:178
IterativeOptimizationParameters::shared_ptr iterativeParams
The container for iterativeOptimization parameters. used in CG Solvers.
double error(const Values &values) const
Various factors that minimize some Frobenius norm.
static ConjugateGradientParameters parameters
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
Vector xi
Definition: testPose2.cpp:148
traits
Definition: chartTesting.h:28
enum gtsam::SubgraphBuilderParameters::SkeletonWeight skeletonWeight
double eigenvalue() const
Return the eigenvalue.
Definition: PowerMethod.h:147
void setFromTriplets(const InputIterators &begin, const InputIterators &end)
ShonanAveraging3(const Measurements &measurements, const Parameters &parameters=Parameters())
enum gtsam::SubgraphBuilderParameters::AugmentationWeight augmentationWeight
std::vector< BinaryMeasurement< Rot > > Measurements
static const double v0
static ShonanAveraging2::Measurements extractRot2Measurements(const BetweenFactorPose2s &factors)
Two-sided Jacobi SVD decomposition of a rectangular matrix.
float * p
static ShonanAveraging3::Measurements extractRot3Measurements(const BetweenFactorPose3s &factors)
const SingularValuesType & singularValues() const
Definition: SVDBase.h:129
static const double sigma
void insert(Key j, const Value &val)
Definition: Values.cpp:155
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
Definition: dataset.h:212
const KeyVector keys
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
#define X
Definition: icosphere.cpp:20
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
const VALUE & measured() const
return the measurement
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
LinearSolverType linearSolverType
The type of linear solver to use in the nonlinear optimizer.
std::ptrdiff_t j
static BinaryMeasurement< Rot3 > convert(const BetweenFactor< Pose3 >::shared_ptr &f)
Vector eigenvector() const
Return the eigenvector.
Definition: PowerMethod.h:150
Shonan Averaging algorithm.
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose2 > > parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:396
EIGEN_DEVICE_FUNC const RoundReturnType round() const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:42