diff --git a/THANKS.md b/THANKS.md index 5a5be429cb..30b4dbf1d3 100644 --- a/THANKS.md +++ b/THANKS.md @@ -46,6 +46,12 @@ at LAAS-CNRS * Sammy Guo +at Northeastern + +* Zhexin Xu +* Hanna Jiamei Zhang +* David M. Rosen + Many thanks for your hard work!!!! Frank Dellaert diff --git a/examples/CertifiableRotationAveraging.cpp b/examples/CertifiableRotationAveraging.cpp new file mode 100644 index 0000000000..0a3977a8c1 --- /dev/null +++ b/examples/CertifiableRotationAveraging.cpp @@ -0,0 +1,212 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CertifiableRotationAveraging.cpp + * @brief Certifiable SO(d) synchronization (d = 2 or 3) via the Riemannian + * Staircase (Burer–Monteiro low-rank SDP + sparse-eigensolver + * certificate). Minimizes Σ_ij κ_ij ‖R_i R_ij − R_j‖²_F over Rot2 / Rot3. + * @author Zhexin Xu + * @author David M. Rosen + * + * Usage: + * ./CertifiableRotationAveraging --data= --dim=<2|3> + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +namespace { + +constexpr unsigned int kSeed = 60; +constexpr double kPi = 3.14159265358979323846; + +/// If less than half of the `D × D` entries have positive determinant, negate +/// every entry's last column. Required before per-block `Rot::ClosestTo` +/// projects each block to SO(d) — otherwise its sign-correction fires only on +/// the det < 0 blocks and the rounded rotations end up with inconsistent +/// signs across keys. +template +void AlignBlockDetSigns(Values& qcqpValues) { + size_t numNeg = 0, numBlocks = 0; + for (const auto& [key, M] : qcqpValues.extract()) { + if (M.rows() != D) continue; + ++numBlocks; + if (M.determinant() < 0) ++numNeg; + } + if (numBlocks == 0 || numNeg <= numBlocks / 2) return; + for (const auto& [key, M] : qcqpValues.extract()) { + if (M.rows() != D) continue; + Matrix flipped = M; + flipped.col(M.cols() - 1) *= -1.0; + qcqpValues.update(key, flipped); + } +} + +/// Random Rot2 / Rot3 init at column count `IntrinsicDim` (D ≥ ambient dim). +template +Values RandomInitial(const std::set& keys) { + Values v; + std::mt19937 rng(kSeed); + std::uniform_real_distribution uni(-kPi, kPi); + for (Key key : keys) { + if constexpr (std::is_same_v) { + InsertQcqpValue(key, Rot2::fromAngle(uni(rng)), &v); + } else { + const Vector3 rpy(uni(rng), uni(rng), uni(rng)); + InsertQcqpValue( + key, Rot3::RzRyRx(rpy(0), rpy(1), rpy(2)), &v); + } + } + return v; +} + +template +int RunCertifiableRA(const std::string& dataPath) { + constexpr bool kIs3D = std::is_same_v; + + // Drop translations, wrap each BetweenFactor's rotation in a + // FrobeniusBetweenFactor with isotropic precision kappa (SE-Sync): + // 2D: kappa = I(theta, theta) + // 3D: kappa = d / (2 * trace(RotInfo^-1)) + // g2o initial values are discarded; we use random init below. + auto [poseGraph, _] = readG2o(dataPath, /*is3D=*/kIs3D); + NonlinearFactorGraph graph; + std::set keys; + size_t skipped = 0; + for (const auto& f : *poseGraph) { + const auto bf = std::dynamic_pointer_cast>(f); + if (!bf) { + ++skipped; + continue; + } + + const auto gaussian = + std::dynamic_pointer_cast(bf->noiseModel()); + + double kappa; + if constexpr (kIs3D) { + const Matrix6 info = + gaussian ? gaussian->information() : Matrix6::Identity(); + kappa = 3.0 / (2.0 * info.topLeftCorner<3, 3>().inverse().trace()); + } else { + const Matrix3 info = + gaussian ? gaussian->information() : Matrix3::Identity(); + kappa = info(2, 2); + } + + auto rotNoise = + noiseModel::Isotropic::Variance(traits::dimension, 1.0 / kappa); + graph.emplace_shared>( + bf->key1(), bf->key2(), bf->measured().rotation(), rotNoise); + keys.insert(bf->key1()); + keys.insert(bf->key2()); + } + + if (graph.empty()) { + std::cerr << "No BetweenFactor<" << (kIs3D ? "Pose3" : "Pose2") + << "> found in " << dataPath + << ". Wrong --dim, or wrong file format?\n"; + return 1; + } + std::cout << "Loaded " << keys.size() << " rotations, " << graph.size() + << " edges from " << dataPath << " (d=" << IntrinsicDim << ")"; + if (skipped > 0) std::cout << " (" << skipped << " non-Between skipped)"; + std::cout << "\n\n"; + + // Low initial mu + slow growth lets LM make cost progress before the + // constraints tighten. Spectra verifier + ALM solver are the defaults. + RiemannianStaircaseParams params; + params.pMin = IntrinsicDim; + params.verbose = true; + params.almParams->initialMuEq = 1.0; + params.almParams->muEqIncreaseRate = 2.0; + params.almParams->maxIterations = 50; + params.almParams->absoluteViolationTolerance = 1e-8; + params.almParams->relativeViolationTolerance = 1e-8; + params.almParams->absoluteCostTolerance = 1e-10; + params.almParams->relativeCostTolerance = 1e-10; + params.almParams->verbose = false; + + // graph must be QCQP-representable — constructor probes once and throws. + RiemannianStaircaseOptimizer rso( + graph, RandomInitial(keys), params); + + const auto t0 = std::chrono::steady_clock::now(); + const auto result = rso.optimize(); + const double wallSeconds = std::chrono::duration( + std::chrono::steady_clock::now() - t0).count(); + + Values rounded; + if (result.rounded) { + // unstack → gauge-align → per-block project to RotT. + Values atRankD = result.layout.unstack(result.rounded->Yd); + AlignBlockDetSigns(atRankD); + for (auto& [key, R] : + ExtractQcqpValues(atRankD)) { + rounded.insert(key, R); + } + } + // graph.error is GTSAM's 0.5-NLL form; multiply by 2 for SE-Sync's paper F. + std::cout << "\n========== Result ==========\n" + << "Certified: " << (result.certified ? "yes" : "no") << "\n" + << "Final rank: " << result.finalRank << "\n"; + if (rounded.empty()) { + std::cout << "Rounded objective: n/a\n"; + } else { + std::cout << "Rounded objective: " << graph.error(rounded) << "\n"; + } + std::cout << "Wall time: " << wallSeconds << " s\n"; + + return result.certified ? 0 : 1; +} + +} // namespace + +int main(int argc, char** argv) { + std::string dataPath; + int dim = 0; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (arg.rfind("--data=", 0) == 0) { + dataPath = arg.substr(7); + } else if (arg.rfind("--dim=", 0) == 0) { + dim = std::stoi(arg.substr(6)); + } + } + if (dataPath.empty() || (dim != 2 && dim != 3)) { + std::cerr << "Usage: " << argv[0] << " --data= --dim=<2|3>\n"; + return 2; + } + + if (dim == 3) { + return RunCertifiableRA(dataPath); + } else { + return RunCertifiableRA(dataPath); + } +} diff --git a/gtsam/constrained/RiemannianStaircaseOptimizer.cpp b/gtsam/constrained/RiemannianStaircaseOptimizer.cpp new file mode 100644 index 0000000000..12aaf5c818 --- /dev/null +++ b/gtsam/constrained/RiemannianStaircaseOptimizer.cpp @@ -0,0 +1,566 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file RiemannianStaircaseOptimizer.cpp + * @brief Implementation of the Burer-Monteiro staircase. + */ + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +RiemannianStaircaseOptimizer::Layout +RiemannianStaircaseOptimizer::Layout::From(const Values& values) { + Layout layout; + // Sorted-Key order so the layout is deterministic across runs. + std::set sortedKeys; + for (const auto& [key, _] : values.extract()) sortedKeys.insert(key); + + size_t offset = 0; + for (Key key : sortedKeys) { + const Matrix X = values.at(key); + const size_t rowDim = static_cast(X.rows()); + layout.slices.emplace(key, LayoutSlice{offset, rowDim}); + offset += rowDim; + } + layout.totalDim = offset; + return layout; +} + +/* ************************************************************************* */ +size_t RiemannianStaircaseOptimizer::Layout::offsetOf(Key key) const { + auto it = slices.find(key); + if (it == slices.end()) { + throw std::out_of_range( + "RiemannianStaircaseOptimizer::Layout::offsetOf: key not in layout."); + } + return it->second.offset; +} + +/* ************************************************************************* */ +size_t RiemannianStaircaseOptimizer::Layout::rowDimOf(Key key) const { + auto it = slices.find(key); + if (it == slices.end()) { + throw std::out_of_range( + "RiemannianStaircaseOptimizer::Layout::rowDimOf: key not in layout."); + } + return it->second.rowDim; +} + +/* ************************************************************************* */ +const RiemannianStaircaseOptimizer::LayoutSlice& +RiemannianStaircaseOptimizer::Layout::sliceOf(Key key) const { + auto it = slices.find(key); + if (it == slices.end()) { + throw std::out_of_range( + "RiemannianStaircaseOptimizer::Layout::sliceOf: key not in layout."); + } + return it->second; +} + +/* ************************************************************************* */ +bool RiemannianStaircaseOptimizer::Layout::conformsTo( + const Values& values) const { + size_t matrixCount = 0; + for (const auto& [key, X] : values.extract()) { + ++matrixCount; + auto it = slices.find(key); + if (it == slices.end()) return false; + if (static_cast(X.rows()) != it->second.rowDim) return false; + } + return matrixCount == slices.size(); +} + +/* ************************************************************************* */ +Matrix RiemannianStaircaseOptimizer::Layout::stack(const Values& values) const { + if (slices.empty()) return Matrix(); + // Probe column count from the first present value. + size_t cols = 0; + bool first = true; + for (const auto& [key, X] : values.extract()) { + if (first) { + cols = static_cast(X.cols()); + first = false; + } else if (static_cast(X.cols()) != cols) { + throw std::invalid_argument( + "Layout::stack: all matrix-valued variables must share the same " + "column count."); + } + } + Matrix Y = Matrix::Zero(static_cast(totalDim), static_cast(cols)); + for (const auto& [key, slice] : slices) { + if (!values.exists(key)) { + throw std::invalid_argument("Layout::stack: missing key in values."); + } + const Matrix X = values.at(key); + if (static_cast(X.rows()) != slice.rowDim) { + throw std::invalid_argument( + "Layout::stack: row count mismatch for key."); + } + Y.block(static_cast(slice.offset), 0, + static_cast(slice.rowDim), static_cast(cols)) = X; + } + return Y; +} + +/* ************************************************************************* */ +Values RiemannianStaircaseOptimizer::Layout::unstack(const Matrix& Y) const { + if (static_cast(Y.rows()) != totalDim) { + throw std::invalid_argument( + "Layout::unstack: input row count does not match totalDim."); + } + Values values; + for (const auto& [key, slice] : slices) { + values.insert( + key, + Matrix(Y.block(static_cast(slice.offset), 0, + static_cast(slice.rowDim), Y.cols()))); + } + return values; +} + +/* ************************************************************************* */ +size_t RiemannianStaircaseOptimizer::Layout::maxRowDim() const { + size_t maxDim = 0; + for (const auto& [_, slice] : slices) { + if (slice.rowDim > maxDim) maxDim = slice.rowDim; + } + return maxDim; +} + +/* ************************************************************************* */ +void RiemannianStaircaseOptimizer::validateParams( + const RiemannianStaircaseParams& params) { + if (params.pMin > params.pMax) { + throw std::invalid_argument( + "RiemannianStaircaseOptimizer: pMin must be <= pMax."); + } + if (params.pMin == 0) { + throw std::invalid_argument( + "RiemannianStaircaseOptimizer: pMin must be >= 1."); + } +} + +/* ************************************************************************* */ +Values RiemannianStaircaseOptimizer::padInitialValues(const Values& Y, + size_t pMin) { + Values padded = Y; + for (const auto& [key, X] : Y.extract()) { + const size_t cols = static_cast(X.cols()); + if (cols == pMin) continue; + if (cols > pMin) { + throw std::invalid_argument( + "RiemannianStaircaseOptimizer::padInitialValues: initial value has " + "more columns than pMin; either lower pMin or drop the extra " + "columns."); + } + Matrix pad = Matrix::Zero(X.rows(), static_cast(pMin)); + pad.leftCols(static_cast(cols)) = X; + padded.update(key, pad); + } + return padded; +} + +/* ************************************************************************* */ +RiemannianStaircaseOptimizer::InnerSolveResult +RiemannianStaircaseOptimizer::runLocalSolver( + const QcqpProblem& qcqp, const Values& Y0, + AugmentedLagrangianParams::shared_ptr almParams) { + // Local copy so we can force storeOptProgress without mutating the caller's. + auto localParams = almParams + ? std::make_shared(*almParams) + : std::make_shared(); + localParams->storeOptProgress = true; + + AugmentedLagrangianOptimizer alm(qcqp, Y0, localParams); + Values Y = alm.optimize(); + if (alm.progress().empty()) { + throw std::runtime_error( + "RiemannianStaircaseOptimizer::runLocalSolver: ALM produced no " + "progress."); + } + return {Y, alm.progress().back().lambdaEq}; +} + +/* ************************************************************************* */ +Eigen::SparseMatrix RiemannianStaircaseOptimizer::buildDataMatrix( + const QcqpProblem& qcqp, const Layout& layout) { + const size_t n = layout.totalDim; + std::vector> triplets; + + // QpCost stores each Q_ij as a K-Kronecker expansion; the natural + // rowDim_i x rowDim_j block is the top-left corner of each expanded block. + for (const auto& factor : qcqp.costs()) { + auto qpCost = std::dynamic_pointer_cast(factor); + if (!qpCost) { + throw std::runtime_error( + "buildDataMatrix: non-QpCost factor in qcqp.costs()."); + } + const HessianFactor& hessian = qpCost->hessianFactor(); + const KeyVector& keys = qpCost->keys(); + for (size_t i = 0; i < keys.size(); ++i) { + const size_t off_i = layout.offsetOf(keys[i]); + const size_t rd_i = layout.rowDimOf(keys[i]); + for (size_t j = i; j < keys.size(); ++j) { + const size_t off_j = layout.offsetOf(keys[j]); + const size_t rd_j = layout.rowDimOf(keys[j]); + const Matrix expandedBlock = hessian.info().block(i, j); + const Matrix Q_ij = expandedBlock.topLeftCorner( + static_cast(rd_i), static_cast(rd_j)); + for (size_t r = 0; r < rd_i; ++r) { + for (size_t c = 0; c < rd_j; ++c) { + const double v = Q_ij(static_cast(r), static_cast(c)); + if (v == 0.0) continue; + triplets.emplace_back(off_i + r, off_j + c, v); + if (i != j) { + triplets.emplace_back(off_j + c, off_i + r, v); + } + } + } + } + } + } + + Eigen::SparseMatrix Q(static_cast(n), static_cast(n)); + Q.setFromTriplets(triplets.begin(), triplets.end()); + Q.makeCompressed(); + return Q; +} + +/* ************************************************************************* */ +Eigen::SparseMatrix RiemannianStaircaseOptimizer::buildMultiplierMatrix( + const QcqpProblem& qcqp, const Layout& layout, + const std::vector& lambdaEq) { + const size_t n = layout.totalDim; + std::vector> triplets; + + // A*(lambda) = sum_m (lambda_m / sigma_m) * A_m. ALM tracks lambdas paired + // with the whitened residual h_m / sigma_m, so divide by sigma_m to get the + // multiplier on A_m. + // + // lambdaEq is indexed in eConstraints() order (ALM convention). + size_t m = 0; + for (const auto& factor : qcqp.eConstraints()) { + auto qcFactor = + std::dynamic_pointer_cast( + factor); + if (!qcFactor) { + ++m; + continue; + } + const QuadraticConstraint& qc = qcFactor->quadraticConstraint(); + if (m >= lambdaEq.size()) { + throw std::runtime_error( + "buildMultiplierMatrix: lambdaEq shorter than equality constraint " + "count."); + } + if (lambdaEq[m].size() != 1) { + throw std::runtime_error( + "buildMultiplierMatrix: QuadraticEqualityConstraintFactor expects a " + "scalar multiplier; got lambdaEq[m].size() != 1."); + } + const double lambdaM = lambdaEq[m](0) / qc.sigma(); + const Key key = qc.key(); + const size_t off = layout.offsetOf(key); + const size_t rd = layout.rowDimOf(key); + const Matrix& A = qc.A(); + if (static_cast(A.rows()) != rd || + static_cast(A.cols()) != rd) { + throw std::runtime_error( + "buildMultiplierMatrix: constraint A size does not match key " + "rowDim."); + } + for (size_t r = 0; r < rd; ++r) { + for (size_t c = 0; c < rd; ++c) { + const double v = lambdaM * A(static_cast(r), static_cast(c)); + if (v == 0.0) continue; + triplets.emplace_back(off + r, off + c, v); + } + } + ++m; + } + + Eigen::SparseMatrix Aadj(static_cast(n), static_cast(n)); + Aadj.setFromTriplets(triplets.begin(), triplets.end()); + Aadj.makeCompressed(); + return Aadj; +} + +/* ************************************************************************* */ +Eigen::SparseMatrix RiemannianStaircaseOptimizer::buildCertificate( + const QcqpProblem& qcqp, const Layout& layout, + const std::vector& lambdaEq) { + Eigen::SparseMatrix S = buildDataMatrix(qcqp, layout); + S += buildMultiplierMatrix(qcqp, layout, lambdaEq); + S.makeCompressed(); + return S; +} + +/* ************************************************************************* */ +Values RiemannianStaircaseOptimizer::escapeSaddleAndLift( + const Values& Ystar, const Vector& vMin, const Layout& layout, + double alpha) { + if (!layout.conformsTo(Ystar)) { + throw std::invalid_argument( + "RiemannianStaircaseOptimizer::escapeSaddleAndLift: layout does not " + "match values."); + } + if (static_cast(vMin.size()) != layout.totalDim) { + throw std::invalid_argument( + "RiemannianStaircaseOptimizer::escapeSaddleAndLift: vMin size does not " + "match layout.totalDim."); + } + Values lifted; + for (const auto& [key, X] : Ystar.extract()) { + const LayoutSlice& slice = layout.sliceOf(key); + Matrix Xlifted(X.rows(), X.cols() + 1); + Xlifted.leftCols(X.cols()) = X; + Xlifted.col(X.cols()) = + alpha * vMin.segment(static_cast(slice.offset), + static_cast(slice.rowDim)); + lifted.insert(key, Xlifted); + } + return lifted; +} + +/* ************************************************************************* */ +RiemannianStaircaseOptimizer::RoundedSolution +RiemannianStaircaseOptimizer::truncateToRankD(const Values& Y, + const Layout& layout, int d) { + if (d < 2) { + throw std::invalid_argument( + "RiemannianStaircaseOptimizer::truncateToRankD: d must be >= 2."); + } + const Matrix Ystacked = layout.stack(Y); + if (d > Ystacked.cols()) { + throw std::invalid_argument( + "RiemannianStaircaseOptimizer::truncateToRankD: d exceeds the column " + "count of the stacked matrix."); + } + Eigen::JacobiSVD svd(Ystacked, + Eigen::ComputeFullU | Eigen::ComputeFullV); + Matrix Yd = svd.matrixU().leftCols(d) * + svd.singularValues().head(d).asDiagonal(); + return {Yd}; +} + +/* ************************************************************************* */ +std::tuple RiemannianStaircaseOptimizer::verify( + const Eigen::SparseMatrix& S, + const RiemannianStaircaseParams& params) { + switch (params.verificationMethod) { + case RiemannianStaircaseParams::VerificationMethod::DenseEigen: + return verifyDenseEigen(S, params); + case RiemannianStaircaseParams::VerificationMethod::Spectra: + default: + return verifySpectra(S, params); + } +} + +/* ************************************************************************* */ +std::tuple RiemannianStaircaseOptimizer::verifyDenseEigen( + const Eigen::SparseMatrix& S, + const RiemannianStaircaseParams& params) { + const Matrix Sdense(S); + Eigen::SelfAdjointEigenSolver es(Sdense); + if (es.info() != Eigen::Success) { + throw std::runtime_error( + "RiemannianStaircaseOptimizer::verifyDenseEigen: " + "SelfAdjointEigenSolver failed."); + } + const double lambdaMin = es.eigenvalues()(0); + Vector vMin = es.eigenvectors().col(0); + const bool passed = (lambdaMin >= -params.eta); + if (params.verbose) { + std::cout << " [dense Eigen] verified=" << (passed ? 1 : 0) + << " lambda_min=" << lambdaMin << " dim=" << S.rows() + << std::endl; + } + return {passed, lambdaMin, vMin}; +} + +/* ************************************************************************* */ +std::tuple RiemannianStaircaseOptimizer::verifySpectra( + const Eigen::SparseMatrix& S, + const RiemannianStaircaseParams& params) { + const int n = static_cast(S.rows()); + + // Stage 1: Cholesky on M = S + eta*I. Success => lambda_min(S) >= -eta; + // return -eta as a conservative bound (v_min unused on the pass path). + { + Eigen::SparseMatrix M = S; + for (int k = 0; k < n; ++k) M.coeffRef(k, k) += params.eta; + M.makeCompressed(); + Eigen::SimplicialLLT> chol; + chol.compute(M); + if (chol.info() == Eigen::Success) { + if (params.verbose) { + std::cout << " [Chol] verified=1 dim=" << n << std::endl; + } + return {true, -params.eta, Vector::Zero(n)}; + } + } + + // Stage 2: two-pass Lanczos to recover precise (lambda_min, v_min). + const int nev = 1; + // Spectra needs 2*nev+1 <= ncv <= n; tiny n falls back to dense. + if (n < 2 * nev + 1) { + return verifyDenseEigen(S, params); + } + const int ncv = std::clamp(static_cast(params.numLanczosVectors), + 2 * nev + 1, n); + const int maxIters = static_cast(params.maxSpectraIters); + const double tol = params.spectraTol; + + // Pass 1: lambda_max of S. + Spectra::SparseSymMatProd op(S); + Spectra::SymEigsSolver> eigs(op, nev, ncv); + eigs.init(); + eigs.compute(Spectra::SortRule::LargestAlge, maxIters, tol); + if (eigs.info() != Spectra::CompInfo::Successful) { + throw std::runtime_error( + "RiemannianStaircaseOptimizer::verifySpectra: Spectra Lanczos failed " + "on the first pass (lambda_max)."); + } + const double lambdaMax = eigs.eigenvalues()(0); + + // Pass 2 (shift trick): largest eigenvalue of (lambdaMax * I - S) equals + // lambdaMax - lambda_min, and Lanczos converges fastest at the extremal end. + Eigen::SparseMatrix shifted = -S; + for (int k = 0; k < n; ++k) shifted.coeffRef(k, k) += lambdaMax; + shifted.makeCompressed(); + Spectra::SparseSymMatProd opShift(shifted); + Spectra::SymEigsSolver> eigsShift( + opShift, nev, ncv); + eigsShift.init(); + eigsShift.compute(Spectra::SortRule::LargestAlge, maxIters, tol); + if (eigsShift.info() != Spectra::CompInfo::Successful) { + throw std::runtime_error( + "RiemannianStaircaseOptimizer::verifySpectra: Spectra Lanczos failed " + "on the second pass (shifted)."); + } + const double shiftedMaxEig = eigsShift.eigenvalues()(0); + const double lambdaMin = lambdaMax - shiftedMaxEig; + Vector vMin = eigsShift.eigenvectors().col(0); + vMin.normalize(); + + const bool passed = (lambdaMin >= -params.eta); + if (params.verbose) { + std::cout << " [Spectra] verified=" << (passed ? 1 : 0) + << " lambda_min=" << lambdaMin << " lambda_max=" << lambdaMax + << " dim=" << n << std::endl; + } + return {passed, lambdaMin, vMin}; +} + +/* ************************************************************************* */ +RiemannianStaircaseResult RiemannianStaircaseOptimizer::optimize() const { + using Clock = std::chrono::steady_clock; + const auto seconds = [](Clock::time_point a, Clock::time_point b) { + return std::chrono::duration(b - a).count(); + }; + const auto totalStart = Clock::now(); + + RiemannianStaircaseResult result; + Values Y = padInitialValues(initialValues_, params_.pMin); + + // Algorithm 1, lines 2-13. See header for the full chain; per iteration: + // local solve -> build S -> verify -> return on pass, else lift to p+1. + for (size_t p = params_.pMin; p <= params_.pMax; ++p) { + result.ranksVisited.push_back(p); + if (params_.verbose) { + std::cout << "Staircase at rank = " << p << std::endl; + } + + // Inner solve (Alg. 1 line 3). Swap point for non-ALM solvers — downstream + // needs (Y*, lambdaEq) aligned with qcqp.eConstraints(). + const auto nlpStart = Clock::now(); + QcqpProblem qcqp(graph_, p); + const InnerSolveResult inner = + runLocalSolver(qcqp, Y, params_.almParams); + Y = inner.Y; + + result.costPerLevel.push_back(qcqp.costs().error(Y)); + result.nlpTimePerLevel.push_back(seconds(nlpStart, Clock::now())); + + // Certificate + verify (Alg. 1 lines 4-9). Pass => global SDP optimum. + const auto verifyStart = Clock::now(); + const Layout layout = Layout::From(Y); + // Solvers without native multipliers need a closed-form extractor here; + // not implemented yet, so insist on non-empty. + if (inner.lambdaEq.empty()) { + throw std::runtime_error( + "RiemannianStaircaseOptimizer::optimize: local solver returned no " + "multipliers and the closed-form extractor is not implemented."); + } + const Eigen::SparseMatrix S = + buildCertificate(qcqp, layout, inner.lambdaEq); + auto [passed, lambdaMin, vMin] = verify(S, params_); + result.verifyTimePerLevel.push_back(seconds(verifyStart, Clock::now())); + result.minEigenvaluePerLevel.push_back(lambdaMin); + + if (params_.verbose) { + // lambda_min only printed on failure (Cholesky gives just the bound). + std::cout << "Staircase verified=" << (passed ? 1 : 0) + << " cost=" << result.costPerLevel.back(); + if (!passed) std::cout << " lambda_min=" << lambdaMin; + std::cout << std::endl; + } + + if (passed) { + result.values = Y; + result.layout = layout; + result.finalRank = p; + result.certified = true; + result.minEigenvalue = lambdaMin; + result.rounded = + truncateToRankD(Y, layout, static_cast(layout.maxRowDim())); + result.totalTime = seconds(totalStart, Clock::now()); + return result; + } + + // Alg. 1 lines 10-12: S not PSD, lift to rank p+1 along v_min. The + // O(alpha^2) violation is absorbed by the next inner solve. + if (p < params_.pMax) { + Y = escapeSaddleAndLift(Y, vMin, layout, params_.alpha); + } else { + result.minEigenvalue = lambdaMin; + } + } + + // pMax exhausted without certification: return best-effort. + result.values = Y; + result.layout = Layout::From(Y); + result.finalRank = params_.pMax; + result.certified = false; + result.totalTime = seconds(totalStart, Clock::now()); + return result; +} + +} // namespace gtsam diff --git a/gtsam/constrained/RiemannianStaircaseOptimizer.h b/gtsam/constrained/RiemannianStaircaseOptimizer.h new file mode 100644 index 0000000000..ec73fb56c3 --- /dev/null +++ b/gtsam/constrained/RiemannianStaircaseOptimizer.h @@ -0,0 +1,309 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file RiemannianStaircaseOptimizer.h + * @brief Burer–Monteiro Solver for the SDP Relaxation of a QCQP-Representable Factor Graph + * + * @author Zhexin Xu + * @author David M. Rosen + * + * References: + * Zhexin Xu, Nikolas R. Sanderson, Hanna Jiamei Zhang, David M. Rosen. + * "Certifiable Estimation with Factor Graphs." (Certi-fgo) arXiv:2603.01267, 2026. + * David M. Rosen. "Scalable low-rank semidefinite programming for + * certifiably correct machine perception." Proc. Workshop on the + * Algorithmic Foundations of Robotics (WAFR), 2020. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +struct RiemannianStaircaseResult; + +/// Parameters for the Riemannian Staircase solver. +struct GTSAM_EXPORT RiemannianStaircaseParams { + /// Spectra: sparse Lanczos via gtsam/3rdparty/Spectra. + /// DenseEigen: full O(n^3) self-adjoint eigensolver, not recommended for large problems. + enum class VerificationMethod { Spectra, DenseEigen }; + + size_t pMin = 2; /// Initial level (column dimension here) of Riemannian staircase, should be >= the ambient dim. + size_t pMax = 10; ///< staircase cap before giving up. + double alpha = 1e-2; ///< descent step size on saddle escape. + + VerificationMethod verificationMethod = VerificationMethod::Spectra; + double eta = 1e-3; ///< min-eigenvalue tolerance for certification. + + /// Lanczos iteration cap. 1000 is plenty for one extremal eigenpair on + /// typical rotation-averaging problems; raise only if Pass 2 fails to + /// converge on a very ill-conditioned spectrum. + size_t maxSpectraIters = 1000; + /// Lanczos subspace size. ~100 gives the shift pass enough room to + /// discriminate a tiny negative eigenvalue from a near-zero one. + size_t numLanczosVectors = 100; + double spectraTol = 1e-4; + + /// Parameters for inner local solver ALM. + AugmentedLagrangianParams::shared_ptr almParams = + std::make_shared(); + bool verbose = false; +}; + +/** + * Burer-Monteiro Riemannian Staircase over a QCQP-representable estimation + * problem (Certi-FGO Algorithm 1). + * + * Problem chain (Certi-FGO Sec V; eq. numbers below refer to that paper): + * QCQP (eq. 12): min_{X in R^{r x d}} + * s.t. = b_m, m in [M]. + * SDP (eq. 13): min_{Z in S^r_+} + * s.t. A(Z)_m = = b_m. + * BM (eq. 16): min_{Y in R^{r x p}} + * s.t. = b_m + * (Z = Y Y' parameterizes the SDP at rank <= p). + * + * KKT certificate matrix (eq. 20): + * S := Q + A*(lambda) = Q + sum_m lambda_m * A_m, + * where A* is the adjoint of the constraint map A(Z)_m := . At a + * 1st-order KKT point Y of the BM problem, Y is a global SDP optimum iff + * S >= 0 (Thm 1(a)). Otherwise Thm 1(b) gives v with v' S v < 0, and + * Y+ = [Y | 0] lifted with descent direction [0 | v] is a feasible + * 2nd-order descent for the rank-(p+1) BM problem (saddle escape). + * + * Algorithm 1 per outer level p in [pMin, pMax]: + * (1) build the rank-p QCQP via `QcqpProblem(graph, p)`; + * (2) `runLocalSolver` (ALM today) yields (Y*, lambda*); + * (3) build S via `buildCertificate` (= `buildDataMatrix` + + * `buildMultiplierMatrix`); + * (4) `verify` checks S >= 0 (Cholesky-first; Spectra fallback for the + * saddle-escape eigenvector); + * (5) If Verified: `truncateToRankD` projects Y* to rank d = + * layout.maxRowDim() and the result is returned. Otherwise + * `escapeSaddleAndLift` lifts to rank p+1 and the loop continues. + * + */ +class GTSAM_EXPORT RiemannianStaircaseOptimizer { + public: + /// Placement of one QCQP variable inside the stacked BM matrix Y: + /// `Y_i = Y.block(offset, 0, rowDim, p)`. + struct LayoutSlice { + size_t offset; + size_t rowDim; + }; + + /** + * The cost Q, constraints A_m, certificate S, and SVD rounding step all + * work on Y as one (totalDim x p) dense matrix rather than on per-key + * `Values`. `Layout` is how we get between the two forms: for each Key it + * stores the row offset and row count of that variable's slice inside Y. + * Offsets are assigned in sorted-Key order, so they stay the same across + * runs and across staircase levels. + * + * Block structure of Y (keys x0, x1, x2 with rowDim 2, 3, 2, p columns): + * + * <----------- p -----------> + * +---------------------------+ --- --- + * | | ^ | + * x0 slice | Y_x0 | rowDim=2 | + * | | v | + * +---------------------------+ --- | + * | | ^ | + * x1 slice | Y_x1 | rowDim=3 totalDim + * | | v | + * +---------------------------+ --- | + * | | ^ | + * x2 slice | Y_x2 | rowDim=2 | + * | | v v + * +---------------------------+ --- --- + * + * Offsets: x0 -> 0, x1 -> 2, x2 -> 5. totalDim = 2 + 3 + 2 = 7. + * Per-key slice: `Y_i = Y.block(offset_i, 0, rowDim_i, p)`. + */ + struct GTSAM_EXPORT Layout { + std::unordered_map slices; + size_t totalDim = 0; + + static Layout From(const Values& values); + + bool contains(Key key) const { return slices.find(key) != slices.end(); } + size_t size() const { return slices.size(); } + size_t offsetOf(Key key) const; + size_t rowDimOf(Key key) const; + const LayoutSlice& sliceOf(Key key) const; + + /// True iff `values` matches this layout exactly (keys + per-key rowDim). + bool conformsTo(const Values& values) const; + + /// Stack the Matrix-valued variables in `values` into one (totalDim x p) + /// matrix. All values must share the same column count p. + Matrix stack(const Values& values) const; + + /// Inverse of stack: split a (totalDim x p) matrix into per-key slices. + Values unstack(const Matrix& Y) const; + + /// Largest `rowDim` across slices. Used as the default rank-d for + /// `truncateToRankD` since rotation blocks dominate; translations + /// (rowDim=1) shouldn't drive the truncation. + size_t maxRowDim() const; + }; + + /// Output of `truncateToRankD`. The projection rank is `Yd.cols()`. + struct RoundedSolution { + Matrix Yd; ///< Rank-d projection of stacked Y, shape (totalDim × d). + }; + + /// Output of one local-solver call. + struct InnerSolveResult { + Values Y; + std::vector lambdaEq; + }; + + /// Construct from a QCQP-representable factor graph and an initial + /// feasible value. Every factor in `graph` must override + /// `NonlinearFactor::qcqpFactors` (the base implementation throws). + /// The staircase rebuilds the rank-p QCQP internally at each level by + /// calling `QcqpProblem(graph, p)`. + RiemannianStaircaseOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, + const RiemannianStaircaseParams& params = {}) + : graph_(graph), + initialValues_(initialValues), + params_(params) { + validateParams(params_); + // Fail fast if the graph isn't QCQP-representable. + try { + (void)QcqpProblem(graph_, params_.pMin); + } catch (const std::exception& e) { + throw std::invalid_argument( + std::string("RiemannianStaircaseOptimizer: QcqpProblem(graph, pMin) " + "failed — is the source graph QCQP-representable? " + "Underlying error: ") + + e.what()); + } + } + + /// Run the staircase. If it certifies, `result.rounded` holds the rank-d projection. + RiemannianStaircaseResult optimize() const; + + /// Zero-pad each Matrix entry of `Y` to `pMin` columns. Use when entries are narrower than `pMin`. + static Values padInitialValues(const Values& Y, size_t pMin); + + /// Run the local solver (ALM here). Returns Y* and multipliers (if the solver provides). + static InnerSolveResult runLocalSolver( + const QcqpProblem& qcqp, const Values& Y0, + AugmentedLagrangianParams::shared_ptr almParams); + + /// Build Q = sum_k H_k, the data Hessian assembled from each QpCost + /// factor's `hessianFactor()`. Each H_k is laid out in K-Kronecker form; + /// the natural rowDim_i x rowDim_j block is its top-left corner. + static Eigen::SparseMatrix buildDataMatrix( + const QcqpProblem& qcqp, const Layout& layout); + + /// Build A*(lambda) = sum_m (lambda_m / sigma_m) * A_m, the adjoint constraint matrix. + /// + /// `lambda_m` are inner-solver multipliers for the whitened equality + /// factors `h_m(X) = (trace(X' A_m X) - b_m) / sigma_m`; we divide by + /// sigma_m to recover the unwhitened multiplier that pairs with A_m. + /// `sigma_m` is GTSAM noise-model bookkeeping, not part of the SDP math. + static Eigen::SparseMatrix buildMultiplierMatrix( + const QcqpProblem& qcqp, const Layout& layout, + const std::vector& lambdaEq); + + /// Build the SDP dual matrix S = Q + A*(lambda), i.e. `buildDataMatrix` + /// plus `buildMultiplierMatrix`. + /// + /// Assumes Y is a 1st-order KKT point of the BM problem (what the local + /// solver should return). Under that assumption, S >= 0 iff Z = Y Y' is + /// a global SDP optimum (Thm 1(a)). + static Eigen::SparseMatrix buildCertificate( + const QcqpProblem& qcqp, const Layout& layout, + const std::vector& lambdaEq); + + /// Lift Y* from rank p to rank p+1 by appending `alpha * vMin` as a new + /// column (Thm 1(b), Algorithm 1 lines 10-12). With vMin the negative- + /// eigenvalue eigenvector of S, this is a feasible 2nd-order descent direction + static Values escapeSaddleAndLift(const Values& Ystar, const Vector& vMin, + const Layout& layout, double alpha); + + /// SVD-based rank-d projection: round the stacked BM matrix from rank p down to rank d. + static RoundedSolution truncateToRankD(const Values& Y, const Layout& layout, + int d); + + /// Test PSD of the certificate matrix S. Returns `(passed, lambda_min, + /// v_min)` with `passed == (lambda_min >= -params.eta)`. + /// + /// The Spectra method mirrors SE-Sync's two-stage `fast_verification`: + /// Stage 1 (fast check): sparse Cholesky on M = S + eta * I. Success + /// is binary and ~O(nnz). Returns lambda_min = -eta as a conservative + /// lower bound; v_min is unused on the cert-pass path. + /// Stage 2 (only on Cholesky failure): two-pass Lanczos shift trick + /// to get the precise lambda_min and v_min for the lift direction. + /// SE-Sync uses CHOLMOD + LOBPCG; we use Eigen's SimplicialLLT + Spectra. + static std::tuple verify( + const Eigen::SparseMatrix& S, + const RiemannianStaircaseParams& params); + + // =========================================================================== + + const NonlinearFactorGraph& graph() const { return graph_; } + const Values& initialValues() const { return initialValues_; } + const RiemannianStaircaseParams& params() const { return params_; } + + private: + static std::tuple verifyDenseEigen( + const Eigen::SparseMatrix& S, + const RiemannianStaircaseParams& params); + static std::tuple verifySpectra( + const Eigen::SparseMatrix& S, + const RiemannianStaircaseParams& params); + + static void validateParams(const RiemannianStaircaseParams& params); + + NonlinearFactorGraph graph_; + Values initialValues_; + RiemannianStaircaseParams params_; +}; + +struct GTSAM_EXPORT RiemannianStaircaseResult { + Values values; + RiemannianStaircaseOptimizer::Layout layout; + /// SVD-rounded BM solution + std::optional rounded; + + size_t finalRank = 0; + bool certified = false; + double minEigenvalue = 0.0; + /// Per-level diagnostics recorded during the staircase climb. + std::vector ranksVisited; + std::vector costPerLevel; + std::vector minEigenvaluePerLevel; + std::vector nlpTimePerLevel; + std::vector verifyTimePerLevel; + double totalTime = 0.0; +}; + +} // namespace gtsam diff --git a/gtsam/constrained/tests/testRiemannianStaircase.cpp b/gtsam/constrained/tests/testRiemannianStaircase.cpp new file mode 100644 index 0000000000..ae433511fa --- /dev/null +++ b/gtsam/constrained/tests/testRiemannianStaircase.cpp @@ -0,0 +1,660 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testRiemannianStaircase.cpp + * @brief Unit tests for the Burer-Monteiro Riemannian Staircase solver. + * @author Zhexin Xu + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace gtsam; + +// MSVC doesn't define M_PI by default; use a local constant for portability. +constexpr double kPi = 3.14159265358979323846; + +namespace RingFixture { + +NonlinearFactorGraph RingGraph(size_t numPoses, double delta) { + NonlinearFactorGraph graph; + auto noise = noiseModel::Isotropic::Sigma(1, 1.0); + for (size_t i = 0; i < numPoses; ++i) { + graph.emplace_shared>( + Symbol('x', i), Symbol('x', (i + 1) % numPoses), + Rot2::fromAngle(delta), noise); + } + return graph; +} + +// Matrix-form (D=2) fixture: each value is a 2x2 row-orthonormal matrix. +Values RingQcqpValuesD2(size_t numPoses, double delta, double perturbation) { + Values values; + for (size_t i = 0; i < numPoses; ++i) { + InsertQcqpValue( + Symbol('x', i), + Rot2::fromAngle(i * delta + perturbation * static_cast(i)), + &values); + } + return values; +} + +// Shared ALM tuning used by most tests. Tighter than library defaults so the +// ring fixture converges to a 1st-order KKT point in <100 iterations. +AugmentedLagrangianParams::shared_ptr DefaultAlmParams() { + auto p = std::make_shared(); + p->maxIterations = 50; + p->initialMuEq = 10.0; + p->muEqIncreaseRate = 2.0; + p->absoluteViolationTolerance = 1e-8; + p->relativeViolationTolerance = 1e-8; + return p; +} + +} // namespace RingFixture + +/* ************************************************************************* */ +// At an ALM-converged Ystar, S = Q + sum_m lambda_m * A_m is PSD and S * Y ≈ 0 +// (1st-order KKT for the BM problem at eq. (16)). +TEST(RiemannianStaircase, CertificateMatchesKKT) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + constexpr size_t K = 2; + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const QcqpProblem qcqp(graph, K); + const Values initialValues = RingQcqpValuesD2(N, delta, 0.01); + + auto params = DefaultAlmParams(); + params->storeOptProgress = true; + params->maxIterations = 100; + params->absoluteViolationTolerance = 1e-10; + params->relativeViolationTolerance = 1e-10; + params->absoluteCostTolerance = 1e-12; + params->relativeCostTolerance = 1e-12; + + AugmentedLagrangianOptimizer alm(qcqp, initialValues, params); + const Values Ystar = alm.optimize(); + EXPECT(!alm.progress().empty()); + + const auto& lambdaEq = alm.progress().back().lambdaEq; + EXPECT_LONGS_EQUAL(static_cast(qcqp.eConstraints().size()), + static_cast(lambdaEq.size())); + + const auto layout = RiemannianStaircaseOptimizer::Layout::From(Ystar); + LONGS_EQUAL(N * 2, layout.totalDim); + + const Eigen::SparseMatrix S = + RiemannianStaircaseOptimizer::buildCertificate(qcqp, layout, lambdaEq); + LONGS_EQUAL(static_cast(layout.totalDim), S.rows()); + LONGS_EQUAL(static_cast(layout.totalDim), S.cols()); + + // (a) SDP dual cert PSD up to tolerance. + const Matrix Sdense(S); + Eigen::SelfAdjointEigenSolver es(Sdense); + const double lambdaMin = es.eigenvalues().minCoeff(); + EXPECT(lambdaMin > -1e-3); + + // (b) 1st-order KKT stationarity: ||S * Y|| / ||Y|| ~= 0. + // S is exactly the matrix assembled above; no rebuild needed. + const Matrix Ymat = layout.stack(Ystar); + const Matrix SY = S * Ymat; + const double stationarityRatio = SY.norm() / Ymat.norm(); + EXPECT(stationarityRatio < 1e-3); +} + +/* ************************************************************************* */ +// escapeSaddleAndLift widens each value by one column, leaving the first +// p columns unchanged and writing alpha * v into the new column. +TEST(RiemannianStaircase, EscapeSaddleAndLiftShape) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const Values Ystar = RingQcqpValuesD2(N, delta, 0.0); + const auto layout = RiemannianStaircaseOptimizer::Layout::From(Ystar); + LONGS_EQUAL(N * 2, layout.totalDim); + + Vector v = + Vector::LinSpaced(static_cast(layout.totalDim), 1.0, layout.totalDim); + v.normalize(); + + const double alpha = 1e-2; + const Values Ynext = + RiemannianStaircaseOptimizer::escapeSaddleAndLift(Ystar, v, layout, alpha); + + EXPECT_LONGS_EQUAL(static_cast(Ystar.size()), + static_cast(Ynext.size())); + for (const auto& [key, Yi] : Ystar.extract()) { + const Matrix YiNew = Ynext.at(key); + EXPECT_LONGS_EQUAL(Yi.rows(), YiNew.rows()); + EXPECT_LONGS_EQUAL(Yi.cols() + 1, YiNew.cols()); + EXPECT(assert_equal(Matrix(Yi), Matrix(YiNew.leftCols(Yi.cols())), 1e-12)); + + const auto& slice = layout.sliceOf(key); + const Matrix expectedCol = alpha * v.segment(slice.offset, slice.rowDim); + EXPECT(assert_equal(expectedCol, Matrix(YiNew.rightCols(1)), 1e-12)); + } +} + +/* ************************************************************************* */ +// Constraint violation at the lifted point grows like O(alpha^2): +// feasible Ystar at rank p + h(Y) = trace(Y' A Y) - b = 0, +// lifting to [Ystar | alpha v] gives h = alpha^2 v' A v. +TEST(RiemannianStaircase, EscapeSaddleViolationIsQuadratic) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const Values Ystar = RingQcqpValuesD2(N, delta, 0.0); + const auto layout = RiemannianStaircaseOptimizer::Layout::From(Ystar); + + const QcqpProblem qcqpBase(graph, 2); + EXPECT_DOUBLES_EQUAL(0.0, qcqpBase.eConstraints().violationNorm(Ystar), + 1e-12); + + Vector v = Vector::LinSpaced(static_cast(layout.totalDim), 1.0, + layout.totalDim); + v.normalize(); + + const QcqpProblem qcqpLifted(graph, 3); + const double alphaSmall = 1e-3; + const double alphaLarge = 2e-3; + const Values Yh = RiemannianStaircaseOptimizer::escapeSaddleAndLift( + Ystar, v, layout, alphaSmall); + const Values YH = RiemannianStaircaseOptimizer::escapeSaddleAndLift( + Ystar, v, layout, alphaLarge); + + const double vioSmall = qcqpLifted.eConstraints().violationNorm(Yh); + const double vioLarge = qcqpLifted.eConstraints().violationNorm(YH); + EXPECT(vioSmall > 0.0); + // alphaLarge/alphaSmall = 2, so vioLarge/vioSmall should be 4 (O(alpha^2)). + // 4% tolerance absorbs noise in violationNorm at this alpha regime. + EXPECT_DOUBLES_EQUAL(4.0, vioLarge / vioSmall, 4e-2); +} + +/* ************************************************************************* */ +// Both verifier methods reach the same verdict + final rank on an easy ring. +// Precise eigenvalue agreement is in VerifyStage2OnNegativeEigenvalue. +TEST(RiemannianStaircase, BothVerifyMethodsCertifyEasyRing) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const Values initial = RingQcqpValuesD2(N, delta, /*perturbation=*/0.01); + + RiemannianStaircaseParams paramsS; + paramsS.pMin = 2; + paramsS.pMax = 5; + paramsS.alpha = 1e-2; + paramsS.eta = 1e-3; + paramsS.verificationMethod = + RiemannianStaircaseParams::VerificationMethod::Spectra; + paramsS.almParams = DefaultAlmParams(); + paramsS.almParams->maxIterations = 100; + + RiemannianStaircaseParams paramsD = paramsS; + paramsD.verificationMethod = + RiemannianStaircaseParams::VerificationMethod::DenseEigen; + paramsD.almParams = + std::make_shared(*paramsS.almParams); + + const auto resultS = + RiemannianStaircaseOptimizer(graph, initial, paramsS).optimize(); + const auto resultD = + RiemannianStaircaseOptimizer(graph, initial, paramsD).optimize(); + + EXPECT(resultS.certified == resultD.certified); + EXPECT_LONGS_EQUAL(static_cast(resultS.finalRank), + static_cast(resultD.finalRank)); + // Spectra takes the Cholesky shortcut and reports lambda_min = -eta as a + // bound; both methods land in the cert-passing region. + EXPECT(resultS.minEigenvalue >= -paramsS.eta); + EXPECT(resultD.minEigenvalue >= -paramsD.eta); +} + +/* ************************************************************************* */ +// Easy case: well-initialized 5-Rot2 ring. The relaxation is tight; the +// certificate should pass at the starting rank or after at most one lift. +TEST(RiemannianStaircase, Rot2RingEasy) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const Values initial = RingQcqpValuesD2(N, delta, /*perturbation=*/0.01); + + RiemannianStaircaseParams params; + params.pMin = 2; + params.pMax = 5; + params.alpha = 1e-2; + params.eta = 1e-3; + params.almParams = DefaultAlmParams(); + params.almParams->maxIterations = 100; + params.almParams->absoluteCostTolerance = 1e-10; + params.almParams->relativeCostTolerance = 1e-10; + + const auto result = + RiemannianStaircaseOptimizer(graph, initial, params).optimize(); + + EXPECT(result.certified); + EXPECT(result.finalRank <= 3); + EXPECT(result.minEigenvalue >= -params.eta); +} + +/* ************************************************************************* */ +// Layout::From walks variables in sorted-Key order with contiguous offsets +// and totalDim equal to the sum of row dims. +TEST(Layout, OrderAndTotalDim) { + Values values; + const Matrix zero4x3 = Matrix::Zero(4, 3); + values.insert(Symbol('x', 2), zero4x3); + values.insert(Symbol('x', 0), zero4x3); + values.insert(Symbol('x', 1), zero4x3); + + const auto layout = RiemannianStaircaseOptimizer::Layout::From(values); + LONGS_EQUAL(12, layout.totalDim); + LONGS_EQUAL(3, layout.size()); + LONGS_EQUAL(0, layout.offsetOf(Symbol('x', 0))); + LONGS_EQUAL(4, layout.offsetOf(Symbol('x', 1))); + LONGS_EQUAL(8, layout.offsetOf(Symbol('x', 2))); +} + +/* ************************************************************************* */ +// Layout::From handles heterogeneous row dims (Rot2 + Rot3 shapes mixed). +TEST(Layout, HeterogeneousRowDims) { + Values values; + values.insert(Symbol('x', 0), Matrix(Matrix::Zero(2, 2))); + values.insert(Symbol('x', 1), Matrix(Matrix::Zero(3, 2))); + values.insert(Symbol('x', 2), Matrix(Matrix::Zero(2, 2))); + + const auto layout = RiemannianStaircaseOptimizer::Layout::From(values); + LONGS_EQUAL(2 + 3 + 2, layout.totalDim); + LONGS_EQUAL(0, layout.offsetOf(Symbol('x', 0))); + LONGS_EQUAL(2, layout.offsetOf(Symbol('x', 1))); + LONGS_EQUAL(5, layout.offsetOf(Symbol('x', 2))); + LONGS_EQUAL(3, layout.maxRowDim()); +} + +/* ************************************************************************* */ +// stack/unstack round-trip is exact. +TEST(Layout, StackUnstackRoundTrip) { + Values values; + const Matrix A = (Matrix(2, 3) << 1, 2, 3, 4, 5, 6).finished(); + const Matrix B = (Matrix(3, 3) << 7, 8, 9, 10, 11, 12, 13, 14, 15).finished(); + const Matrix C = (Matrix(2, 3) << 16, 17, 18, 19, 20, 21).finished(); + values.insert(Symbol('x', 0), A); + values.insert(Symbol('x', 1), B); + values.insert(Symbol('x', 2), C); + + const auto layout = RiemannianStaircaseOptimizer::Layout::From(values); + const Matrix Y = layout.stack(values); + LONGS_EQUAL(7, Y.rows()); + LONGS_EQUAL(3, Y.cols()); + + const Values back = layout.unstack(Y); + EXPECT(assert_equal(A, back.at(Symbol('x', 0)), 1e-15)); + EXPECT(assert_equal(B, back.at(Symbol('x', 1)), 1e-15)); + EXPECT(assert_equal(C, back.at(Symbol('x', 2)), 1e-15)); +} + +/* ************************************************************************* */ +// Non-QCQP-representable factors are rejected at construction (probe build +// at pMin throws). +TEST(RiemannianStaircase, RejectsNonQcqpFactor) { + NonlinearFactorGraph graph; + // BetweenFactor doesn't override qcqpFactors — the base throws. + graph.emplace_shared>( + Symbol('x', 0), Symbol('x', 1), Pose2(), + noiseModel::Isotropic::Sigma(3, 1.0)); + Values values; + InsertQcqpValue(Symbol('x', 0), Rot2::fromAngle(0.0), &values); + InsertQcqpValue(Symbol('x', 1), Rot2::fromAngle(0.0), &values); + + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer(graph, values, RiemannianStaircaseParams{}), + std::invalid_argument); +} + +/* ************************************************************************* */ +// pMin > pMax is rejected at construction. +TEST(RiemannianStaircase, RejectsInvertedPMinPMax) { + NonlinearFactorGraph graph; + auto noise = noiseModel::Isotropic::Sigma(1, 1.0); + graph.emplace_shared>( + Symbol('x', 0), Symbol('x', 1), Rot2::fromAngle(0.3), noise); + Values values; + InsertQcqpValue(Symbol('x', 0), Rot2::fromAngle(0.0), &values); + InsertQcqpValue(Symbol('x', 1), Rot2::fromAngle(0.0), &values); + + RiemannianStaircaseParams params; + params.pMin = 3; + params.pMax = 2; + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer(graph, values, params), + std::invalid_argument); +} + +/* ************************************************************************* */ +// truncateToRankD rejects d < 2 and d > stacked-matrix column count. +TEST(RiemannianStaircase, TruncateToRankDRejectsBadD) { + Values values; + values.insert(Symbol('x', 0), Matrix(Matrix::Zero(2, 4))); + values.insert(Symbol('x', 1), Matrix(Matrix::Zero(2, 4))); + const auto layout = RiemannianStaircaseOptimizer::Layout::From(values); + + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer::truncateToRankD(values, layout, /*d=*/1), + std::invalid_argument); + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer::truncateToRankD(values, layout, /*d=*/5), + std::invalid_argument); +} + +/* ************************************************************************* */ +// optimize() zero-pads initial values narrower than pMin and still certifies. +// XX' is invariant under zero-column padding, so feasibility is preserved. +TEST(RiemannianStaircase, AutoPadsNarrowInitialValues) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + // Initial values stored as 2x2 (rank-2 form). + const Values initial = RingQcqpValuesD2(N, delta, /*perturbation=*/0.01); + + // Ask the staircase to start at rank 3 — wider than the supplied 2x2. + RiemannianStaircaseParams params; + params.pMin = 3; + params.pMax = 4; + params.almParams = DefaultAlmParams(); + params.almParams->maxIterations = 100; + + const auto result = + RiemannianStaircaseOptimizer(graph, initial, params).optimize(); + + // Auto-pad to rank 3 must not crash and the run must certify on the ring. + EXPECT(result.certified); + EXPECT(result.finalRank >= 3 && result.finalRank <= params.pMax); +} + +/* ************************************************************************* */ +// padInitialValues zero-pads narrower entries to pMin columns; matching +// entries pass through; wider entries throw. +TEST(RiemannianStaircase, PadInitialValuesWidens) { + Values Y; + Y.insert(Symbol('x', 0), Matrix(Matrix::Identity(2, 2))); + Y.insert(Symbol('x', 1), Matrix(Matrix::Identity(3, 3))); + + const Values padded = + RiemannianStaircaseOptimizer::padInitialValues(Y, /*pMin=*/4); + + const Matrix M0 = padded.at(Symbol('x', 0)); + EXPECT_LONGS_EQUAL(2, M0.rows()); + EXPECT_LONGS_EQUAL(4, M0.cols()); + EXPECT(assert_equal(Matrix(Matrix::Identity(2, 2)), + Matrix(M0.leftCols<2>()), 1e-15)); + EXPECT(assert_equal(Matrix(Matrix::Zero(2, 2)), + Matrix(M0.rightCols<2>()), 1e-15)); + + const Matrix M1 = padded.at(Symbol('x', 1)); + EXPECT_LONGS_EQUAL(3, M1.rows()); + EXPECT_LONGS_EQUAL(4, M1.cols()); + EXPECT(assert_equal(Matrix(Matrix::Identity(3, 3)), + Matrix(M1.leftCols<3>()), 1e-15)); + EXPECT(assert_equal(Matrix(Matrix::Zero(3, 1)), + Matrix(M1.rightCols<1>()), 1e-15)); +} + +TEST(RiemannianStaircase, PadInitialValuesRejectsTooWide) { + Values Y; + Y.insert(Symbol('x', 0), Matrix(Matrix::Zero(2, 5))); + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer::padInitialValues(Y, /*pMin=*/3), + std::invalid_argument); +} + +/* ************************************************************************* */ +// runLocalSolver returns lambdaEq aligned 1:1 with qcqp.eConstraints() +// and a Y conforming to the input layout. +TEST(RiemannianStaircase, RunLocalSolverAlignsLambdaEq) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const QcqpProblem qcqp(graph, 2); + const Values initial = RingQcqpValuesD2(N, delta, 0.01); + + const auto inner = + RiemannianStaircaseOptimizer::runLocalSolver(qcqp, initial, + DefaultAlmParams()); + + EXPECT_LONGS_EQUAL(static_cast(qcqp.eConstraints().size()), + static_cast(inner.lambdaEq.size())); + const auto layout = RiemannianStaircaseOptimizer::Layout::From(initial); + EXPECT(layout.conformsTo(inner.Y)); +} + +/* ************************************************************************* */ +// buildDataMatrix returns a square symmetric matrix of size totalDim, with +// the K=p cost rolled into its 2x2 (rowDim=2) blocks. +TEST(RiemannianStaircase, BuildDataMatrixShape) { + using namespace RingFixture; + constexpr size_t N = 3; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const QcqpProblem qcqp(graph, 2); + const Values values = RingQcqpValuesD2(N, delta, 0.0); + const auto layout = RiemannianStaircaseOptimizer::Layout::From(values); + + const auto Q = + RiemannianStaircaseOptimizer::buildDataMatrix(qcqp, layout); + EXPECT_LONGS_EQUAL(static_cast(layout.totalDim), Q.rows()); + EXPECT_LONGS_EQUAL(static_cast(layout.totalDim), Q.cols()); + + const Matrix Qdense(Q); + EXPECT(assert_equal(Qdense, Matrix(Qdense.transpose()), 1e-12)); +} + +/* ************************************************************************* */ +// buildCertificate throws when lambdaEq is shorter than the number of +// quadratic equality constraints. +TEST(RiemannianStaircase, BuildCertificateShortLambdaEqThrows) { + using namespace RingFixture; + const NonlinearFactorGraph graph = RingGraph(3, 2.0 * kPi / 3.0); + const QcqpProblem qcqp(graph, 2); + const Values values = RingQcqpValuesD2(3, 2.0 * kPi / 3.0, 0.0); + const auto layout = RiemannianStaircaseOptimizer::Layout::From(values); + + const std::vector empty; + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer::buildCertificate(qcqp, layout, empty), + std::runtime_error); +} + +/* ************************************************************************* */ +// buildMultiplierMatrix throws when the constraint's A size doesn't match the +// per-key rowDim recorded in the layout. +TEST(RiemannianStaircase, BuildMultiplierMatrixRejectsASizeMismatch) { + using namespace RingFixture; + constexpr size_t N = 3; + constexpr double delta = 2.0 * kPi / static_cast(N); + + // QCQP built from a Rot2 graph — each equality constraint's A is 2 × 2. + const NonlinearFactorGraph graph = RingGraph(N, delta); + const QcqpProblem qcqp(graph, /*K=*/2); + + // But supply a layout where each key has rowDim = 3, mismatching A. + Values badValues; + for (size_t i = 0; i < N; ++i) { + badValues.insert(Symbol('x', i), Matrix(Matrix::Zero(3, 2))); + } + const auto badLayout = + RiemannianStaircaseOptimizer::Layout::From(badValues); + + std::vector lambdaEq(qcqp.eConstraints().size(), + Vector::Constant(1, 1.0)); + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer::buildMultiplierMatrix( + qcqp, badLayout, lambdaEq), + std::runtime_error); +} + +/* ************************************************************************* */ +// verify on a synthetic S = diag(1,1,1,1,-0.5) — Cholesky on S + eta*I +// fails, Stage-2 runs. Spectra and DenseEigen agree on the verdict and +// lambda_min to Lanczos tolerance. +TEST(RiemannianStaircase, VerifyStage2OnNegativeEigenvalue) { + const int n = 5; + std::vector> tri = { + {0, 0, 1.0}, {1, 1, 1.0}, {2, 2, 1.0}, {3, 3, 1.0}, {4, 4, -0.5}}; + Eigen::SparseMatrix S(n, n); + S.setFromTriplets(tri.begin(), tri.end()); + S.makeCompressed(); + + RiemannianStaircaseParams paramsS; + paramsS.eta = 1e-3; + paramsS.numLanczosVectors = 5; + paramsS.maxSpectraIters = 500; + paramsS.spectraTol = 1e-8; + paramsS.verificationMethod = + RiemannianStaircaseParams::VerificationMethod::Spectra; + + RiemannianStaircaseParams paramsD = paramsS; + paramsD.verificationMethod = + RiemannianStaircaseParams::VerificationMethod::DenseEigen; + + auto [passS, lamS, vS] = + RiemannianStaircaseOptimizer::verify(S, paramsS); + auto [passD, lamD, vD] = + RiemannianStaircaseOptimizer::verify(S, paramsD); + + EXPECT(!passS); + EXPECT(!passD); + EXPECT_DOUBLES_EQUAL(-0.5, lamD, 1e-9); + EXPECT(std::abs(lamS - lamD) < 1e-4); + // v_min concentrated on the negative-eigenvalue index, up to sign. + EXPECT(std::abs(std::abs(vD(4)) - 1.0) < 1e-9); + EXPECT(std::abs(std::abs(vS(4)) - 1.0) < 1e-3); +} + +/* ************************************************************************* */ +// pMin == 0 is rejected at construction. +TEST(RiemannianStaircase, RejectsZeroPMin) { + using namespace RingFixture; + const NonlinearFactorGraph graph = RingGraph(3, 2.0 * kPi / 3.0); + const Values values = RingQcqpValuesD2(3, 2.0 * kPi / 3.0, 0.0); + RiemannianStaircaseParams params; + params.pMin = 0; + params.pMax = 2; + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer(graph, values, params), + std::invalid_argument); +} + +/* ************************************************************************* */ +// Layout::stack throws when entries have inconsistent column counts. +TEST(Layout, StackColumnMismatchThrows) { + Values values; + values.insert(Symbol('x', 0), Matrix(Matrix::Zero(2, 3))); + values.insert(Symbol('x', 1), Matrix(Matrix::Zero(2, 4))); + const auto layout = + RiemannianStaircaseOptimizer::Layout::From(values); + CHECK_EXCEPTION(layout.stack(values), std::invalid_argument); +} + +/* ************************************************************************* */ +// Layout::unstack throws when the input row count doesn't match totalDim. +TEST(Layout, UnstackRowMismatchThrows) { + Values values; + values.insert(Symbol('x', 0), Matrix(Matrix::Zero(2, 3))); + values.insert(Symbol('x', 1), Matrix(Matrix::Zero(2, 3))); + const auto layout = + RiemannianStaircaseOptimizer::Layout::From(values); + const Matrix wrong = Matrix::Zero(7, 3); // totalDim is 4 + CHECK_EXCEPTION(layout.unstack(wrong), std::invalid_argument); +} + +/* ************************************************************************* */ +// escapeSaddleAndLift error paths: short vMin and non-conforming layout. +TEST(RiemannianStaircase, EscapeSaddleErrorPaths) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + const Values Ystar = RingQcqpValuesD2(N, delta, 0.0); + const auto layout = RiemannianStaircaseOptimizer::Layout::From(Ystar); + + const Vector shortV = Vector::Zero(layout.totalDim - 1); + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer::escapeSaddleAndLift( + Ystar, shortV, layout, 1e-2), + std::invalid_argument); + + Values nonConforming; + nonConforming.insert(Symbol('z', 0), Matrix(Matrix::Zero(3, 2))); + const Vector v = Vector::Zero(layout.totalDim); + CHECK_EXCEPTION( + RiemannianStaircaseOptimizer::escapeSaddleAndLift( + nonConforming, v, layout, 1e-2), + std::invalid_argument); +} + +/* ************************************************************************* */ +// pMax exhausted without certification: result.certified == false, +// result.rounded is empty, finalRank == pMax. +TEST(RiemannianStaircase, UncertifiedReturnWhenPMaxTooLow) { + using namespace RingFixture; + constexpr size_t N = 5; + constexpr double delta = 2.0 * kPi / static_cast(N); + + const NonlinearFactorGraph graph = RingGraph(N, delta); + const Values initial = RingQcqpValuesD2(N, delta, 0.01); + + // pMin = pMax = 2, but force a very strict eta so the verifier rejects + // any non-negative lambda_min that isn't *strongly* positive — guarantees + // the Cholesky stage fails and the staircase exhausts pMax with one level. + RiemannianStaircaseParams params; + params.pMin = 2; + params.pMax = 2; + params.eta = -1.0; // require lambda_min >= 1.0, impossible at KKT + params.almParams = DefaultAlmParams(); + + const auto result = + RiemannianStaircaseOptimizer(graph, initial, params).optimize(); + + EXPECT(!result.certified); + EXPECT_LONGS_EQUAL(2, static_cast(result.finalRank)); + EXPECT(!result.rounded.has_value()); + EXPECT_LONGS_EQUAL(1, static_cast(result.ranksVisited.size())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +}