32 #include <Eigen/Eigenvalues> 52 double optimalityThreshold,
double alpha,
double beta,
double gamma)
54 optimalityThreshold(optimalityThreshold),
59 certifyOptimality(true) {
70 auto pcg = boost::make_shared<PCGSolverParameters>();
73 if (method ==
"SUBGRAPH") {
75 boost::make_shared<SubgraphSolverParameters>(builderParameters);
76 }
else if (method ==
"SGPC") {
77 pcg->preconditioner_ =
78 boost::make_shared<SubgraphPreconditionerParameters>(builderParameters);
80 }
else if (method ==
"JACOBI") {
81 pcg->preconditioner_ =
82 boost::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();
113 if (maxKey != N - 1) {
114 throw std::invalid_argument(
115 "As of now, ShonanAveraging expects keys numbered 0..N-1");
124 : parameters_(parameters),
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.");
170 return graph.
error(values);
175 boost::shared_ptr<LevenbergMarquardtOptimizer>
191 return boost::make_shared<LevenbergMarquardtOptimizer>(
graph,
initial,
200 return lm->optimize();
207 const size_t N = values.
size();
210 for (
const auto it : values.
filter<
SOn>()) {
211 S.middleCols<d>(it.key *
d) =
212 it.value.matrix().leftCols<d>();
221 for (
const auto it : values.
filter<
SOn>()) {
222 assert(it.value.rows() ==
p);
223 const auto &
M = it.value.matrix();
233 for (
const auto it : values.
filter<
SOn>()) {
234 assert(it.value.rows() ==
p);
235 const auto &
M = it.value.matrix();
245 const size_t N = S.cols() /
d;
252 DiagonalMatrix Sigma_d;
259 size_t numPositiveBlocks = 0;
260 for (
size_t i = 0;
i <
N; ++
i) {
263 if (determinant > 0) ++numPositiveBlocks;
266 if (numPositiveBlocks < N / 2) {
269 DiagonalMatrix reflector;
270 reflector.setIdentity();
271 reflector.diagonal()(
d - 1) = -1;
288 values.insert(
j, Ri);
302 values.insert(
j, Ri);
329 for (
const auto it : values.
filter<
Rot>()) {
332 return graph.
error(result);
337 template <
typename T,
size_t d>
344 sigma = isotropic->sigma();
352 throw std::invalid_argument(
353 "Certification of optimality does not work with robust cost.");
359 throw std::invalid_argument(
360 "Shonan averaging noise models must be isotropic (but robust losses " 364 return 1.0 / (sigma *
sigma);
372 static constexpr
size_t stride = 2 *
d;
375 std::vector<Eigen::Triplet<double>> triplets;
385 const size_t di =
d *
keys[0], dj =
d * keys[1];
386 for (
size_t k = 0; k <
d; k++) {
388 triplets.emplace_back(di + k, di + k, kappa);
390 triplets.emplace_back(dj + k, dj + k, kappa);
407 static constexpr
size_t stride = 2 *
d *
d;
410 std::vector<Eigen::Triplet<double>> triplets;
423 const size_t di = d *
keys[0], dj = d * keys[1];
424 for (
size_t r = 0; r <
d; r++) {
425 for (
size_t c = 0;
c <
d;
c++) {
427 triplets.emplace_back(di + r, dj +
c, kappa * Rij(r,
c));
429 triplets.emplace_back(dj + r, di +
c, kappa * Rij(
c, r));
446 static constexpr
size_t stride =
d *
d;
450 std::vector<Eigen::Triplet<double>> triplets;
451 triplets.reserve(stride * N);
456 for (
size_t j = 0;
j <
N;
j++) {
458 const size_t dj = d *
j;
459 Matrix B = QSt.middleRows(dj, d) * S.middleCols<d>(dj);
462 for (
size_t r = 0; r <
d; r++)
463 for (
size_t c = 0;
c <
d;
c++)
464 triplets.emplace_back(dj + r, dj +
c, 0.5 * (B(r,
c) + B(
c, r)));
468 Sparse Lambda(d * N, d * N);
507 int n = initialVector.rows();
508 Vector disturb = Vector::Random(n);
511 double magnitude = initialVector.norm();
512 Vector perturbedVector = initialVector + 0.03 * magnitude * disturb;
513 perturbedVector.normalize();
514 return perturbedVector;
530 Vector *minEigenVector = 0,
size_t *numIterations = 0,
532 double minEigenvalueNonnegativityTolerance = 10
e-4) {
537 const bool pmConverged = pmOperator.
compute(
541 if (!pmConverged)
return false;
543 const double pmEigenValue = pmOperator.
eigenvalue();
545 if (pmEigenValue < 0) {
548 minEigenValue = pmEigenValue;
549 if (minEigenVector) {
551 minEigenVector->normalize();
556 const Sparse C = pmEigenValue * Matrix::Identity(A.
rows(), A.
cols()).sparseView() - A;
560 const bool minConverged = apmShiftedOperator.compute(
561 maxIterations, minEigenvalueNonnegativityTolerance / pmEigenValue);
563 if (!minConverged)
return false;
565 minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue();
566 if (minEigenVector) {
567 *minEigenVector = apmShiftedOperator.eigenvector();
568 minEigenVector->normalize();
570 if (numIterations) *numIterations = apmShiftedOperator.nrIterations();
588 : A_(A), sigma_(
sigma) {}
600 Y = A_ * X + sigma_ *
X;
633 Vector *minEigenVector = 0,
size_t *numIterations = 0,
635 double minEigenvalueNonnegativityTolerance = 10
e-4,
641 lmEigenValueSolver(&lmOperator, 1,
std::min(numLanczosVectors, A.
rows()));
642 lmEigenValueSolver.init();
644 const int lmConverged = lmEigenValueSolver.compute(
648 if (lmConverged != 1)
return false;
650 const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
652 if (lmEigenValue < 0) {
655 *minEigenValue = lmEigenValue;
656 if (minEigenVector) {
657 *minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
658 minEigenVector->normalize();
675 minEigenValueSolver(&minShiftedOperator, 1,
692 Vector perturbation(v0.size());
693 perturbation.setRandom();
694 perturbation.normalize();
695 Vector xinit = v0 + (.03 * v0.norm()) * perturbation;
698 minEigenValueSolver.init(xinit.data());
703 const int minConverged = minEigenValueSolver.compute(
704 maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
705 Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
707 if (minConverged != 1)
return false;
709 *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
710 if (minEigenVector) {
711 *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
712 minEigenVector->normalize();
714 if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
721 Vector *minEigenVector)
const {
726 double minEigenValue;
729 throw std::runtime_error(
730 "SparseMinimumEigenValue failed to compute minimum eigenvalue.");
732 return minEigenValue;
738 Vector *minEigenVector)
const {
743 double minEigenValue = 0;
746 throw std::runtime_error(
747 "PowerMinimumEigenValue failed to compute minimum eigenvalue.");
749 return minEigenValue;
758 return std::make_pair(minEigenValue, minEigenVector);
775 double sign0 =
pow(-1.0,
round((p + 1) / 2) + 1);
776 for (
size_t i = 0;
i < v.size() /
d;
i++) {
778 const auto v_i = v.segment<
d>(
d *
i);
779 Vector xi = Vector::Zero(dimension);
781 for (
size_t j = 0;
j <
d;
j++) {
782 xi(
j + p - d - 1) = sign * v_i(d -
j - 1);
796 Matrix euclideanGradient = 2 * (
L_ * (S_dot.transpose())).transpose();
804 const size_t di =
d *
i;
805 const Matrix P = S_dot.middleCols<
d>(di).transpose() *
806 euclideanGradient.middleCols<
d>(di);
808 Matrix S = .5 * (P + P.transpose());
810 symBlockDiagProduct.middleCols<
d>(di) = S_dot.middleCols<
d>(di) *
S;
819 const Vector &minEigenVector) {
829 double minEigenValue,
double gradienTolerance,
830 double preconditionedGradNormTolerance)
const {
831 double funcVal =
costAt(p - 1, values);
832 double alphaMin = 1
e-2;
834 std::max(1024 * alphaMin, 10 * gradienTolerance /
fabs(minEigenValue));
835 vector<double> alphas;
836 vector<double> fvals;
838 while ((alpha >= alphaMin)) {
840 double funcValTest =
costAt(p, Qplus);
842 double gradTestNorm = gradTest.norm();
844 alphas.push_back(alpha);
845 fvals.push_back(funcValTest);
846 if ((funcVal > funcValTest) && (gradTestNorm > gradienTolerance)) {
852 auto fminIter = min_element(fvals.begin(), fvals.end());
853 auto minIdx = distance(fvals.begin(), fminIter);
854 double fMin = fvals[minIdx];
855 double aMin = alphas[minIdx];
856 if (fMin < funcVal) {
869 initial.
insert(
j, Rot::Random(rng));
883 std::mt19937 &
rng)
const {
885 return LiftTo<Rot3>(
p, randomRotations);
900 Values initialSOp = LiftTo<Rot>(pMin, initialEstimate);
901 for (
size_t p = pMin;
p <= pMax;
p++) {
907 throw std::runtime_error(
908 "When using robust norm, Shonan only tests a single rank. Set pMin = pMax");
911 return std::make_pair(SO3Values, 0);
919 return std::make_pair(SO3Values, minEigenValue);
930 throw std::runtime_error(
"Shonan::run did not converge for given pMax");
944 parameters.getUseHuber()),
958 parameters.getUseHuber()),
970 throw std::invalid_argument(
971 "parseMeasurements<Rot3> can only convert Pose3 measurements " 972 "with Gaussian noise models.");
973 const Matrix6
M = gaussian->covariance();
982 result.reserve(factors.size());
983 for (
auto f : factors) result.push_back(
convert(
f));
990 parameters.getUseHuber()),
enum gtsam::SubgraphBuilderParameters::Skeleton skeletonType
const SharedNoiseModel & noiseModel() const
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
double computeMinEigenValueAP(const Values &values, Vector *minEigenVector=nullptr) const
Matrix< RealScalar, Dynamic, Dynamic > M
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
static std::mt19937 kRandomNumberGenerator(42)
double beta
weight of Karcher-based prior (default 1)
double computeMinEigenValue(const Values &values, Vector *minEigenVector=nullptr) const
Main factor type in Shonan averaging, on SO(n) pairs.
void setLinearSolverType(const std::string &solver)
Values initializeRandomly() const
Random initialization for wrapper, fixed random seed.
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
double cost(const Values &values) const
Vector eigenvector() const
Return the eigenvector.
Sparse buildD() const
Build 3Nx3N sparse degree matrix D.
Factor Graph consisting of non-linear factors.
JacobiRotation< float > G
noiseModel::Diagonal::shared_ptr model
const SingularValuesType & singularValues() const
static VectorValues TangentVectorValues(size_t p, const Vector &v)
Create a VectorValues with eigenvector v_i.
void insert(Key j, const Value &val)
A matrix or vector expression mapping an existing array of data.
Compute maximum Eigenpair with accelerated power method.
void determinant(const MatrixType &m)
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)
bool getCertifyOptimality() const
MatrixProdFunctor(const Sparse &A, double sigma=0)
static const double sigma
Sparse computeA(const Values &values) const
Compute A matrix whose Eigenvalues we will examine.
Rot2 R(Rot2::fromAngle(0.1))
size_t nrUnknowns() const
Return number of unknowns.
double costAt(size_t p, const Values &values) const
Represents a diagonal matrix with its storage.
bool compute(size_t maxIterations, double tol)
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
LevenbergMarquardtParams lm
LM parameters.
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
const VALUE & measured() const
return the measurement
static Matrix StiefelElementMatrix(const Values &values)
Project to pxdN Stiefel manifold.
static Matrix RoundSolutionS(const Matrix &S)
Values retract(const VectorValues &delta) const
static size_t Dimension(size_t n)
Matrix riemannianGradient(size_t p, const Values &values) const
Calculate the riemannian gradient of F(values) at values.
static Values LiftwithDescent(size_t p, const Values &values, const Vector &minEigenVector)
typename Parameters::Rot Rot
NonlinearFactorGraph graph
Matrix< SCALARB, Dynamic, Dynamic > B
Eigen::SparseMatrix< double > Sparse
Key maxKey(const PROBLEM &problem)
Sparse computeLambda(const Matrix &S) const
Version that takes pxdN Stiefel manifold elements.
Values projectFrom(size_t p, const Values &values) const
const SharedNoiseModel & noiseModel() const
access to the noise model
Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const
Sparse Q() const
Sparse version of Q.
static Rot2 atan2(double y, double x)
std::pair< Values, double > run(const Values &initialEstimate, size_t pMin=d, size_t pMax=10) const
static Vector perturb(const Vector &initialVector)
ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters=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)
EIGEN_DEVICE_FUNC const RoundReturnType round() const
boost::shared_ptr< BetweenFactor > shared_ptr
ShonanAveraging(const Measurements &measurements, const Parameters ¶meters=Parameters())
double alpha
weight of anchor-based prior (default 0)
Parameters governing optimization etc.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Factor used in Shonan Averaging to clamp down gauge freedom.
double augmentationFactor
factor multiplied with n, yields number of extra edges.
EIGEN_DEVICE_FUNC const SignReturnType sign() const
Values roundSolutionS(const Matrix &S) const
Project pxdN Stiefel manifold matrix S to Rot3^N.
const ValueType at(Key j) const
const mpreal gamma(const mpreal &x, mp_rnd_t r=mpreal::get_default_rnd())
TransposeReturnType transpose()
static double Kappa(const BinaryMeasurement< T > &measurement, const ShonanAveragingParameters< d > ¶meters)
EIGEN_DEVICE_FUNC const DiagonalVectorType & diagonal() const
Compute maximum Eigenpair with power method.
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T >> &measurements, bool useRobustModel=false) const
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
boost::shared_ptr< LevenbergMarquardtOptimizer > createOptimizerAt(size_t p, const Values &initial) const
std::vector< BinaryMeasurement< Pose2 > > parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Anchor anchor
pose to use as anchor if not Karcher
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
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)
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.
const MatrixVType & matrixV() const
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
IterativeOptimizationParameters::shared_ptr iterativeParams
The container for iterativeOptimization parameters. used in CG Solvers.
double optimalityThreshold
threshold used in checkOptimality
Measurements measurements_
Various factors that minimize some Frobenius norm.
static ConjugateGradientParameters parameters
Matrix< Scalar, Dynamic, Dynamic > C
const mpreal dim(const mpreal &a, const mpreal &b, mp_rnd_t r=mpreal::get_default_rnd())
enum gtsam::SubgraphBuilderParameters::SkeletonWeight skeletonWeight
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Values roundSolution(const Values &values) const
void setFromTriplets(const InputIterators &begin, const InputIterators &end)
ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters=Parameters())
NonlinearFactorGraph buildGraphAt(size_t p) const
Values tryOptimizingAt(size_t p, const Values &initial) const
enum gtsam::SubgraphBuilderParameters::AugmentationWeight augmentationWeight
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Sparse D() const
Sparse version of D.
Two-sided Jacobi SVD decomposition of a rectangular matrix.
static ShonanAveraging3::Measurements extractRot3Measurements(const BetweenFactorPose3s &factors)
static Rot3 ClosestTo(const Matrix3 &M)
std::pair< double, Vector > computeMinEigenVector(const Values &values) const
void perform_op(const double *x, double *y) const
double error(const Values &values) const
Jet< T, N > pow(const Jet< T, N > &f, double g)
double eigenvalue() const
Return the eigenvalue.
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
iterator insert(const std::pair< Key, Vector > &key_value)
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
std::uint64_t Key
Integer nonlinear key type.
LinearSolverType linearSolverType
The type of linear solver to use in the nonlinear optimizer.
static BinaryMeasurement< Rot3 > convert(const BetweenFactor< Pose3 >::shared_ptr &f)
bool checkOptimality(const Values &values) const
Shonan Averaging algorithm.
Values initializeWithDescent(size_t p, const Values &values, const Vector &minEigenVector, double minEigenValue, double gradienTolerance=1e-2, double preconditionedGradNormTolerance=1e-4) const
std::vector< BinaryMeasurement< Rot >> Measurements
Filtered< Value > filter(const boost::function< bool(Key)> &filterFcn)