32 #include <Eigen/Eigenvalues>
52 double optimalityThreshold,
double alpha,
double beta,
double gamma)
54 optimalityThreshold(optimalityThreshold),
59 certifyOptimality(true) {
70 auto pcg = std::make_shared<PCGSolverParameters>();
73 if (method ==
"SUBGRAPH") {
75 std::make_shared<SubgraphSolverParameters>(builderParameters);
76 }
else if (method ==
"SGPC") {
77 pcg->preconditioner_ =
78 std::make_shared<SubgraphPreconditionerParameters>(builderParameters);
80 }
else if (method ==
"JACOBI") {
81 pcg->preconditioner_ =
82 std::make_shared<BlockJacobiPreconditionerParameters>();
84 }
else if (method ==
"QR") {
86 }
else if (method ==
"CHOLESKY") {
89 throw std::invalid_argument(
"ShonanAveragingParameters: unknown method");
112 size_t N =
keys.size();
114 throw std::invalid_argument(
115 "As of now, ShonanAveraging expects keys numbered 0..N-1");
125 measurements_(measurements),
130 measurement.print(
"Factor with incorrect noise model:\n");
131 throw std::invalid_argument(
132 "ShonanAveraging: measurements passed to "
133 "constructor have incorrect dimension.");
154 if (parameters_.beta > 0) {
155 const size_t dim = SOn::Dimension(
p);
159 if (parameters_.gamma > 0 &&
p >
d + 1) {
175 std::shared_ptr<LevenbergMarquardtOptimizer>
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);
189 return std::make_shared<LevenbergMarquardtOptimizer>(
graph,
initial,
197 auto lm = createOptimizerAt(
p,
initial);
198 return lm->optimize();
209 S.middleCols<
d>(it.first *
d) =
210 it.second.matrix().leftCols<
d>();
220 assert(it.second.rows() ==
p);
221 const auto &
M = it.second.matrix();
232 assert(it.second.rows() ==
p);
233 const auto &
M = it.second.matrix();
234 const Rot3 R = Rot3::ClosestTo(
M.topLeftCorner<3, 3>());
243 const size_t N =
S.cols() /
d;
250 DiagonalMatrix Sigma_d;
254 Matrix R = Sigma_d *
svd.matrixV().leftCols<
d>().transpose();
257 size_t numPositiveBlocks = 0;
258 for (
size_t i = 0;
i <
N; ++
i) {
264 if (numPositiveBlocks <
N / 2) {
267 DiagonalMatrix reflector;
268 reflector.setIdentity();
269 reflector.diagonal()(
d - 1) = -1;
284 for (
size_t j = 0;
j < nrUnknowns(); ++
j) {
298 for (
size_t j = 0;
j < nrUnknowns(); ++
j) {
299 const Rot3 Ri = Rot3::ClosestTo(
R.middleCols<3>(3 *
j));
311 return roundSolutionS(
S);
328 result.insert(it.first,
SO<d>(it.second.matrix()));
335 template <
typename T,
size_t d>
338 const auto &isotropic = std::dynamic_pointer_cast<noiseModel::Isotropic>(
342 sigma = isotropic->sigma();
344 const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
350 throw std::invalid_argument(
351 "Certification of optimality does not work with robust cost.");
357 throw std::invalid_argument(
358 "Shonan averaging noise models must be isotropic (but robust losses "
370 static constexpr
size_t stride = 2 *
d;
373 std::vector<Eigen::Triplet<double>> triplets;
374 triplets.reserve(stride * measurements_.size());
381 double kappa = Kappa<Rot, d>(
measurement, parameters_);
383 const size_t di =
d *
keys[0], dj =
d *
keys[1];
384 for (
size_t k = 0; k <
d; k++) {
386 triplets.emplace_back(di + k, di + k, kappa);
388 triplets.emplace_back(dj + k, dj + k, kappa);
393 const size_t dN =
d * nrUnknowns();
395 D.setFromTriplets(triplets.begin(), triplets.end());
405 static constexpr
size_t stride = 2 *
d *
d;
408 std::vector<Eigen::Triplet<double>> triplets;
409 triplets.reserve(stride * measurements_.size());
419 double kappa = Kappa<Rot, d>(
measurement, parameters_);
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++) {
425 triplets.emplace_back(di + r, dj +
c, kappa * Rij(r,
c));
427 triplets.emplace_back(dj + r, di +
c, kappa * Rij(
c, r));
433 const size_t dN =
d * nrUnknowns();
435 Q.setFromTriplets(triplets.begin(), triplets.end());
444 static constexpr
size_t stride =
d *
d;
447 const size_t N = nrUnknowns();
448 std::vector<Eigen::Triplet<double>> triplets;
449 triplets.reserve(stride *
N);
452 auto QSt = Q_ *
S.transpose();
454 for (
size_t j = 0;
j <
N;
j++) {
456 const size_t dj =
d *
j;
457 Matrix B = QSt.middleRows(dj,
d) *
S.middleCols<
d>(dj);
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)));
477 return computeLambda(
S);
485 auto Lambda = computeLambda(
S);
492 auto Lambda = computeLambda(
S);
505 int n = initialVector.rows();
506 Vector disturb = Vector::Random(
n);
509 double magnitude = initialVector.norm();
510 Vector perturbedVector = initialVector + 0.03 * magnitude * disturb;
511 perturbedVector.normalize();
512 return perturbedVector;
528 Vector *minEigenVector = 0,
size_t *numIterations = 0,
529 size_t maxIterations = 1000,
530 double minEigenvalueNonnegativityTolerance = 10
e-4) {
535 const bool pmConverged = pmOperator.
compute(
536 maxIterations, 1
e-5);
539 if (!pmConverged)
return false;
541 const double pmEigenValue = pmOperator.
eigenvalue();
543 if (pmEigenValue < 0) {
546 minEigenValue = pmEigenValue;
547 if (minEigenVector) {
549 minEigenVector->normalize();
554 const Sparse C = pmEigenValue * Matrix::Identity(
A.rows(),
A.cols()).sparseView() -
A;
558 const bool minConverged = apmShiftedOperator.
compute(
559 maxIterations, minEigenvalueNonnegativityTolerance / pmEigenValue);
561 if (!minConverged)
return false;
563 minEigenValue = pmEigenValue - apmShiftedOperator.
eigenvalue();
564 if (minEigenVector) {
565 *minEigenVector = apmShiftedOperator.
eigenvector();
566 minEigenVector->normalize();
568 if (numIterations) *numIterations = apmShiftedOperator.
nrIterations();
598 Y = A_ *
X + sigma_ *
X;
631 Vector *minEigenVector = 0,
size_t *numIterations = 0,
632 size_t maxIterations = 1000,
633 double minEigenvalueNonnegativityTolerance = 10
e-4,
639 lmEigenValueSolver(&lmOperator, 1,
std::min(numLanczosVectors,
A.rows()));
640 lmEigenValueSolver.init();
642 const int lmConverged = lmEigenValueSolver.compute(
646 if (lmConverged != 1)
return false;
648 const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
650 if (lmEigenValue < 0) {
653 *minEigenValue = lmEigenValue;
654 if (minEigenVector) {
655 *minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
656 minEigenVector->normalize();
673 minEigenValueSolver(&minShiftedOperator, 1,
691 perturbation.setRandom();
692 perturbation.normalize();
693 Vector xinit =
v0 + (.03 *
v0.norm()) * perturbation;
696 minEigenValueSolver.init(xinit.data());
701 const int minConverged = minEigenValueSolver.compute(
702 maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
705 if (minConverged != 1)
return false;
707 *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
708 if (minEigenVector) {
709 *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
710 minEigenVector->normalize();
712 if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
719 Vector *minEigenVector)
const {
722 auto A = computeA(
S);
724 double minEigenValue;
727 throw std::runtime_error(
728 "SparseMinimumEigenValue failed to compute minimum eigenvalue.");
730 return minEigenValue;
736 Vector *minEigenVector)
const {
739 auto A = computeA(
S);
741 double minEigenValue = 0;
744 throw std::runtime_error(
745 "PowerMinimumEigenValue failed to compute minimum eigenvalue.");
747 return minEigenValue;
755 double minEigenValue = computeMinEigenValue(
values, &minEigenVector);
756 return {minEigenValue, minEigenVector};
762 double minEigenValue = computeMinEigenValue(
values);
763 return minEigenValue > parameters_.optimalityThreshold;
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++) {
776 const auto v_i =
v.segment<
d>(
d *
i);
777 Vector xi = Vector::Zero(dimension);
779 for (
size_t j = 0;
j <
d;
j++) {
794 Matrix euclideanGradient = 2 * (L_ * (S_dot.transpose())).transpose();
799 Matrix symBlockDiagProduct(
p,
d * nrUnknowns());
800 for (
size_t i = 0;
i < nrUnknowns();
i++) {
802 const size_t di =
d *
i;
803 const Matrix P = S_dot.middleCols<
d>(di).transpose() *
804 euclideanGradient.middleCols<
d>(di);
808 symBlockDiagProduct.middleCols<
d>(di) = S_dot.middleCols<
d>(di) *
S;
810 Matrix riemannianGradient = euclideanGradient - symBlockDiagProduct;
811 return riemannianGradient;
817 const Vector &minEigenVector) {
827 double minEigenValue,
double gradienTolerance,
828 double preconditionedGradNormTolerance)
const {
829 double funcVal = costAt(
p - 1,
values);
830 double alphaMin = 1
e-2;
832 std::max(1024 * alphaMin, 10 * gradienTolerance /
fabs(minEigenValue));
833 vector<double> alphas;
834 vector<double> fvals;
836 while ((
alpha >= alphaMin)) {
838 double funcValTest = costAt(
p, Qplus);
839 Matrix gradTest = riemannianGradient(
p, Qplus);
840 double gradTestNorm = gradTest.norm();
842 alphas.push_back(
alpha);
843 fvals.push_back(funcValTest);
844 if ((funcVal > funcValTest) && (gradTestNorm > gradienTolerance)) {
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);
859 return LiftwithDescent(
p,
values,
alpha * minEigenVector);
866 for (
size_t j = 0;
j < nrUnknowns();
j++) {
881 std::mt19937 &
rng)
const {
882 const Values randomRotations = initializeRandomly(
rng);
883 return LiftTo<Rot3>(
p, randomRotations);
898 throw std::runtime_error(
"pMin is smaller than the base dimension d");
901 Values initialSOp = LiftTo<Rot>(pMin, initialEstimate);
902 for (
size_t p = pMin;
p <= pMax;
p++) {
904 Qstar = tryOptimizingAt(
p, initialSOp);
905 if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) {
908 throw std::runtime_error(
909 "When using robust norm, Shonan only tests a single rank. Set pMin = pMax");
911 const Values SO3Values = roundSolution(Qstar);
912 return {SO3Values, 0};
916 double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
917 if (minEigenValue > parameters_.optimalityThreshold) {
919 const Values SO3Values = roundSolution(Qstar);
920 return {SO3Values, minEigenValue};
927 initializeWithDescent(
p + 1, Qstar, minEigenVector, minEigenValue);
931 throw std::runtime_error(
"Shonan::run did not converge for given pMax");
953 std::dynamic_pointer_cast<noiseModel::Gaussian>(
f->noiseModel());
955 throw std::invalid_argument(
956 "parseMeasurements<Rot2> can only convert Pose2 measurements "
957 "with Gaussian noise models.");
958 const Matrix3
M = gaussian->covariance();
1001 std::dynamic_pointer_cast<noiseModel::Gaussian>(
f->noiseModel());
1003 throw std::invalid_argument(
1004 "parseMeasurements<Rot3> can only convert Pose3 measurements "
1005 "with Gaussian noise models.");
1006 const Matrix6
M = gaussian->covariance();