diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 616d8a696d..3fdb3bdbd4 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -16,6 +16,7 @@ * @author Frank Dellaert */ +#include #include #include @@ -155,6 +156,14 @@ double Fair::loss(double distance) const { return c_2 * (normalizedError - std::log1p(normalizedError)); } +double Fair::graduatedWeight(double /*error*/, double /*error*/) const { + throw std::logic_error("Fair loss has no graduated form."); +} + +double Fair::graduatedLoss(double /*error*/, double /*error*/) const { + throw std::logic_error("Fair loss has no graduated form."); +} + void Fair::print(const std::string &s="") const { cout << s << "fair (" << c_ << ")" << endl; } @@ -177,20 +186,32 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { } } -double Huber::weight(double distance) const { +double Huber::Weight(double k, double distance) { const double absError = std::abs(distance); - return (absError <= k_) ? (1.0) : (k_ / absError); + return (absError <= k) ? (1.0) : (k / absError); } -double Huber::loss(double distance) const { +double Huber::Loss(double k, double distance) { const double absError = std::abs(distance); - if (absError <= k_) { // |x| <= k - return distance*distance / 2; - } else { // |x| > k - return k_ * (absError - (k_/2)); + if (absError <= k) { // |x| <= k + return distance * distance / 2; + } else { // |x| > k + return k * (absError - (k / 2)); } } +double Huber::weight(double distance) const { return Weight(k_, distance); } + +double Huber::loss(double distance) const { return Loss(k_, distance); } + +double Huber::graduatedWeight(double distance, double mu) const { + return Weight(mu * k_, distance); +} + +double Huber::graduatedLoss(double distance, double mu) const { + return Loss(mu * k_, distance); +} + void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } @@ -215,13 +236,27 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), } } +double Cauchy::Weight(double ksquared, double distance) { + return ksquared / (ksquared + distance * distance); +} + +double Cauchy::Loss(double ksquared, double distance) { + const double val = std::log1p(distance * distance / ksquared); + return ksquared * val * 0.5; +} + double Cauchy::weight(double distance) const { - return ksquared_ / (ksquared_ + distance*distance); + return Weight(ksquared_, distance); } -double Cauchy::loss(double distance) const { - const double val = std::log1p(distance * distance / ksquared_); - return ksquared_ * val * 0.5; +double Cauchy::loss(double distance) const { return Loss(ksquared_, distance); } + +double Cauchy::graduatedWeight(double distance, double mu) const { + return Cauchy::Weight((mu * mu) * ksquared_, distance); +} + +double Cauchy::graduatedLoss(double distance, double mu) const { + return Cauchy::Loss((mu * mu) * ksquared_, distance); } void Cauchy::print(const std::string &s="") const { @@ -248,25 +283,41 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c } } -double Tukey::weight(double distance) const { - if (std::abs(distance) <= c_) { - const double one_minus_xc2 = 1.0 - distance*distance/csquared_; +double Tukey::Weight(double c, double csquared, double distance) { + if (std::abs(distance) <= c) { + const double one_minus_xc2 = 1.0 - distance * distance / csquared; return one_minus_xc2 * one_minus_xc2; } return 0.0; } -double Tukey::loss(double distance) const { - double absError = std::abs(distance); - if (absError <= c_) { - const double one_minus_xc2 = 1.0 - distance*distance/csquared_; - const double t = one_minus_xc2*one_minus_xc2*one_minus_xc2; - return csquared_ * (1 - t) / 6.0; +double Tukey::Loss(double c, double csquared, double distance) { + const double absError = std::abs(distance); + if (absError <= c) { + const double one_minus_xc2 = 1.0 - distance * distance / csquared; + const double t = one_minus_xc2 * one_minus_xc2 * one_minus_xc2; + return csquared * (1 - t) / 6.0; } else { - return csquared_ / 6.0; + return csquared / 6.0; } } +double Tukey::weight(double distance) const { + return Weight(c_, csquared_, distance); +} + +double Tukey::loss(double distance) const { + return Loss(c_, csquared_, distance); +} + +double Tukey::graduatedWeight(double distance, double mu) const { + return Weight(mu * c_, (mu * mu) * csquared_, distance); +} + +double Tukey::graduatedLoss(double distance, double mu) const { + return Loss(mu * c_, (mu * mu) * csquared_, distance); +} + void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; } @@ -287,14 +338,28 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} -double Welsch::weight(double distance) const { - const double xc2 = (distance*distance)/csquared_; +double Welsch::Weight(double csquared, double distance) { + const double xc2 = (distance * distance) / csquared; return std::exp(-xc2); } -double Welsch::loss(double distance) const { - const double xc2 = (distance*distance)/csquared_; - return csquared_ * 0.5 * -std::expm1(-xc2); +double Welsch::Loss(double csquared, double distance) { + const double xc2 = (distance * distance) / csquared; + return csquared * 0.5 * -std::expm1(-xc2); +} + +double Welsch::weight(double distance) const { + return Weight(csquared_, distance); +} + +double Welsch::loss(double distance) const { return Loss(csquared_, distance); } + +double Welsch::graduatedWeight(double distance, double mu) const { + return Weight((mu * mu) * csquared_, distance); +} + +double Welsch::graduatedLoss(double distance, double mu) const { + return Loss((mu * mu) * csquared_, distance); } void Welsch::print(const std::string &s="") const { @@ -314,23 +379,46 @@ Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // GemanMcClure /* ************************************************************************* */ -GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) - : Base(reweight), c_(c), csquared_(c * c) { +GemanMcClure::GemanMcClure(double c, const GemanMcClure::GradScheme graduation, + const ReweightScheme reweight) + : Base(reweight), c_(c), csquared_(c * c), graduation_(graduation) {} + +double GemanMcClure::Weight(double csquared, double distance2) { + const double c4 = csquared * csquared; + const double c2error = csquared + distance2; + return c4 / (c2error * c2error); } -double GemanMcClure::weight(double distance) const { - return Weight(distance*distance, csquared_); +double GemanMcClure::Loss(double csquared, double distance2) { + return 0.5 * (csquared * distance2) / (csquared + distance2); } -double GemanMcClure::Weight(double distance2, double c2) { - const double c4 = c2*c2; - const double c2error = c2 + distance2; - return c4/(c2error*c2error); +double GemanMcClure::weight(double distance) const { + return Weight(csquared_, distance * distance); } double GemanMcClure::loss(double distance) const { - const double error2 = distance*distance; - return 0.5 * (csquared_ * error2) / (csquared_ + error2); + return Loss(csquared_, distance * distance); +} + +double GemanMcClure::graduatedWeight(double distance, double mu) const { + const double d2 = distance * distance; + if (graduation_ == GemanMcClure::GradScheme::STANDARD) { + return Weight(mu * csquared_, d2); + } else { // GemanMcClure::GradScheme::SCALE_INVARIANT + const double sqrt_denom = csquared_ + std::pow(d2, mu); + return (csquared_ * (csquared_ + std::pow(d2, mu) * (1 - mu))) / + (sqrt_denom * sqrt_denom); + } +} + +double GemanMcClure::graduatedLoss(double distance, double mu) const { + const double d2 = distance * distance; + if (graduation_ == GemanMcClure::GradScheme::STANDARD) { + return Loss(mu * csquared_, d2); + } else { // GemanMcClure::GradScheme::SCALE_INVARIANT + return 0.5 * (csquared_ * d2) / (csquared_ + std::pow(d2, mu)); + } } void GemanMcClure::print(const std::string &s="") const { @@ -343,51 +431,128 @@ bool GemanMcClure::equals(const Base &expected, double tol) const { return std::abs(c_ - p->c_) < tol; } -GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new GemanMcClure(c, reweight)); +GemanMcClure::shared_ptr GemanMcClure::Create( + double c, const GemanMcClure::GradScheme graduation, + const ReweightScheme reweight) { + return shared_ptr(new GemanMcClure(c, graduation, reweight)); +} + +/* ************************************************************************* */ +double GemanMcClure::shapeParamFromInfThresh(double influence_thresh, size_t dof, + double chi2_outlier_thresh) { + double outlier_residual_thresh = + 2 * gtsam_cephes_igami(dof / 2, chi2_outlier_thresh); + // Equation [d/dr \rho(x) = influence_thresh] solved for c + const double t1 = + std::sqrt(2 * influence_thresh * std::pow(outlier_residual_thresh, 5)); + const double t2 = influence_thresh * std::pow(outlier_residual_thresh, 2); + const double t3 = influence_thresh - 2 * outlier_residual_thresh; + return std::sqrt(-((t1 + t2) / t3)); } /* ************************************************************************* */ // TruncatedLeastSquares /* ************************************************************************* */ -TruncatedLeastSquares::TruncatedLeastSquares(double c, const ReweightScheme reweight) - : Base(reweight), c_(c), csquared_(c * c) { +TruncatedLeastSquares::TruncatedLeastSquares( + double c, const TruncatedLeastSquares::GradScheme graduation, + const ReweightScheme reweight) + : Base(reweight), c_(c), csquared_(c * c), graduation_(graduation) { if (c_ <= 0) { - throw runtime_error("mEstimator TruncatedLeastSquares takes only positive double in constructor."); + throw runtime_error( + "mEstimator TruncatedLeastSquares takes only positive double in " + "constructor."); } } -double TruncatedLeastSquares::weight(double distance) const { - const auto w = Weight(distance * distance, csquared_, csquared_); - return w.value(); +double TruncatedLeastSquares::Weight(double csquared, double distance2) { + if (distance2 <= csquared) { + return 1.0; + } + return 0.0; +} + +double TruncatedLeastSquares::Loss(double csquared, double distance2) { + if (distance2 <= csquared) { + return 0.5 * distance2; + } + return 0.5 * csquared; +} + +double TruncatedLeastSquares::GraduatedWeight(GradScheme graduation, + double csquared, double distance2, + double mu) { + if (graduation == TruncatedLeastSquares::GradScheme::STANDARD) { + return Weight(csquared * (mu * mu), distance2); + } else if (graduation == TruncatedLeastSquares::GradScheme::GNC_LINEAR) { + const double lowerbound = (mu / (mu + 1.0)) * csquared; + const double upperbound = ((mu + 1.0) / mu) * csquared; + + if (distance2 <= lowerbound) return 1.0; + if (distance2 >= upperbound) return 0.0; + return std::sqrt(csquared * mu * (mu + 1.0) / distance2) - mu; + } else { // TruncatedLeastSquares::GradScheme::GNC_SUPERLINEAR + const double lowerbound = csquared; + const double upperbound = ((mu + 1.0) * (mu + 1.0) / (mu * mu)) * csquared; + + if (distance2 <= lowerbound) return 1.0; + if (distance2 >= upperbound) return 0.0; + return std::sqrt(csquared / distance2) * (mu + 1.0) - mu; + } +} + +double TruncatedLeastSquares::GraduatedLoss(GradScheme graduation, + double csquared, double distance2, + double mu) { + if (graduation == TruncatedLeastSquares::GradScheme::STANDARD) { + return Loss((mu * mu) * csquared, distance2); + } else if (graduation == TruncatedLeastSquares::GradScheme::GNC_LINEAR) { + const double lowerbound = (mu / (mu + 1.0)) * csquared; + const double upperbound = ((mu + 1.0) / mu) * csquared; + + if (distance2 <= lowerbound) return 0.5 * distance2; + if (distance2 >= upperbound) return 0.5 * csquared; + return 0.5 * (2 * std::sqrt(csquared) * std::sqrt(distance2) * + std::sqrt(mu * (mu + 1.0)) - + (mu * (csquared + distance2))); + } else { // TruncatedLeastSquares::GradScheme::GNC_SUPERLINEAR + throw std::runtime_error( + "TLS with GradScheme::GNC_SUPERLINEAR has no loss form"); + } } -std::optional TruncatedLeastSquares::Weight(double distance2, double lowerbound, double upperbound) { - if (distance2 <= lowerbound) return 1.0; - if (distance2 >= upperbound) return 0.0; - return std::nullopt; +double TruncatedLeastSquares::weight(double distance) const { + return Weight(csquared_, distance * distance); } double TruncatedLeastSquares::loss(double distance) const { - if (std::abs(distance) <= c_) { - return 0.5 * distance * distance; - } - return 0.5 * csquared_; + return Loss(csquared_, distance * distance); +} + +double TruncatedLeastSquares::graduatedWeight(double distance, + double mu) const { + return GraduatedWeight(graduation_, csquared_, distance * distance, mu); +} + +double TruncatedLeastSquares::graduatedLoss(double distance, double mu) const { + return GraduatedLoss(graduation_, csquared_, distance * distance, mu); } -void TruncatedLeastSquares::print(const std::string &s="") const { +void TruncatedLeastSquares::print(const std::string& s = "") const { std::cout << s << ": TLS (" << c_ << ")" << std::endl; } -bool TruncatedLeastSquares::equals(const Base &expected, double tol) const { - const TruncatedLeastSquares* p = dynamic_cast(&expected); +bool TruncatedLeastSquares::equals(const Base& expected, double tol) const { + const TruncatedLeastSquares* p = + dynamic_cast(&expected); if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } -TruncatedLeastSquares::shared_ptr TruncatedLeastSquares::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new TruncatedLeastSquares(c, reweight)); +TruncatedLeastSquares::shared_ptr TruncatedLeastSquares::Create( + double c, TruncatedLeastSquares::GradScheme graduation, + const ReweightScheme reweight) { + return shared_ptr(new TruncatedLeastSquares(c, graduation, reweight)); } /* ************************************************************************* */ @@ -397,25 +562,34 @@ DCS::DCS(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double DCS::weight(double distance) const { - const double e2 = distance*distance; - if (e2 > c_) - { - const double w = 2.0*c_/(c_ + e2); - return w*w; +double DCS::Weight(double c, double distance) { + const double e2 = distance * distance; + if (e2 > c) { + const double w = 2.0 * c / (c + e2); + return w * w; } - return 1.0; } -double DCS::loss(double distance) const { +double DCS::Loss(double c, double distance) { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. - const double e2 = distance*distance; - const double e4 = e2*e2; - const double c2 = c_*c_; + const double e2 = distance * distance; + const double e4 = e2 * e2; + const double c2 = c * c; + return (c2 * e2 + c * e4) / ((e2 + c) * (e2 + c)); +} + +double DCS::weight(double distance) const { return Weight(c_, distance); } + +double DCS::loss(double distance) const { return Loss(c_, distance); } + +double DCS::graduatedWeight(double distance, double mu) const { + return Weight(mu * c_, distance); +} - return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_)); +double DCS::graduatedLoss(double distance, double mu) const { + return Loss(mu * c_, distance); } void DCS::print(const std::string &s="") const { @@ -457,6 +631,15 @@ double L2WithDeadZone::loss(double distance) const { return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } +double L2WithDeadZone::graduatedWeight(double /*error*/, + double /*error*/) const { + throw std::logic_error("L2WithDeadZone loss has no graduated form."); +} + +double L2WithDeadZone::graduatedLoss(double /*error*/, double /*error*/) const { + throw std::logic_error("L2WithDeadZone loss has no graduated form."); +} + void L2WithDeadZone::print(const std::string &s="") const { std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; } @@ -505,6 +688,16 @@ double AsymmetricTukey::loss(double distance) const { return csquared_ / 6.0; } +double AsymmetricTukey::graduatedWeight(double /*error*/, + double /*error*/) const { + throw std::logic_error("AsymmetricTukey loss has no graduated form."); +} + +double AsymmetricTukey::graduatedLoss(double /*error*/, + double /*error*/) const { + throw std::logic_error("AsymmetricTukey loss has no graduated form."); +} + void AsymmetricTukey::print(const std::string &s="") const { std::cout << s << ": AsymmetricTukey (" << c_ << ")" << std::endl; } @@ -549,6 +742,16 @@ double AsymmetricCauchy::loss(double distance) const { return ksquared_ * val * 0.5; } +double AsymmetricCauchy::graduatedWeight(double /*error*/, + double /*error*/) const { + throw std::logic_error("AsymmetricCauchy loss has no graduated form."); +} + +double AsymmetricCauchy::graduatedLoss(double /*error*/, + double /*error*/) const { + throw std::logic_error("AsymmetricCauchy loss has no graduated form."); +} + void AsymmetricCauchy::print(const std::string &s="") const { std::cout << s << ": AsymmetricCauchy (" << k_ << ")" << std::endl; } @@ -568,16 +771,36 @@ AsymmetricCauchy::shared_ptr AsymmetricCauchy::Create(double k, const ReweightSc // Custom /* ************************************************************************* */ -Custom::Custom(std::function weight, std::function loss, const ReweightScheme reweight, - std::string name) - : Base(reweight), weight_(std::move(weight)), loss_(loss), name_(std::move(name)) {} - -double Custom::weight(double distance) const { - return weight_(distance); +Custom::Custom(std::function weight, + std::function loss, + std::optional> grad_weight, + std::optional> grad_loss, + const ReweightScheme reweight, std::string name) + : Base(reweight), + weight_(std::move(weight)), + loss_(loss), + grad_weight_(grad_weight), + grad_loss_(grad_loss), + name_(std::move(name)) {} + +double Custom::weight(double distance) const { return weight_(distance); } + +double Custom::loss(double distance) const { return loss_(distance); } + +double Custom::graduatedWeight(double distance, double mu) const { + if (grad_weight_) { + return (*grad_weight_)(distance, mu); + } else { + throw std::logic_error("Custom loss provided no graduated form."); + } } -double Custom::loss(double distance) const { - return loss_(distance); +double Custom::graduatedLoss(double distance, double mu) const { + if (grad_loss_) { + return (*grad_loss_)(distance, mu); + } else { + throw std::logic_error("Custom loss provided no graduated form."); + } } void Custom::print(const std::string &s = "") const { @@ -592,9 +815,14 @@ bool Custom::equals(const Base &expected, double tol) const { loss_.target() == p->loss_.target() && reweight_ == p->reweight_; } -Custom::shared_ptr Custom::Create(std::function weight, std::function loss, - const ReweightScheme reweight, const std::string &name) { - return std::make_shared(std::move(weight), std::move(loss), reweight, name); +Custom::shared_ptr Custom::Create( + std::function weight, std::function loss, + std::optional> grad_weight, + std::optional> grad_loss, + const ReweightScheme reweight, const std::string& name) { + return std::make_shared(std::move(weight), std::move(loss), + std::move(grad_weight), std::move(grad_loss), + reweight, name); } } // namespace mEstimator diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 3976466d48..0e852558ed 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -54,6 +54,13 @@ namespace noiseModel { * and hence we can solve the equivalent weighted least squares problem \sum w(r_i) \rho(r_i) * * Each M-estimator in the mEstimator name space simply implements the above functions. + * + * Each M-estimator additionally implements "graduated" versions of these functions. + * Name Symbol + * Graduated Loss \phi(x,\mu) + * Graduated Weight \w(x,\mu) + * The control parameter \mu transitions the loss from convex to its original robust form. + * This is used by continuation-style algorithms (GNC, riSAM) to modify the underlying problem structure. */ // clang-format on namespace mEstimator { @@ -109,6 +116,22 @@ class GTSAM_EXPORT Base { */ virtual double weight(double distance) const = 0; + /** + * This method is responsible for returning the total penalty for a given + * amount of error and the current control parameter \f$\mu\f$. + * + * This returns \f$\rho(x, \mu)\f$ in \ref mEstimator + */ + virtual double graduatedLoss(double distance, double mu) const = 0; + + /** + * This method is responsible for returning the weight for a given + * amount of error and the current control parameter \f$\mu\f$. + * + * This returns \f$w(x, \mu)\f$ in \ref mEstimator + */ + virtual double graduatedWeight(double distance, double mu) const = 0; + virtual void print(const std::string &s) const = 0; virtual bool equals(const Base &expected, double tol = 1e-8) const = 0; @@ -157,6 +180,8 @@ class GTSAM_EXPORT Null : public Base { ~Null() override {} double weight(double /*error*/) const override { return 1.0; } double loss(double distance) const override { return 0.5 * distance * distance; } + double graduatedWeight(double distance, double /*error*/) const override { return weight(distance); } + double graduatedLoss(double distance, double /*error*/) const override { return loss(distance); } void print(const std::string &s) const override; bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; } static shared_ptr Create(); @@ -179,6 +204,8 @@ class GTSAM_EXPORT Null : public Base { * - Loss \rho(x) = c² (|x|/c - log(1+|x|/c)) * - Derivative \phi(x) = x/(1+|x|/c) * - Weight w(x) = \phi(x)/x = 1/(1+|x|/c) + * + * Fair loss has no graduated form. */ class GTSAM_EXPORT Fair : public Base { protected: @@ -190,6 +217,8 @@ class GTSAM_EXPORT Fair : public Base { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double /*error*/, double /*error*/) const override; + double graduatedLoss(double /*error*/, double /*error*/) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); @@ -214,6 +243,9 @@ class GTSAM_EXPORT Fair : public Base { * - Loss \rho(x) = 0.5 x² if |x| Robust: 1 + * - Loss, Derivative, and Weight computed by scaling k by control parameter mu */ class GTSAM_EXPORT Huber : public Base { protected: @@ -225,11 +257,18 @@ class GTSAM_EXPORT Huber : public Base { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); double modelParameter() const { return k_; } + /// @brief Static implementation of Huber Weight + static double Weight(double k, double distance); + /// @brief Static implementation of Huber Loss + static double Loss(double k, double distance); + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -254,6 +293,9 @@ class GTSAM_EXPORT Huber : public Base { * - Loss \rho(x) = 0.5 k² log(1+x²/k²) * - Derivative \phi(x) = (k²x)/(x²+k²) * - Weight w(x) = \phi(x)/x = k²/(x²+k²) + * + * Cauchy loss is graduated from Convex: \infty -> Robust: 1 + * - Loss, Derivative, and Weight computed by scaling k by control parameter mu */ class GTSAM_EXPORT Cauchy : public Base { protected: @@ -265,11 +307,18 @@ class GTSAM_EXPORT Cauchy : public Base { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); double modelParameter() const { return k_; } + /// @brief Static implementation of Cauchy Weight + static double Weight(double ksquared, double distance); + /// @brief Static implementation of Cauchy Loss + static double Loss(double ksquared, double distance); + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -290,6 +339,9 @@ class GTSAM_EXPORT Cauchy : public Base { * - Loss \f$ \rho(x) = c² (1 - (1-x²/c²)³)/6 \f$ if |x| Robust: 1 + * - Loss, Derivative, and Weight computed by scaling c by control parameter mu */ class GTSAM_EXPORT Tukey : public Base { protected: @@ -301,11 +353,19 @@ class GTSAM_EXPORT Tukey : public Base { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); double modelParameter() const { return c_; } + /// @brief Static implementation of Tukey Weight + static double Weight(double c, double csquared, double distance); + /// @brief Static implementation of Tukey Loss + static double Loss(double c, double csquared, double distance); + + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -325,6 +385,9 @@ class GTSAM_EXPORT Tukey : public Base { * - Loss \f$ \rho(x) = -0.5 c² (exp(-x²/c²) - 1) \f$ * - Derivative \f$ \phi(x) = x exp(-x²/c²) \f$ * - Weight \f$ w(x) = \phi(x)/x = exp(-x²/c²) \f$ + * + * Welsch loss is graduated from Convex: \infty -> Robust: 1 + * - Loss, Derivative, and Weight computed by scaling c by control parameter mu */ class GTSAM_EXPORT Welsch : public Base { protected: @@ -336,11 +399,18 @@ class GTSAM_EXPORT Welsch : public Base { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); double modelParameter() const { return c_; } + /// @brief Static implementation of Welsch Weight + static double Weight(double csquared, double distance); + /// @brief Static implementation of Welsch Loss + static double Loss(double csquared, double distance); + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ @@ -363,36 +433,60 @@ class GTSAM_EXPORT Welsch : public Base { * - Loss \rho(x) = 0.5 (c²x²)/(c²+x²) * - Derivative \phi(x) = xc⁴/(c²+x²)² * - Weight w(x) = \phi(x)/x = c⁴/(c²+x²)² + * + * Geman-McClure loss has two graduated forms + * + * STANDARD [1] is graduated from Convex: \infty -> Robust: 1 + * - Loss, Derivative, and Weight computed by scaling c² by control parameter mu + * + * SCALE_INVARIANT [2] is graduated from Convex: 0.0 -> Robust: 1.0 + * - Loss \rho(x) = 0.5 (c²x²)/(c²+(x²)^\mu) + * - Derivative \phi(x) = x(c²(c²+(x²)^\mu * (1-\mu)))/(c²+(x²)^\mu)² + * - Weight w(x) = \phi(x)/x = (c²(c²+(x²)^\mu * (1-\mu)))/(c²+(x²)^\mu)² */ class GTSAM_EXPORT GemanMcClure : public Base { public: typedef std::shared_ptr shared_ptr; + enum GradScheme { STANDARD, SCALE_INVARIANT }; - GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); + GemanMcClure(double c = 1.0, + const GradScheme graduation = GradScheme::STANDARD, + const ReweightScheme reweight = Block); ~GemanMcClure() override {} double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; - static shared_ptr Create(double k, const ReweightScheme reweight = Block); + static shared_ptr Create(double k, + const GradScheme graduation = GradScheme::STANDARD, + const ReweightScheme reweight = Block); double modelParameter() const { return c_; } - /** @brief A static helper function to compute the Geman-McClure robust weight. - * The static function takes the squared value of the residual and the scale parameter. - * The weight member function now calls this function. While the member function takes the residual as input, - * it passes x² and c² to the static helper. - * - * w(x², c²) = \phi(x)/x = c⁴/(c²+x²)² - * - * - * @param distance2 Squared residual magnitude. - * @param c2 Squared scale parameter. - * @return Weight w(x) in (0, 1] + + /// @brief Static implementation of GemanMcClure Weight + static double Weight(double csquared, double distance); + /// @brief Static implementation of GemanMcClure Loss + static double Loss(double csquared, double distance); + + /** @brief Static helper to compute shape param (c) using outlier influence. + * Computes a shape param such that an outlier (any measurement with + * residual equal to or greater than the chi2_outlier_threshold) will have an + * "influence" (derivative of loss) less than or equal to the + * influence_threshold: d/dx(\rho(x)) < influence_thresh + * @param influence_thresh - The max influence permited by an outlier + * @param dof - The degrees of freedom of the corresponding measurement + * @param chi2_outlier_thresh - The threshold for outlier (i.e. 0.95 = any + * measurement with residual greater than 95% of expected is an outlier) + * @returns The shape param */ - static double Weight(double distance2, double c2); + static double shapeParamFromInfThresh(double influence_thresh, size_t dof, + double chi2_outlier_thresh); protected: double c_; double csquared_; + GradScheme graduation_; private: #if GTSAM_ENABLE_BOOST_SERIALIZATION @@ -414,35 +508,62 @@ class GTSAM_EXPORT GemanMcClure : public Base { * - Loss \rho(x) = 0.5 x^2 if |x|<=c, 0.5 c^2 otherwise * - Derivative \phi(x) = x if |x|<=c, 0 otherwise * - Weight w(x) = \phi(x)/x = 1 if |x|<=c, 0 otherwise + * + * TLS has three graduated forms + * + * STANDARD TLS is graduated from Convex: \infty -> Robust: 1 + * - Loss, Derivative, and Weight computed by scaling c by control parameter mu + * + * GNC_LINEAR is graduated from Convex: 0 -> Robust \infty + * Let LB = c^2 (\mu / (\mu + 1)) and UB = c^2 ((\mu + 1) / \mu) + * - Loss \rho(x,\mu) = 0.5 * x^2 if x < LB + * = c|x|\sqrt(\mu(\mu+1))-\mu(c^2 + r^2) if LB < x < UB + * = c^2 if UB < x + * - Weight w(x, \mu) = 1 if x < LB + * = \sqrt(c^2 \mu ((\mu + 1.0) / x^2)) - \mu if LB < x < UB + * = 0 if UB < x + * + * GNC_SUPERLINEAR is graduated from Convex: 0 -> Robust \infty + * Let LB = c^2 and UB = c^2 (\mu + 1)^2 / \mu^2 + * - Loss \rho(x, \mu) = (not formally defined) + * - Weight w(x, \mu) = 1 if x < LB + * = sqrt(c^2 / x^2) * (\mu + 1.0) - \mu */ class GTSAM_EXPORT TruncatedLeastSquares : public Base { public: typedef std::shared_ptr shared_ptr; + enum GradScheme { STANDARD, GNC_LINEAR, GNC_SUPERLINEAR }; - TruncatedLeastSquares(double c = 1.0, const ReweightScheme reweight = Block); + TruncatedLeastSquares(double c = 1.0, + GradScheme graduation = GradScheme::STANDARD, + const ReweightScheme reweight = Block); double weight(double distance) const override; double loss(double distance) const override; - void print(const std::string &s) const override; - bool equals(const Base &expected, double tol = 1e-8) const override; - static shared_ptr Create(double c, const ReweightScheme reweight = Block); + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; + void print(const std::string& s) const override; + bool equals(const Base& expected, double tol = 1e-8) const override; + static shared_ptr Create(double c, + GradScheme graduation = GradScheme::STANDARD, + const ReweightScheme reweight = Block); double modelParameter() const { return c_; } - /** @brief A static helper function to compute the TLS robust weight. - * The static function takes the squared value of the residual, the squared lower bound, the squared upper bound. - * This helper returns a optional because it is also used for GNC, and we encounter transition weight cases, - * where the weight is not strictly binary (0 or 1) when the residual is within the transition region between inliers and outliers. - * The weight member function now calls the this function. - * While the member function takes the residual as input, it passes x², c² and c² to the static helper. - * - * @param distance2 Squared residual magnitude. - * @param lowerbound Squared lower bound. - * @param upperbound Squared upper bound. - * @return Weight w(x) is {0, 1} or None if the residual is between lowerbound and upperbound. - */ - static std::optional Weight(double distance2, double lowerbound, double upperbound); + + /// @brief Static implementation of TLS Weight + static double Weight(double csquared, double distance2); + /// @brief Static implementation of TLS Loss + static double Loss(double csquared, double distance2); + + /// @brief Static implementation of TLS Graduated Weight + static double GraduatedWeight(GradScheme graduation, double csquared, + double distance2, double mu); + /// @brief Static implementation of TLS Graduated Loss + static double GraduatedLoss(GradScheme graduation, double csquared, + double distance2, double mu); protected: double c_; double csquared_; + GradScheme graduation_; private: #if GTSAM_ENABLE_BOOST_SERIALIZATION @@ -468,6 +589,9 @@ class GTSAM_EXPORT TruncatedLeastSquares : public Base { * - Loss \rho(x) = (c²x² + cx⁴)/(x²+c)² (for any "x") * - Derivative \phi(x) = 2c²x/(x²+c)² * - Weight w(x) = \phi(x)/x = 2c²/(x²+c)² if x²>c, 1 otherwise + * + * DCS loss is graduated from Convex: \infty -> Robust: 1 + * - Loss, Derivative, and Weight computed by scaling c by control parameter mu */ class GTSAM_EXPORT DCS : public Base { public: @@ -477,11 +601,18 @@ class GTSAM_EXPORT DCS : public Base { ~DCS() override {} double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); double modelParameter() const { return c_; } + /// @brief Static implementation of DCS Weight + static double Weight(double c, double distance); + /// @brief Static implementation of DCS Loss + static double Loss(double c, double distance); + protected: double c_; @@ -509,6 +640,8 @@ class GTSAM_EXPORT DCS : public Base { * - Loss \f$ \rho(x) = 0 \f$ if |x|k, (k+x) if x<-k * - Weight \f$ w(x) = \phi(x)/x = 0 \f$ if |x|k, (k+x)/x if x<-k + * + * L2WithDeadZone loss has no graduated form. */ class GTSAM_EXPORT L2WithDeadZone : public Base { protected: @@ -520,6 +653,8 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double distance) const override; double loss(double distance) const override; + double graduatedWeight(double /*error*/, double /*error*/) const override; + double graduatedLoss(double /*error*/, double /*error*/) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -545,6 +680,8 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { * - Loss \rho(x) = c² (1 - (1-x²/c²)³)/6 if |x|; using CustomWeightFunction = std::function; +using CustomGraduatedLossFunction = + std::optional>; +using CustomGraduatedWeightFunction = + std::optional>; /** Implementation of the "Custom" robust error model. * * This model just takes two functions as input, one for the loss and one for the weight. + * + * Optionally this model can also define graduated loss functions */ class GTSAM_EXPORT Custom : public Base { protected: std::function weight_, loss_; + std::optional> grad_weight_, grad_loss_; std::string name_; public: typedef std::shared_ptr shared_ptr; Custom(CustomWeightFunction weight, CustomLossFunction loss, + CustomGraduatedWeightFunction grad_weight = std::nullopt, + CustomGraduatedLossFunction grad_loss = std::nullopt, const ReweightScheme reweight = Block, std::string name = "Custom"); double weight(double distance) const override; double loss(double distance) const override; - void print(const std::string &s) const override; - bool equals(const Base &expected, double tol = 1e-8) const override; - static shared_ptr Create(std::function weight, std::function loss, - const ReweightScheme reweight = Block, const std::string &name = "Custom"); + double graduatedWeight(double distance, double mu) const override; + double graduatedLoss(double distance, double mu) const override; + void print(const std::string& s) const override; + bool equals(const Base& expected, double tol = 1e-8) const override; + static shared_ptr Create( + std::function weight, std::function loss, + CustomGraduatedWeightFunction grad_weight = std::nullopt, + CustomGraduatedLossFunction grad_loss = std::nullopt, + const ReweightScheme reweight = Block, + const std::string& name = "Custom"); inline std::string& name() { return name_; } inline std::function& weightFunction() { return weight_; } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index f31f6ec323..1dd2040e5a 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -687,6 +687,49 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); } +/* ************************************************************************* */ +TEST(NoiseModel, robustFunctionGemanMcClureGraduatedScaleInvariant) { + mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create( + 1.0, mEstimator::GemanMcClure::GradScheme::SCALE_INVARIANT); + // At zero error is not dependent on mu + DOUBLES_EQUAL(0.0, gmc->graduatedLoss(0.0, 0.0), 1e-9); + CHECK(assert_equal(0.0, gmc->graduatedLoss(0.0, 0.5), 1e-9)); + CHECK(assert_equal(0.0, gmc->graduatedLoss(0.0, 1.0), 1e-9)); + + // For Mu = 0.0 error is quadratic + DOUBLES_EQUAL(0.0025, gmc->graduatedLoss(0.1, 0.0), 1e-9); + DOUBLES_EQUAL(0.4225, gmc->graduatedLoss(1.3, 0.0), 1e-9); + DOUBLES_EQUAL(38.750625, gmc->graduatedLoss(12.45, 0.0), 1e-9); + + // For 0.0 < Mu Error depends on shape + DOUBLES_EQUAL(0.00454545454, gmc->graduatedLoss(0.1, 0.5), 1e-9); + DOUBLES_EQUAL(0.36739130434, gmc->graduatedLoss(1.3, 0.5), 1e-9); + DOUBLES_EQUAL(5.76217472119, gmc->graduatedLoss(12.45, 0.5), 1e-9); + + // For Mu == 1.0 error depends is Geman-Mcclure + DOUBLES_EQUAL(0.00495049504, gmc->graduatedLoss(0.1, 1.0), 1e-9); + DOUBLES_EQUAL(0.31412639405, gmc->graduatedLoss(1.3, 1.0), 1e-9); + DOUBLES_EQUAL(0.49679492315, gmc->graduatedLoss(12.45, 1.0), 1e-9); + + // At Mu = 0 weights are identical at 0.5 + DOUBLES_EQUAL(0.5, gmc->graduatedWeight(0.1, 0.0), 1e-9); + DOUBLES_EQUAL(0.5, gmc->graduatedWeight(1.3, 0.0), 1e-9); + DOUBLES_EQUAL(0.5, gmc->graduatedWeight(12.45, 0.0), 1e-9); + + // At Mu = 1 weights are higher for low error + DOUBLES_EQUAL(0.9802960494, gmc->graduatedWeight(0.1, 1.0), 1e-9); + DOUBLES_EQUAL(0.13819598955, gmc->graduatedWeight(1.3, 1.0), 1e-9); + DOUBLES_EQUAL(0.00004109007, gmc->graduatedWeight(12.45, 1.0), 1e-9); + + // At Mu = 1 large residuals have ~0 weight + DOUBLES_EQUAL(0.0, gmc->graduatedWeight(2000.0, 1.0), 1e-9); + + // Across Mu residual of 0 will have weight = 1 + DOUBLES_EQUAL(1.0, gmc->graduatedWeight(0.0, 0.1), 1e-9); + DOUBLES_EQUAL(1.0, gmc->graduatedWeight(0.0, 0.5), 1e-9); + DOUBLES_EQUAL(1.0, gmc->graduatedWeight(0.0, 0.8), 1e-9); +} + TEST(NoiseModel, robustFunctionTLS) { const double k = 4.0, error1 = 0.5, error2 = 10.0, error3 = -10.0, error4 = -0.5; @@ -702,6 +745,39 @@ TEST(NoiseModel, robustFunctionTLS) DOUBLES_EQUAL(0.1250, tls->loss(error4), 1e-8); } + +TEST(NoiseModel, robustFunctionTruncatedLeastSquaresGraduatedLinear) { + const double k = 5.0, e1 = 1.0, e2 = 10.0; + const mEstimator::TruncatedLeastSquares::shared_ptr tls = + mEstimator::TruncatedLeastSquares::Create( + k, mEstimator::TruncatedLeastSquares::GradScheme::GNC_LINEAR); + // Convex for \mu = 0 + DOUBLES_EQUAL(0.005, tls->graduatedWeight(e1, 1e-6), 1e-6); + DOUBLES_EQUAL(0.0005, tls->graduatedWeight(e2, 1e-6), 1e-6); + // Standard for large \mu + DOUBLES_EQUAL(tls->weight(e1), tls->graduatedWeight(e1, 1e8), 1e-6); + DOUBLES_EQUAL(tls->weight(e2), tls->graduatedWeight(e2, 1e8), 1e-6); + // Convex for \mu = 0 + DOUBLES_EQUAL(0.0005, tls->graduatedLoss(e1, 1e-8), 1e-4); + DOUBLES_EQUAL(0.005, tls->graduatedLoss(e2, 1e-8), 1e-4); + // Standard for large \mu + DOUBLES_EQUAL(tls->loss(e1), tls->graduatedLoss(e1, 1e8), 1e-6); + DOUBLES_EQUAL(tls->loss(e2), tls->graduatedLoss(e2, 1e8), 1e-6); +} + +TEST(NoiseModel, robustFunctionTruncatedLeastSquaresGraduatedSuperLinear) { + const double k = 5.0, e1 = 1.0, e2 = 10.0; + const mEstimator::TruncatedLeastSquares::shared_ptr tls = + mEstimator::TruncatedLeastSquares::Create( + k, mEstimator::TruncatedLeastSquares::GradScheme::GNC_SUPERLINEAR); + // Convex for \mu = 0 + DOUBLES_EQUAL(1, tls->graduatedWeight(e1, 1e-6), 1e-6); + DOUBLES_EQUAL(0.5, tls->graduatedWeight(e2, 1e-6), 1e-6); + // Standard for large \mu + DOUBLES_EQUAL(tls->weight(e1), tls->graduatedWeight(e1, 1e8), 1e-6); + DOUBLES_EQUAL(tls->weight(e2), tls->graduatedWeight(e2, 1e8), 1e-6); +} + TEST(NoiseModel, robustFunctionWelsch) { const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; @@ -734,6 +810,7 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); } + TEST(NoiseModel, robustFunctionAsymmetricTukey) { const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; @@ -809,9 +886,11 @@ TEST(NoiseModel, robustNoiseGemanMcClure) const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0; Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); Vector b = Vector2(error1, error2); - const Robust::shared_ptr robust = Robust::Create( - mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar), - Unit::Create(2)); + const Robust::shared_ptr robust = + Robust::Create(mEstimator::GemanMcClure::Create( + k, mEstimator::GemanMcClure::GradScheme::STANDARD, + mEstimator::GemanMcClure::Scalar), + Unit::Create(2)); robust->WhitenSystem(A, b); @@ -838,8 +917,10 @@ TEST(NoiseModel, robustNoiseTLS) Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); Vector b = Vector2(error1, error2); const Robust::shared_ptr robust = Robust::Create( - mEstimator::TruncatedLeastSquares::Create(k, mEstimator::TruncatedLeastSquares::Scalar), - Unit::Create(2)); + mEstimator::TruncatedLeastSquares::Create( + k, mEstimator::TruncatedLeastSquares::GradScheme::STANDARD, + mEstimator::TruncatedLeastSquares::Scalar), + Unit::Create(2)); robust->WhitenSystem(A, b); @@ -891,7 +972,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, robust->squaredMahalanobisDistance(error), 1e-8); } -} +} /* ************************************************************************* */ TEST(NoiseModel, robustNoiseCustomHuber) { @@ -908,6 +989,7 @@ TEST(NoiseModel, robustNoiseCustomHuber) { const auto abs_e = std::abs(e); return abs_e <= k ? abs_e * abs_e / 2.0 : k * abs_e - k * k / 2.0; }, + std::nullopt, std::nullopt, noiseModel::mEstimator::Custom::Scalar, "Huber"), Unit::Create(2)); @@ -922,8 +1004,38 @@ TEST(NoiseModel, robustNoiseCustomHuber) { DOUBLES_EQUAL(sqrt(k / 100.0) * 1000.0, A(1, 1), 1e-8); } -TEST(NoiseModel, lossFunctionAtZero) -{ +TEST(NoiseModel, graduatedWeightLossAll) { + const double e1 = 1.0, e2 = 10.0, k = 5.0; + auto testFunc = [&](const mEstimator::Base::shared_ptr mest, double infinity, bool is_dcs) -> void { + // Convex For large \mu + DOUBLES_EQUAL(1.0, mest->graduatedWeight(e1, infinity), 1e-5); + DOUBLES_EQUAL(1.0, mest->graduatedWeight(e2, infinity), 1e-5); + // Standard for \mu = 1 + DOUBLES_EQUAL(mest->weight(e1), mest->graduatedWeight(e1, 1.0), 1e-5); + DOUBLES_EQUAL(mest->weight(e2), mest->graduatedWeight(e2, 1.0), 1e-5); + // Convex For large \mu + if (is_dcs) { + DOUBLES_EQUAL(e1 * e1, mest->graduatedLoss(e1, infinity), 1e-5); + DOUBLES_EQUAL(e2 * e2, mest->graduatedLoss(e2, infinity), 1e-5); + } else { + DOUBLES_EQUAL(0.5 * e1 * e1, mest->graduatedLoss(e1, infinity), 1e-5); + DOUBLES_EQUAL(0.5 * e2 * e2, mest->graduatedLoss(e2, infinity), 1e-5); + } + // Standard for \mu = 1 + DOUBLES_EQUAL(mest->loss(e1), mest->graduatedLoss(e1, 1.0), 1e-5); + DOUBLES_EQUAL(mest->loss(e2), mest->graduatedLoss(e2, 1.0), 1e-5); + }; + + testFunc(mEstimator::Huber::Create(k), 1e8, false); + testFunc(mEstimator::Cauchy::Create(k), 1e8, false); + testFunc(mEstimator::GemanMcClure::Create(k), 1e12, false); + testFunc(mEstimator::TruncatedLeastSquares::Create(k), 1e12, false); + testFunc(mEstimator::Welsch::Create(k), 1e12, false); + testFunc(mEstimator::Tukey::Create(k), 1e5, false); + testFunc(mEstimator::DCS::Create(k), 1e12, true); +} + +TEST(NoiseModel, lossFunctionAtZero) { const double k = 5.0; auto fair = mEstimator::Fair::Create(k); DOUBLES_EQUAL(fair->loss(0), 0, 1e-8); @@ -955,8 +1067,48 @@ TEST(NoiseModel, lossFunctionAtZero) auto assy_tukey = mEstimator::AsymmetricTukey::Create(k); DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); + auto tls = mEstimator::TruncatedLeastSquares::Create(k); + DOUBLES_EQUAL(tls->loss(0), 0, 1e-8); + DOUBLES_EQUAL(tls->weight(0), 1, 1e-8); } +TEST(NoiseModel, lossFunctionAtZeroGraduated) { + const double k = 5.0; + const double mu = 10; + auto huber = mEstimator::Huber::Create(k); + DOUBLES_EQUAL(huber->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(huber->graduatedWeight(0, mu), 1, 1e-8); + auto cauchy = mEstimator::Cauchy::Create(k); + DOUBLES_EQUAL(cauchy->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(cauchy->graduatedWeight(0, mu), 1, 1e-8); + auto gmc = mEstimator::GemanMcClure::Create(k); + DOUBLES_EQUAL(gmc->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(gmc->graduatedWeight(0, mu), 1, 1e-8); + auto gmc_si = mEstimator::GemanMcClure::Create( + k, mEstimator::GemanMcClure::GradScheme::SCALE_INVARIANT); + DOUBLES_EQUAL(gmc_si->graduatedLoss(0, 0.5), 0, 1e-8); + DOUBLES_EQUAL(gmc_si->graduatedWeight(0, 0.5), 1, 1e-8); + auto welsch = mEstimator::Welsch::Create(k); + DOUBLES_EQUAL(welsch->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(welsch->graduatedWeight(0, mu), 1, 1e-8); + auto tukey = mEstimator::Tukey::Create(k); + DOUBLES_EQUAL(tukey->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(tukey->graduatedWeight(0, mu), 1, 1e-8); + auto dcs = mEstimator::DCS::Create(k); + DOUBLES_EQUAL(dcs->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(dcs->graduatedWeight(0, mu), 1, 1e-8); + auto tls_std = mEstimator::TruncatedLeastSquares::Create( + k, mEstimator::TruncatedLeastSquares::GradScheme::STANDARD); + DOUBLES_EQUAL(tls_std->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(tls_std->graduatedWeight(0, mu), 1, 1e-8); + auto tls_lin = mEstimator::TruncatedLeastSquares::Create( + k, mEstimator::TruncatedLeastSquares::GradScheme::GNC_LINEAR); + DOUBLES_EQUAL(tls_lin->graduatedLoss(0, mu), 0, 1e-8); + DOUBLES_EQUAL(tls_lin->graduatedWeight(0, mu), 1, 1e-8); + auto tls_sup = mEstimator::TruncatedLeastSquares::Create( + k, mEstimator::TruncatedLeastSquares::GradScheme::GNC_SUPERLINEAR); + DOUBLES_EQUAL(tls_sup->graduatedWeight(0, mu), 1, 1e-8); +} /* ************************************************************************* */ #define TEST_GAUSSIAN(gaussian)\ diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3f4f769f59..a7cfccb437 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -515,7 +515,7 @@ class GncOptimizer { for (size_t k = 0; k < nfg_.size(); k++) { if (needsWeightUpdate(factorTypes_[k])) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - weights[k] = noiseModel::mEstimator::GemanMcClure::Weight(u2_k, mu * barcSq_[k]); + weights[k] = noiseModel::mEstimator::GemanMcClure::Weight(mu * barcSq_[k], u2_k); } } return weights; @@ -526,29 +526,19 @@ class GncOptimizer { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual switch (params_.scheduler) { case GncScheduler::SuperLinear: { - double lowerbound = barcSq_[k]; - double upperbound = ((mu + 1.0) * (mu + 1.0) / (mu * mu)) * barcSq_[k]; - auto w = noiseModel::mEstimator::TruncatedLeastSquares::Weight(u2_k, lowerbound, upperbound); - if (w) { - weights[k] = *w; - } - else { - double transition_weight = std::sqrt(barcSq_[k] / u2_k) * (mu + 1.0) - mu; - weights[k] = std::clamp(transition_weight, 0.0, 1.0); - } + weights[k] = noiseModel::mEstimator::TruncatedLeastSquares:: + GraduatedWeight( + noiseModel::mEstimator::TruncatedLeastSquares:: + GradScheme::GNC_SUPERLINEAR, + barcSq_[k], u2_k, mu); break; } case GncScheduler::Linear: { // use eq (14) in GNC paper - double upperbound = ((mu + 1.0) / mu) * barcSq_[k]; - double lowerbound = (mu / (mu + 1.0)) * barcSq_[k]; - auto w = noiseModel::mEstimator::TruncatedLeastSquares::Weight(u2_k, lowerbound, upperbound); - if (w) { - weights[k] = *w; - } - else { - double transition_weight = std::sqrt(barcSq_[k] * mu * (mu + 1.0) / u2_k) - mu; - weights[k] = std::clamp(transition_weight, 0.0, 1.0); - } + weights[k] = noiseModel::mEstimator::TruncatedLeastSquares:: + GraduatedWeight( + noiseModel::mEstimator::TruncatedLeastSquares:: + GradScheme::GNC_LINEAR, + barcSq_[k], u2_k, mu); break; } default: diff --git a/gtsam/nonlinear/doc/RISAM.ipynb b/gtsam/nonlinear/doc/RISAM.ipynb new file mode 100644 index 0000000000..71769de9fb --- /dev/null +++ b/gtsam/nonlinear/doc/RISAM.ipynb @@ -0,0 +1,125 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "c950beef", + "metadata": {}, + "source": [ + "# RISAM - Robust Incremental Smoothing and Mapping\n", + "\n", + "## Overview\n", + "\n", + "The `RISAM` class in GTSAM is designed to perform online robust optimization using an incrementalized version of [Graduated Non-Convexity](GNCOptimizer.ipynb) built on the [ISAM2](ISAM2.ipynb) algorithm. This method is intended for scenarios where the incremental optimization problem is affected by outliers. In cases where all measurements are known to be inliers `RISAM` functions identically to `ISAM2`, however, when potential outliers are incorporated `RISAM` applies an incremental GNC step to the effected problem improving robustness over standard ISAM2 but preventing sensitivity to initialization found in M-Estimation approaches.\n", + "\n", + "Like the `GNCOptimizer`, `RISAM` leverages a robust cost function $\\rho(e)$, where $e$ is the error term. The goal is to minimize the sum of these robust costs over all measurements:\n", + "\n", + "$$\n", + "\\min_x \\sum_i \\rho(e_i(x))\n", + "$$\n", + "\n", + "Unlike the batch setting that `GNCOptimzier` is used for, `RISAM` targets the incremental problem where we incrementally incorporate measurements online.\n", + "\n", + "$$\n", + "\\min_{x^t} \\sum_i \\rho(e_i(x^t))\n", + "$$\n", + "\n", + "Where we re-solve only a small subproblem at each step that is affected by the new measurements. `RISAM` solves this sub-problem robustly by solving a continuation of problems defined by graduated robust kernel $\\rho(e, \\mu)$ where the control parameter $\\mu$ smoothly transitions the kernel from quadratic ($x^2$) to a robust loss.\n", + "\n", + "$$\n", + "\\rho(e^t, \\mu)\n", + "$$\n", + "\n", + "By starting with non-robust error `RISAM` better handles poor-initialization, and by transitioning to the final robust loss `RISAM` removes the influence of outliers." + ] + }, + { + "cell_type": "markdown", + "id": "6750517f", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "id": "b9287492", + "metadata": {}, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ab1f5a90", + "metadata": {}, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass" + ] + }, + { + "cell_type": "markdown", + "id": "48cc5667", + "metadata": {}, + "source": [ + "## Key features:\n", + "\n", + "- **Online Robust Optimization**: `RISAM` is designed to support incremental optimization problems with outliers, using a robust cost function that can mitigate their effects.\n", + "- **Incremental Graduated Non-Convexity**: This technique allows the optimizer to solve each incremental subproblem with a convex problem and gradually transform it into the original non-convex problem, which helps in avoiding local minima.\n", + "\n", + "## Key Methods + Classes\n", + "\n", + "`RISAM` is designed to be a drop-in replacement for ISAM2. To see details on its key methods see [ISAM2.ipynb](ISAM2.ipynb).\n", + "\n", + "Additional Key Methods:\n", + "* `getOutliers`: Returns the set of measurements currently classified as outliers.\n", + "\n", + "Additional Key Helpers + Classes\n", + "* `make_graduated`: Constructs any factor as a `GraduatedFactor` which identifies factors as possible outliers to `RISAM`. All other factors are treated as known inliers.\n", + "* `SIGKernel`: The suggested kernel to use with `GraduatedFactors` provides stable optimization performance.\n", + "\n", + "## Parameters\n", + "\n", + "The `RISAM::Parameters` class defines parameters specific to `RISAM`:\n", + "\n", + "| Parameter | Type | Default Value | Description |\n", + "|-----------|------|---------------|-------------|\n", + "| isam2_params | ISAM2Params | ISAM2Params() | The parameters for the encapsulated ISAM2 optimizer. It is recommended to use DogLegLineSearch for optimization. |\n", + "| increment_outlier_mu | bool | true | Whether to increment the initial value of $\\mu$ used for each incremental GNC update over time as certainty of their inlier/outlier status increases. |\n", + "| outlier_mu_chisq_upper_bound | double | 0.95 | The $\\chi^2$ threshold for factor residual to consider it an outlier for $\\mu_{init}$ updates.. |\n", + "| outlier_mu_chisq_lower_bound | double | 0.25| The $\\chi^2$ threshold for factor residual to consider it strong inlier for $\\mu_{init}$ updates. |\n", + "| outlier_mu_avg_var_convergence_thresh | double | 0.01 | The threshold average variable delta to initiate $\\mu_{init}$ updates. |\n", + "| number_extra_iters | size_t | 1 | The number of extra `ISAM2::updates` called internally at each iteration after converging to the fully robust problem. |\n", + "\n", + "## Usage Considerations\n", + "\n", + "- **Outlier Rejection**: `RISAM` is particularly effective in online scenarios with significant outlier presence, such as online SLAM.\n", + "- **Trust Region Optimization**: While `RISAM` can use any underlying optimization step methods supported by ISAM2 it is strongly recommended to use `DoglegLineSearch` as it accounts for the changes in problem structure (convexity changes) and prevents divergence through trust region steps.\n", + "\n", + "## Files\n", + "\n", + "- [RISAM.h](https://github.com/borglab/gtsam/blob/develop/gtsam/sam/RISAM.h)\n", + "- [RISAMGraduatedFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/sam/RISAMGraduatedFactor.h)\n", + "- [RISAMGraduatedKernel.h](https://github.com/borglab/gtsam/blob/develop/gtsam/sam/RISAMGraduatedKernel.h)" + ] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/sam/RISAM.cpp b/gtsam/sam/RISAM.cpp new file mode 100644 index 0000000000..e61eb950f7 --- /dev/null +++ b/gtsam/sam/RISAM.cpp @@ -0,0 +1,366 @@ +/* ---------------------------------------------------------------------------- + + * 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 RISAM.cpp + * @brief Implementation of the RISAM algorithm. + * @author Dan McGann + * @date October 2025 + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +RISAM::UpdateResult RISAM::update( + const NonlinearFactorGraph& new_factors, const Values& new_theta, + const std::optional> extra_gnc_involved_keys, + const FactorIndices& remove_factor_indices, + const std::optional>& constrained_keys, + const std::optional>& no_relin_keys, + const std::optional>& extra_reelim_keys, + bool force_relinearize) { + ISAM2UpdateParams update_params; + update_params.removeFactorIndices = remove_factor_indices; + update_params.constrainedKeys = constrained_keys; + update_params.noRelinKeys = no_relin_keys; + update_params.extraReelimKeys = extra_reelim_keys; + update_params.force_relinearize = force_relinearize; + return update(new_factors, new_theta, extra_gnc_involved_keys, update_params); +} + +/* ************************************************************************* */ +RISAM::UpdateResult RISAM::update( + const NonlinearFactorGraph& new_factors, const Values& new_theta, + const std::optional> extra_gnc_involved_keys, + const ISAM2UpdateParams& update_params) { + // Determine if the update includes any graduated factors + bool update_includes_potential_outliers = false; + for (auto& factor : new_factors) { + auto grad_factor = std::dynamic_pointer_cast(factor); + if (grad_factor) update_includes_potential_outliers = true; + } + + // Update housekeeping for any mu_inits + updateHouseKeeping(new_factors, update_params); + + // Run the update: robust if we have potential outliers (or if requested) + // otherwise standard iSAM2 to improve efficiency + UpdateResult result; + if (extra_gnc_involved_keys || update_includes_potential_outliers) { + result = updateRobust(new_factors, new_theta, extra_gnc_involved_keys, + update_params); + } else { + result.isam2_result = + solver_->update(new_factors, new_theta, update_params); + solver_->calculateEstimate(); + } + + // If we have converged update mu_inits_based on status + if (params_.increment_outlier_mu) incrementMuInits(); + + return result; +} + +/* ************************************************************************* */ +Values RISAM::calculateEstimate() { return solver_->calculateEstimate(); } + +/* ************************************************************************* */ +std::set RISAM::getOutliers(double chi2_outlier_thresh) { + std::set outlier_factors; + Values current_est = solver_->calculateEstimate(); + + for (size_t i = 0; i < factors_.size(); i++) { + auto nlf_ptr = factors_.at(i); + auto grad_ptr = std::dynamic_pointer_cast(nlf_ptr); + if (grad_ptr) { + const double thresh = + internal::chiSquaredQuantile(nlf_ptr->dim(), chi2_outlier_thresh); + const double residual = grad_ptr->residual(current_est); + if (residual > thresh) outlier_factors.insert(i); + } + } + return outlier_factors; +} + +/* ************************************************************************* */ +RISAM::UpdateResult RISAM::updateRobust( + const NonlinearFactorGraph& new_factors, const Values& new_theta, + const std::optional>& extra_gnc_involved_keys, + const ISAM2UpdateParams& update_params) { + // Setup the result structure + UpdateResult result; + + // Convexify involved factors + ISAM2UpdateParams init_params = update_params; + FactorIndices convex_factors = convexifyInvolvedFactors( + new_factors, new_theta, extra_gnc_involved_keys, init_params, result); + + // Run the initial update to add new factors, after this the iSAM2 + // factors/var_index will match the risam copies + result.isam2_result = solver_->update(new_factors, new_theta, init_params); + solver_->calculateEstimate(); + + // Update count used for some convexity graduation schedules + std::map mu_update_count; + for (auto& fidx : convex_factors) mu_update_count[fidx] = 0; + /// Update until all convex factors have converged + while (convex_factors.size() > 0) { + convex_factors = runRobustIteration(convex_factors, mu_update_count); + } + + // Orig RISAM preformed 1 extra iteration for better convergence + for (size_t i = 0; i < params_.number_extra_iters; i++) { + solver_->update(); + solver_->calculateEstimate(); + } + + return result; +} + +/* ************************************************************************* */ +FactorIndices RISAM::runRobustIteration( + const FactorIndices& convex_factors, + std::map& mu_update_count) { + // Get the current solution + Values current_est = solver_->calculateEstimate(); + + // Update mu for all convex factors and get the convex keys + FastList convex_keys; + for (FactorIndex fidx : convex_factors) { + updateConvexFactorMu(current_est, fidx, mu_update_count, convex_keys); + } + + // Run the Update, re-eliminating the subproblem defined at this time-step + ISAM2UpdateParams params_internal; + params_internal.constrainedKeys = FastMap(); + params_internal.extraReelimKeys = convex_keys; + solver_->update({}, {}, params_internal); + solver_->calculateEstimate(); + + // Update set of convex Factors + return updateConvexFactors(convex_factors); +} + +/* ************************************************************************* */ +void RISAM::updateConvexFactorMu(const Values& current_est, const size_t fidx, + std::map& mu_update_count, + FastList& convex_keys) { + // Invariant: fidx referrs to a GraduatedFactor + auto grad_factor = + std::dynamic_pointer_cast(factors_.at(fidx)); + // Update the mu value using the factor's scheduler + const double residual = grad_factor->residual(current_est); + *(mu_[fidx]) = grad_factor->scheduler()->updateMu(*(mu_[fidx]), residual, + mu_update_count[fidx]); + // Aggregate all convex keys + convex_keys.insert(convex_keys.end(), factors_.at(fidx)->begin(), + factors_.at(fidx)->end()); + mu_update_count[fidx]++; +} + +/* ************************************************************************* */ +FactorIndices RISAM::updateConvexFactors( + const FactorIndices& convex_factors) const { + FactorIndices new_convex_factors; + // For all previously convex factors check \mu convergence + for (FactorIndex fidx : convex_factors) { + auto grad_factor = + std::dynamic_pointer_cast(factors_.at(fidx)); + if (!grad_factor->scheduler()->isMuConverged(*(mu_.at(fidx)))) { + new_convex_factors.push_back(fidx); + } + } + return new_convex_factors; +} + +/* ************************************************************************* */ +FactorIndices RISAM::convexifyInvolvedFactors( + const NonlinearFactorGraph& new_factors, const Values& new_theta, + const std::optional>& extra_gnc_involved_keys, + ISAM2UpdateParams& update_params, UpdateResult& update_result) { + // Gather all involved keys - Directly induced by the new factors + KeySet involved_keys = accumulateInvolvedKeys( + new_factors, extra_gnc_involved_keys, update_params); + + // Gather all affected keys - Super set of involved keys including keys + // modified by user params + auto [affected_keys, is_batch_update] = + solver_->predictUpdateInfo(new_factors, new_theta, update_params); + + // Convexify update-involved factors + std::set convex_factors; + for (Key affected_key : affected_keys) { + for (FactorIndex fidx : variable_index_[affected_key]) { + convexifyFactorIfInvolved(fidx, involved_keys, affected_keys, + is_batch_update, convex_factors); + } + } + + // Fill out the Update Result + update_result.involved_variables = involved_keys; + update_result.convexified_factors = convex_factors; + update_result.affected_variables = affected_keys; + + // Return + return FactorIndices(convex_factors.begin(), convex_factors.end()); +} + +/* ************************************************************************* */ +KeySet RISAM::accumulateInvolvedKeys( + const NonlinearFactorGraph& new_factors, + const std::optional>& extra_gnc_involved_keys, + ISAM2UpdateParams& update_params) const { + // Gather all involved keys - Directly induced by the new factors + KeySet new_factor_keys = new_factors.keys(); + KeySet involved_keys = solver_->collectAffectedKeys(new_factors.keyVector()); + involved_keys.insert(new_factor_keys.begin(), new_factor_keys.end()); + + // Add to the gathered involved keys, any keys specified by the user + if (extra_gnc_involved_keys) { + involved_keys.insert(extra_gnc_involved_keys->begin(), + extra_gnc_involved_keys->end()); + // For any user specified involved keys also add them to the extra-reelim + if (!update_params.extraReelimKeys) { + // Create container if it does not exist + update_params.extraReelimKeys = FastList(); + } + update_params.extraReelimKeys->insert(update_params.extraReelimKeys->end(), + extra_gnc_involved_keys->begin(), + extra_gnc_involved_keys->end()); + } + return involved_keys; +} + +/* ************************************************************************* */ +void RISAM::convexifyFactorIfInvolved(const FactorIndex fidx, + const KeySet& involved_keys, + const KeySet& affected_keys, + const bool is_batch_update, + std::set& convex_factors) { + auto grad_factor = + std::dynamic_pointer_cast(factors_.at(fidx)); + if (grad_factor) { + // Indicates that all variables in this factor are in the affected set + bool inside = true; + // Indicates a factor touches at least one variable in the involved set + bool update_involved = false; + // Compute inside and update involved + for (Key factor_key : factors_.at(fidx)->keys()) { + inside = inside && affected_keys.count(factor_key); + update_involved = update_involved || involved_keys.count(factor_key); + } + + // If the factor is update involved and marked as inside, we need to + // convexify Note: everything is inside on a batch update + if ((inside || is_batch_update) && update_involved) { + convex_factors.insert(fidx); + factors_to_check_status_.insert(fidx); + *(mu_[fidx]) = *(mu_inits_[fidx]); + } + } +} + +/* ************************************************************************* */ +void RISAM::updateHouseKeeping(const NonlinearFactorGraph& new_factors, + const ISAM2UpdateParams& update_params) { + // To match the behavior of iSAM2 and ensure matching indices we first add + // info for new factors then remove info + + // Add the factors and get their new indices + FactorIndices new_factor_indices = factors_.add_factors( + new_factors, params_.isam2_params.findUnusedFactorSlots); + // Update the variable index for the new factors + variable_index_.augment(new_factors, new_factor_indices); + // Add a mu and mu_init entry for each factor + augmentMu(new_factors, new_factor_indices); + + // Once we add new factors we can remove any factors and corresponding info + NonlinearFactorGraph removed_factors; + removed_factors.reserve(update_params.removeFactorIndices.size()); + for (const auto fidx : update_params.removeFactorIndices) { + removed_factors.push_back(factors_.at(fidx)); + factors_.remove(fidx); + factors_to_check_status_.erase(fidx); + mu_.at(fidx).reset(); + mu_inits_.at(fidx).reset(); + } + variable_index_.remove(update_params.removeFactorIndices.begin(), + update_params.removeFactorIndices.end(), + removed_factors); +} + +/* ************************************************************************* */ +void RISAM::augmentMu(const NonlinearFactorGraph& new_factors, + const FactorIndices& new_factor_indices) { + for (size_t i = 0; i < new_factors.nrFactors(); i++) { + FactorIndex fidx = new_factor_indices[i]; + auto grad_factor = + std::dynamic_pointer_cast(new_factors.at(i)); + + // Get the pointers to mu and mu_init for this factor + std::shared_ptr mu, mu_init; + if (grad_factor) { + mu = grad_factor->mu_; + mu_init = std::make_shared(grad_factor->scheduler()->muInit()); + } else { + mu = std::make_shared(0); + mu_init = std::make_shared(0); + } + + // Extend or replace mu and mu_inits + if (fidx >= mu_.size()) { + mu_.push_back(mu); + mu_inits_.push_back(mu_init); + } else { + mu_[fidx] = mu; + mu_inits_[fidx] = mu_init; + } + } +} + +/* ************************************************************************* */ +void RISAM::incrementMuInits() { + // Compute Average Delta + VectorValues delta = solver_->getDelta(); + bool is_sufficient_delta = + (delta.size() > 0) && (delta.norm() / delta.size() < + params_.outlier_mu_avg_var_convergence_thresh); + + // Evaluate All Graduated factors if sufficient average delta + if (is_sufficient_delta) { + Values theta = solver_->calculateEstimate(); + for (auto fidx : factors_to_check_status_) { + auto grad_factor = + std::dynamic_pointer_cast(factors_[fidx]); + + double mahdist = grad_factor->residual(theta); + const double mah_upper_bound = internal::chiSquaredQuantile( + factors_[fidx]->dim(), params_.outlier_mu_chisq_upper_bound); + const double mah_lower_bound = internal::chiSquaredQuantile( + factors_[fidx]->dim(), params_.outlier_mu_chisq_lower_bound); + + if (mahdist > mah_upper_bound) { + *(mu_inits_[fidx]) = + grad_factor->scheduler()->updateMuInit(*(mu_inits_[fidx]), false); + } else if (mahdist < mah_lower_bound) { + *(mu_inits_[fidx]) = + grad_factor->scheduler()->updateMuInit(*(mu_inits_[fidx]), true); + } + } + // Reset accumulator + factors_to_check_status_.clear(); + } +} + +} // namespace gtsam diff --git a/gtsam/sam/RISAM.h b/gtsam/sam/RISAM.h new file mode 100644 index 0000000000..9ae5692e2d --- /dev/null +++ b/gtsam/sam/RISAM.h @@ -0,0 +1,314 @@ +/* ---------------------------------------------------------------------------- + + * 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 RISAM.h + * @brief Robust Incremental Smoothing and Mapping (riSAM) + * @author Dan McGann + * @date October 2025 + */ +#pragma once +#include +#include +#include + +namespace gtsam { + +/** @brief Robust Incremental Smoothing and Mapping (riSAM) is a robust variant + * of iSAM2 for incremental factor-graph optimization. riSAM solves each + * incremental update using an efficient form of Graduated Non-Convexity to + * reject outliers while maintaining robustness to initialization. + * + * Citation: + * Robust Incremental Smoothing and Mapping (riSAM) + * D. McGann and J.G. Rogers III and M. Kaess, + * 2023, Proc. IEEE Intl. Conf. on Robotics and Automation (ICRA) + */ +class GTSAM_EXPORT RISAM { + /// @name Types + /// @{ + public: + /** @brief Struct Containing all configuration parameters for riSAM. + * See below for details on each parameters. + */ + struct Parameters { + /// @brief Explicit constructor to use default values. + Parameters(ISAM2Params isam2_params = ISAM2Params(), + bool increment_outlier_mu = true, + double outlier_mu_chisq_upper_bound = 0.95, + double outlier_mu_chisq_lower_bound = 0.25, + double outlier_mu_avg_var_convergence_thresh = 0.01, + size_t number_extra_iters = 1) + : isam2_params(isam2_params), + increment_outlier_mu(increment_outlier_mu), + outlier_mu_chisq_upper_bound(outlier_mu_chisq_upper_bound), + outlier_mu_chisq_lower_bound(outlier_mu_chisq_lower_bound), + outlier_mu_avg_var_convergence_thresh( + outlier_mu_avg_var_convergence_thresh), + number_extra_iters(number_extra_iters) {}; + + /// @brief The parameters for the encapsulated iSAM2 algorithm @Note some + /// are overriden in RISAM::RISAM(). + ISAM2Params isam2_params; + + /// @brief Flag to increment mu_init when the value estimate converges. + // See RISAM::IncrementMuInit for details. + bool increment_outlier_mu; + /// @brief Increment mu_init if chi^2 > upper bound. + double outlier_mu_chisq_upper_bound; + /// @brief Decrement mu_init if chi^2 < lower bound. + double outlier_mu_chisq_lower_bound; + /// @brief Average variable delta threshold to identify value convergence. + double outlier_mu_avg_var_convergence_thresh; + /// @brief The number of extra iterations to perform after mu convergence. + size_t number_extra_iters; + }; + + /** @brief Struct containing information about the riSAM update. + * See below for details about the information included. + */ + struct UpdateResult { + /// @brief The iSAM2 result from the first internal update. + /// NOTE: riSAM may run multiple internal ISAM2 updates. + ISAM2Result isam2_result; + + /// @brief The set variables directly involved in the update. + std::set involved_variables; + /// @brief The set of variables affected by the update. + std::set affected_variables; + /// @brief The set of factors convexified in this update. + std::set convexified_factors; + }; + /// @} + + /// @name Fields + /// @{ + protected: + /// @brief Configuration parameters for the riSAM algorithm. + Parameters params_; + /// @brief The encapsulated iSAM2 algorithm. + std::unique_ptr solver_; + /// @brief The current control parameter values for all factors. If factor i + /// is a GraduatedFactor mu_[i] is a reference to that factors internal state. + /// If factor i is not Graduated mu_[i] is a pointer to a value of zero. + FastVector> mu_; + /// @brief The current initial control parameter values for all factors. If + /// factor i is not Graduated mu_[i] is a pointer to a value of zero. + FastVector> mu_inits_; + /// @brief The set of GraduatedFactors that have been convexified since the + /// last mu_init_ increment These are the factors that we can increment mu's + /// for the next time variables converge. + FastSet factors_to_check_status_; + + /** + * RISAM maintains its own variable index and factors. These match exactly + * that of the underlying iSAM2 solver, but lead ahead of it since we update + * RISAM housekeeping before updating the underlying solver. + */ + /// @brief VariableIndex for the underlying system. + /// INVARIANT: matches that of solver_ after every update. + VariableIndex variable_index_; + /// @brief The Factors for the underlying system. + /// INVARIANT: matches that of solver_ after every update. + NonlinearFactorGraph factors_; + /// @} + + /// @name Public Interface + /// @{ + public: + /** @brief Constructs an instance of the riSAM algorithm with provided + * configuration. Note this constructor will override some values in + * params.isam2_params. + * @param params: The configuration parameters for this instance of riSAM. + */ + RISAM(const Parameters& params) : params_(params) { + /** Override user preferences for iSAM2 params + * To ensure that we update the current estimated theta_ after each + * intermediate GNC iteration we must use relinearizeSkip = 1 so that we + * allow relinearization at any step it is needed. + * + * For each intermediate GNC iteration we update mu_ values for convex + * GraduatedFactors to ensure that GraduatedFactors are linearized according + * to these mu_ values we must turn off caching that could result in old mu_ + * being used. + */ + // We must have relinearization skip as 1. + params_.isam2_params.relinearizeSkip = 1; + // We must not use cached factors. + params_.isam2_params.cacheLinearizedFactors = false; + + // Construct the encapsulated solver using the modified parameters. + solver_ = std::make_unique(params_.isam2_params); + } + + /// @brief Update Interface. See ISAM2 docs for details as parameters match + /// (almost) exactly. + /// @param extra_gnc_involved_keys - overrides internal RISAM logic and + /// performs a robust update. All extra involved keys will be treated as being + /// a part of the current update with respect to factor convexification. + UpdateResult update( + const NonlinearFactorGraph& new_factors = NonlinearFactorGraph(), + const Values& new_theta = Values(), + const std::optional> extra_gnc_involved_keys = std::nullopt, + const FactorIndices& remove_factor_indices = FactorIndices(), + const std::optional>& constrained_keys = std::nullopt, + const std::optional>& no_relin_keys = std::nullopt, + const std::optional>& extra_reelim_keys = std::nullopt, + bool force_relinearize = false); + + /// @brief Update Interface. See ISAM2 docs for details as parameters match + /// (almost) exactly. + /// @param extra_gnc_involved_keys - overrides internal RISAM logic and + /// performs a robust update. All extra involved keys will be treated as being + /// a part of the current update with respect to factor convexification. + UpdateResult update( + const NonlinearFactorGraph& new_factors, const Values& new_theta, + const std::optional> extra_gnc_involved_keys, + const ISAM2UpdateParams& update_params); + + /// @brief Returns the current estimate from the solver. + Values calculateEstimate(); + + /// @brief Returns the underlying factors of the system. + NonlinearFactorGraph getFactorsUnsafe() { return factors_; } + + /** @brief Returns the set of measurements (identified by their factor index) + * that have been deemed outliers. riSAM defines a measurement as an outlier + * if its current residual is greater than the chi2_threshold provided. + * @param chi2_outlier_thresh - The chi2 threshold used to define outliers. + * WARN: Potentially slow since we iterate over all factors. + * @returns The set of factors that are considered outliers by RISAM. + */ + std::set getOutliers(double chi2_outlier_thresh); + /// @} + + /// @name Private Interface + /// @{ + protected: + /** @brief Preforms a robust update to the system. + * A robust update is required any time new_factors contains GraduatedFactors + * (i.e. there are potentially new inlier/outlier measurements). + * @see RISAM::update for details on params. + */ + UpdateResult updateRobust( + const NonlinearFactorGraph& new_factors, const Values& new_theta, + const std::optional>& extra_gnc_involved_keys, + const ISAM2UpdateParams& update_params); + + /** @brief Performs a inner loop iteration of a robust update. + * @param convex_factors: The set of factors that are convex for this update. + * @param mu_update_count: Accum. for # of updates applied to convex factors. + * @returns The set of factors that remain convex after this iteration. + */ + FactorIndices runRobustIteration( + const FactorIndices& convex_factors, + std::map& mu_update_count); + + /** @brief Compute the new value of $\mu$ for a given graduated factor. + * @param current_est: The current estimated solution. + * @param fidx: The index of the graduated factor to update. + * @param mu_update_count: Accum. for # of updates applied to convex factors. + * @param convex_keys: Accum. for all keys associated with convex factors. + * @note INVARIANT: fidx must index a Graduated Factor. + */ + void updateConvexFactorMu(const Values& current_est, const size_t fidx, + std::map& mu_update_count, + FastList& convex_keys); + + /** @brief Compute the new set of convex factors after a robust iteration. + * @brief convex_factors: The set of convex factors after the prev. iter. + * @return The set of factors that are still convex after the current iter. + */ + FactorIndices updateConvexFactors(const FactorIndices& convex_factors) const; + + /** @brief Convexifies factors involved in the update + * Finds all factors inside the total affected set, and involved in the update + * defined by new factors. Marks those factors as convex and resets mu_ to the + * factors current mu_init_. See RISAM::update for adtl. parameter details. + * @param update_result: structure containing information about this update, + * modified by this function to fill in info about involved and affected + * variables as well as convexified factors. + * @returns The indicies of all factors convexified in the update defined by + * the given parameters. + */ + FactorIndices convexifyInvolvedFactors( + const NonlinearFactorGraph& new_factors, const Values& new_theta, + const std::optional>& extra_gnc_involved_keys, + ISAM2UpdateParams& internal_update_params, UpdateResult& update_result); + + /** @brief Accumulates the keys of all vars directly invovled in an update. + * @param new_factors The new factors for hte update. + * @param extra_gnc_involved_keys: User supplied involved keys. + * @param update_params: The parameters for the underlying iSAM2 update. + * @returns The set of involved factors. + */ + KeySet accumulateInvolvedKeys( + const NonlinearFactorGraph& new_factors, + const std::optional>& extra_gnc_involved_keys, + ISAM2UpdateParams& update_params) const; + + /** @brief Convexifies a single factor if it is involved with the update. + * @param fidx: The factor to convexify if involved. + * @param involved_keys: The set of keys involved with the update. + * @param affected_keys: The ser of keys affected by the update. + * @param is_batch_update: Flag indicating all factors are relinearized. + * @param convex_factors: Accumulator for the set of all convex factors. + */ + void convexifyFactorIfInvolved(const FactorIndex fidx, + const KeySet& involved_keys, + const KeySet& affected_keys, + const bool is_batch_update, + std::set& convex_factors); + + /** @brief Update housekeeping information for riSAM this involves: + * 1. Determining the indicies for new_factors. + * 2. Update the riSAM fields: factors_ and variable_index_. + * NOTE: they will be ahead of those in solver_ until solver.update() is + * called. + * 3. Update mu_ and mu_init_ for the new factors. + * @param new_factors: The new factors for the current update. + * @param update_params: The update parameters for the current update. + */ + void updateHouseKeeping(const NonlinearFactorGraph& new_factors, + const ISAM2UpdateParams& update_params); + + /** @brief Extend or update mu and mu_init for new factors. + * @param new_factors: The new factors for which to extend the containers. + * @param new_factor_indicies: the indicies assigned to each new_factor. + */ + void augmentMu(const NonlinearFactorGraph& new_factors, + const FactorIndices& new_factor_indices); + + /** @brief Increments mu_inits_ for any factor recently convexified. + * Used to mitigate the long-term affect of measurements that we are confident + * are outliers. + */ + void incrementMuInits(); + /// @} + + /// @name Static Helpers + /// @{ + public: + /** @brief Constructs a shared_ptr of FACTOR_TYPE graduated factor for riSAM + * This identifies a factor as a potential outlier measurement. + * @param kernel: The graduated kernel to use for this factor + * @param args: The arguments required to construct the FACTOR_TYPE + */ + template + static typename GenericGraduatedFactor::shared_ptr + make_graduated(Args&&... args) { + return std::make_shared>( + std::forward(args)...); + } + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/sam/RISAMGraduatedFactor.h b/gtsam/sam/RISAMGraduatedFactor.h new file mode 100644 index 0000000000..889eab53e3 --- /dev/null +++ b/gtsam/sam/RISAMGraduatedFactor.h @@ -0,0 +1,204 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** @brief Factor that implements a graduated robust cost function. + * A Graduated factor overrides a portion of the NoiseModelFactor interface to + * implement a graduated robust cost function for the factor. + * + * @author Dan McGann + * @date Mar 2022 + */ +#pragma once +#include +#include + +namespace gtsam { + +/// @brief Graduated Factor for riSAM base class +class GraduatedFactor { + /// @name Types + /// @{ + public: + typedef std::shared_ptr shared_ptr; + typedef noiseModel::mEstimator::Base RobustLoss; + /// @} + + /// @name Fields + /// @{ + protected: + /// @brief The robust loss for this factor + RobustLoss::shared_ptr robust_loss_; + /// @brief The control param ($\mu$) scheduler for this factor + GraduationScheduler::shared_ptr scheduler_; + + /// @brief The unique mu control parameter for this factor + std::shared_ptr mu_; + /// @} + + /// Befriend RISAM to give access to these protected values + friend class RISAM; + + /// @name Public Interface + /// @{ + public: + /** @brief Constructor + * @param args: The arguments required to construct the FACTOR_TYPE + * @param loss: The graduated robust loss applied to this factor + * @param scheduler: The control param $\mu$ scheduler for this factor + */ + GraduatedFactor(RobustLoss::shared_ptr loss, + GraduationScheduler::shared_ptr scheduler) + : robust_loss_(loss), scheduler_(scheduler) { + mu_ = std::make_shared(scheduler_->muInit()); + } + + /// @brief Copy constructor + GraduatedFactor(const GraduatedFactor& other) + : robust_loss_(other.robust_loss_), scheduler_(other.scheduler_) { + mu_ = std::make_shared(*(other.mu_)); + } + + /** @brief Linearize this factor using the convexification parameter mu + * @param current_estimate: the estimate at which to linearize the factor + * @param mu: the current value of the convexification parameter + */ + virtual GaussianFactor::shared_ptr linearizeGraduated( + const Values& current_estimate) const = 0; + + /// @brief returns the residual of the factor + virtual double residual(const Values& current_estimate) const = 0; + + /// @brief returns \rho(r) of the factor + virtual double robustResidual(const Values& current_estimate) const = 0; + + /// @brief Returns the robust loss for this graduated factor + const RobustLoss::shared_ptr loss() const { return robust_loss_; } + + /// @brief Returns the graduation scheduler for this factor + const GraduationScheduler::shared_ptr scheduler() const { return scheduler_; } + + /// @brief Copies this factor as an instance of its base type without the + /// graduated robust loss or scheduler + virtual NonlinearFactor::shared_ptr cloneUngraduated() const = 0; + /// @} +}; + +/// @brief Instantiation of Graduated Factor wrapping any Nonlinear Factor +template +class GenericGraduatedFactor : public FACTOR_TYPE, public GraduatedFactor { + static_assert(std::is_base_of::value, + "GraduatedFactor Must be instantiated with a Factor Derived " + "from NonlinearFactor."); + + /// @name Types + /// @{ + public: + typedef std::shared_ptr> shared_ptr; + /// @} + + /// @name Factor Interface + /// @{ + public: + /** @brief Constructor + * @param args: The arguments required to construct the FACTOR_TYPE + * @param loss: The graduated robust loss applied to this factor + * @param scheduler: The control param $\mu$ scheduler for this factor + */ + template + GenericGraduatedFactor(RobustLoss::shared_ptr loss, + GraduationScheduler::shared_ptr scheduler, + Args&&... args) + : FACTOR_TYPE(std::forward(args)...), + GraduatedFactor(loss, scheduler) {} + + /// @brief Copy Constructor + GenericGraduatedFactor(const GenericGraduatedFactor& other) + : FACTOR_TYPE(other), + GraduatedFactor(other.robust_loss_, other.scheduler_) {} + + /// @brief Makes a deep copy of the factor + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr( + new GenericGraduatedFactor(*this))); + } + + /// @brief Linearizes the factor using the current value of mu + GaussianFactor::shared_ptr linearizeGraduated( + const Values& current_estimate) const override { + double residual = this->residual(current_estimate); + + // Use base factor to linearize + Matrix A; + Vector b; + auto whitened_linear_system = FACTOR_TYPE::linearize(current_estimate); + std::tie(A, b) = whitened_linear_system->jacobian(); + size_t output_dim = b.size(); + + // Extract the non-dense linear system + auto keys = whitened_linear_system->keys(); + std::vector Ablocks; + size_t idx_start = 0; + for (const auto& key : keys) { + size_t d = current_estimate.at(key).dim(); + Matrix vblock = Matrix::Zero(output_dim, d); + Ablocks.push_back(A.block(0, idx_start, output_dim, d)); + idx_start += d; + } + + // Weight the Linearized Blocks + double sqrt_weight = sqrt(robust_loss_->graduatedWeight(residual, *mu_)); + for (Matrix& Aj : Ablocks) { + Aj *= sqrt_weight; + } + b *= sqrt_weight; + + // Construct a jacobian factor from the weighted system + FastMap Ablock_map; + for (size_t i = 0; i < Ablocks.size(); i++) { + Ablock_map[keys[i]] = Ablocks[i]; + } + return std::make_shared(Ablock_map, b); + } + + /// @brief Linearize the System @see NonlinearFactor::linearize + GaussianFactor::shared_ptr linearize( + const Values& current_estimate) const override { + // Delegate to linearizeGraduated which is required by the GraduatedFactor + // Interface + return linearizeGraduated(current_estimate); + } + + /// @brief Returns 0.5 \rho(r)^2 + double error(const Values& values) const override { + return 0.5 * std::pow(robustResidual(values), 2); + } + /// @} + + /// @name Graduated Interface + /// @{ + /// @brief See GraduatedFactor::residual + double residual(const Values& current_estimate) const override { + return sqrt(2.0 * FACTOR_TYPE::error(current_estimate)); + } + + /// @brief See GraduatedFactor::robustResidual + double robustResidual(const Values& current_estimate) const override { + return robust_loss_->graduatedLoss(residual(current_estimate), *mu_); + } + + /// @brief See GraduatedFactor::cloneUngraduated + NonlinearFactor::shared_ptr cloneUngraduated() const override { + return FACTOR_TYPE::clone(); + } + /// @} +}; +} // namespace gtsam diff --git a/gtsam/sam/RISAMGraduationScheduler.cpp b/gtsam/sam/RISAMGraduationScheduler.cpp new file mode 100644 index 0000000000..4adc1a039b --- /dev/null +++ b/gtsam/sam/RISAMGraduationScheduler.cpp @@ -0,0 +1,64 @@ + +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +double SIGScheduler::updateMu(const double& mu, const double& residual, + const size_t& update_count) const { + return this->mu_update_strat_(mu, residual, update_count); +} + +/* ************************************************************************* */ +double SIGScheduler::updateMuInit(const double& mu, + const bool is_inlier) const { + double new_mu = mu + (is_inlier ? -mu_init_increment_ : mu_init_increment_); + return std::clamp(new_mu, mu_init_, convergence_thresh_); +} + +/* ************************************************************************* */ +bool SIGScheduler::isMuConverged(const double& mu) const { + return mu >= convergence_thresh_; +} + +/* ************************************************************************* */ +double SIGScheduler::muUpdateMcGann2023(const double& mu, + const double& residual, + const size_t& update_count) { + return std::min(1.0, mu + (mu + 0.1) * 1.2); +} + +/* ************************************************************************* */ +double SIGScheduler::muUpdateStable(const double& mu, const double& residual, + const size_t& update_count) { + // Conditional Arithmetic Series: mu[t+1] = mu[t] + 0.5 if (t==0), mu[t] + + // 0.4 if (t==1), else mu[t] + 0.05 + // Authors attempt to build the most stable method possible from years of + // using the algorithm. + if (update_count == 0) return std::min(1.0, mu + 0.5); + if (update_count == 1) return std::min(1.0, mu + 0.4); + return std::min(1.0, mu + 0.05); +} + +/* ************************************************************************* */ +double ScaledScheduler::updateMu(const double& mu, const double& residual, + const size_t& update_count) const { + return std::clamp(mu_scale_ * mu, convergence_thresh_, mu_init_); +} + +/* ************************************************************************* */ +double ScaledScheduler::updateMuInit(const double& mu_init, + const bool is_inlier) const { + double new_mu_init = + mu_init * (is_inlier ? 1.0 / mu_init_scale_ : mu_init_scale_); + return std::clamp(new_mu_init, convergence_thresh_, mu_init_); +} + +/* ************************************************************************* */ +bool ScaledScheduler::isMuConverged(const double& mu) const { + return mu <= convergence_thresh_; +} + +} // namespace gtsam diff --git a/gtsam/sam/RISAMGraduationScheduler.h b/gtsam/sam/RISAMGraduationScheduler.h new file mode 100644 index 0000000000..c4d4b3d17d --- /dev/null +++ b/gtsam/sam/RISAMGraduationScheduler.h @@ -0,0 +1,175 @@ +/** @brief Interface and implementations for GraduationScheduler + * These are used to define the sequence of problems solved by RISAM. + * The scheduler defines these problems using the the control parameter $\mu$ + * for a specific Graduated Robust Loss Function. + * @author Dan McGann + * @date Mar 2022 + */ +#pragma once +#include +#include +#include + +#include +#include + +namespace gtsam { + +/** @brief Base class for graduation scheduling for riSAM + * Advanced users can write their own schedulers by inheriting from this class + */ +class GraduationScheduler { + /// @name Types + /// @{ + public: + typedef std::shared_ptr shared_ptr; + /// @} + + /// @name Fields + /// @{ + protected: + /// @brief The initial value for mu $\mu_{init}$ + const double mu_init_; + /// @brief The threshold at which to consider mu to be converged + const double convergence_thresh_; + /// @} + + /// @name Public Interface + /// @{ + public: + GraduationScheduler(double mu_init, double convergence_thresh) + : mu_init_(mu_init), convergence_thresh_(convergence_thresh) {} + + /// @brief Returns the value of $\mu_{init}$ for this graduated + double muInit() const { return mu_init_; } + + /** @brief Returns the next value of $\mu$ for this graduated + * @param mu: The current value of $\mu$ + * @param residual: The current residual of the factor + * @param update_count: The number of mu updates during this solve + */ + virtual double updateMu(const double& mu, const double& residual, + const size_t& update_count) const = 0; + + /** @brief Returns the next value of $\mu_{init}$ for strong inliers/outliers + * - Inliers are updated to have more convex $\mu_{init}$ + * - Outliers are update to have less convex $\mu_{init}$ + * @param mu_init: The current value of $\mu_{init}$ + * @param is_inlier: Flag indicating inlier update otherwise an outlier update + */ + virtual double updateMuInit(const double& mu_init, + const bool is_inlier) const = 0; + + /// @brief Returns true iff the value of $\mu$ has converged + virtual bool isMuConverged(const double& mu) const = 0; + /// @} +}; + +/* ************************************************************************* */ +/** @brief Scheduler for Geman McClure Loss with Scale Invariant Graduation + */ +class GTSAM_EXPORT SIGScheduler : public GraduationScheduler { + /// @name Types + /// @{ + public: + /// @brief Shortcut for shared pointer + typedef std::shared_ptr shared_ptr; + /// @brief Function type for $\mu$ update sequence + typedef std::function MuUpdateStrategy; + /// @} + + /// @name Fields + /// @{ + private: + /// @brief The update strategy for the sequence $\mu$ values + MuUpdateStrategy mu_update_strat_; + /// @brief The amount to increment/decrement $\mu_{init}$ if the factor is a + /// strong inlier/outlier when values converge + double mu_init_increment_; + /// @} + + /// @name Public Interface + /// @{ + public: + /** @brief Individual Parameter Constructor + * @param mu_update_strat: The update strategy to use for $\mu$ updates + * Recommend: muUpdateStable, Alt: muUpdateMcGann2023 or Custom + * @param mu_init_increment: The amount to increment/decrement $\mu_{init}$ + */ + SIGScheduler(MuUpdateStrategy mu_update_strat = muUpdateStable, + double mu_init_increment = 0.2) + : GraduationScheduler(0.0, 1.0), + mu_update_strat_(mu_update_strat), + mu_init_increment_(mu_init_increment) {} + + /// @brief @see GraduationScheduler + double updateMu(const double& mu, const double& residual, + const size_t& update_count) const override; + /// @brief @see GraduationScheduler + double updateMuInit(const double& mu_init, + const bool is_inlier) const override; + /// @brief @see GraduationScheduler + bool isMuConverged(const double& mu) const override; + /// @} + + /// @name Default $\mu$ Update Strategies + /// @{ + public: + /// @brief $\mu$ update sequence empirically discovered and presented in the + /// orig riSAM paper + static double muUpdateMcGann2023(const double& mu, const double& residual, + const size_t& update_count); + /// @brief More stable $\mu$ Update sequence developed empirically since + /// algorithm was published + static double muUpdateStable(const double& mu, const double& residual, + const size_t& update_count); + /// @} +}; + +/* ************************************************************************* */ +/** @brief Scheduler losses that use scaled shape parameters for graduation. + * Examples include: Cauchy, TLS, Tukey + * These losses are Convex at \mu=\infty and Robust at \mu=1.0 + */ +class GTSAM_EXPORT ScaledScheduler : public GraduationScheduler { + /// @name Types + /// @{ + public: + /// @brief Shortcut for shared pointer + typedef std::shared_ptr shared_ptr; + /// @} + + /// @name Fields + /// @{ + private: + /// @brief The scale factor to update $\mu$ values + double mu_scale_; + /// @brief The scale factor to update $\mu_{init}$ values + double mu_init_scale_; + /// @} + + /// @name Public Interface + /// @{ + public: + /** @brief Individual Parameter Constructor + * @param mu_init: The initial value for mu + * @param mu_scale: The scale factor for $\mu$ updates + * @param mu_init_increment: The amount to increment/decrement $\mu_{init}$ + */ + ScaledScheduler(double mu_init = 1e4, double mu_scale = 0.715, + double mu_init_scale = 0.9) + : GraduationScheduler(mu_init, 1.0), + mu_scale_(mu_scale), + mu_init_scale_(mu_init_scale) {} + + /// @brief @see GraduationScheduler + double updateMu(const double& mu, const double& residual, + const size_t& update_count) const override; + /// @brief @see GraduationScheduler + double updateMuInit(const double& mu_init, + const bool is_inlier) const override; + /// @brief @see GraduationScheduler + bool isMuConverged(const double& mu) const override; + /// @} +}; +} // namespace gtsam diff --git a/gtsam/sam/tests/testRISAM.cpp b/gtsam/sam/tests/testRISAM.cpp new file mode 100644 index 0000000000..fd0c66f494 --- /dev/null +++ b/gtsam/sam/tests/testRISAM.cpp @@ -0,0 +1,287 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRISAM.cpp + * @brief Unit tests for RISAM and supporting classes + * @author Dan McGann + * @date January 2026 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +typedef noiseModel::mEstimator::GemanMcClure GMLoss; + +/* ************************************************************************* */ +TEST(SIGScheduler, UpdateMuInit) { + SIGScheduler scheduler(SIGScheduler::muUpdateStable, 0.1); + CHECK(assert_equal(0.1, scheduler.updateMuInit(0.0, false), 1e-9)); + CHECK(assert_equal(1.0, scheduler.updateMuInit(1.0, false), 1e-9)); + + CHECK(assert_equal(0.0, scheduler.updateMuInit(0.0, true), 1e-9)); + CHECK(assert_equal(0.9, scheduler.updateMuInit(1.0, true), 1e-9)); +} + +/* ************************************************************************* */ +TEST(SIGScheduler, IsMuConverged) { + SIGScheduler scheduler(SIGScheduler::muUpdateStable, 0.1); + CHECK(!scheduler.isMuConverged(0.5)); + CHECK(scheduler.isMuConverged(1.0)); +} + +/* ************************************************************************* */ +TEST(SIGScheduler, MuUpdateMcGann2023) { + // Standard Sequence + CHECK(assert_equal(0.12, SIGScheduler::muUpdateMcGann2023(0.0, 0, 0), 1e-9)); + CHECK( + assert_equal(0.384, SIGScheduler::muUpdateMcGann2023(0.12, 0, 1), 1e-9)); + CHECK(assert_equal(0.9648, SIGScheduler::muUpdateMcGann2023(0.384, 0, 2), + 1e-9)); + CHECK( + assert_equal(1.0, SIGScheduler::muUpdateMcGann2023(0.9648, 0, 3), 1e-9)); +} + +/* ************************************************************************* */ +TEST(SIGScheduler, MuUpdateStable) { + // Standard Sequence + CHECK(assert_equal(0.5, SIGScheduler::muUpdateStable(0.0, 0.0, 0), 1e-9)); + CHECK(assert_equal(0.9, SIGScheduler::muUpdateStable(0.5, 0.0, 1), 1e-9)); + CHECK(assert_equal(0.95, SIGScheduler::muUpdateStable(0.9, 0.0, 2), 1e-9)); + CHECK(assert_equal(1.0, SIGScheduler::muUpdateStable(0.95, 0.0, 3), 1e-9)); + + // Sequence with Non-Zero mu_init + CHECK(assert_equal(0.7, SIGScheduler::muUpdateStable(0.2, 0.0, 0), 1e-9)); + CHECK(assert_equal(1.0, SIGScheduler::muUpdateStable(0.7, 0.0, 1), 1e-9)); + + // Sequence with Large Non-Zero mu_init + CHECK(assert_equal(1.0, SIGScheduler::muUpdateStable(0.7, 0.0, 0), 1e-9)); +} + +/* ************************************************************************* */ +TEST(ScaledScheduler, UpdateMu) { + ScaledScheduler scheduler(1e4, 0.715, 0.9); + // Standard Sequence + CHECK(assert_equal(71.5, scheduler.updateMu(100.0, 0.0, 0), 1e-9)); + CHECK(assert_equal(1.0, scheduler.updateMu(1.0, 0.0, 0), 1e-9)); +} + +/* ************************************************************************* */ +TEST(ScaledScheduler, UpdateMuInit) { + ScaledScheduler scheduler(1e4, 0.715, 0.9); + CHECK(assert_equal(90.0, scheduler.updateMuInit(100, false), 1e-9)); + CHECK(assert_equal(1.0, scheduler.updateMuInit(1.0, false), 1e-9)); + + CHECK(assert_equal(1e4, scheduler.updateMuInit(1e4, true), 1e-9)); + CHECK(assert_equal(111.111, scheduler.updateMuInit(100, true), 1e-3)); +} + +/* ************************************************************************* */ +TEST(ScaledScheduler, IsMuConverged) { + ScaledScheduler scheduler(1e4, 0.715, 0.9); + CHECK(!scheduler.isMuConverged(1e5)); + CHECK(!scheduler.isMuConverged(1.2)); + CHECK(scheduler.isMuConverged(1.0)); +} + +/* ************************************************************************* */ +TEST(RISAMGraduatedFactor, Linearize) { + GMLoss::shared_ptr robust_loss = + GMLoss::Create(1.0, GMLoss::GradScheme::SCALE_INVARIANT); + SIGScheduler::shared_ptr scheduler = std::make_shared(); + + NonlinearFactor::shared_ptr factor = + RISAM::make_graduated>(robust_loss, scheduler, 0, 0.0, + noiseModel::Unit::Create(1)); + gtsam::Values values; + + // Linearize at Weight = 0.5 + values.insert(0, 0.0); + GaussianFactor::shared_ptr lin_factor = factor->linearize(values); + auto [A, b] = lin_factor->jacobian(); + CHECK(assert_equal(Matrix::Identity(1, 1) * sqrt(0.5), A, 1e-9)); + CHECK(assert_equal(Vector::Zero(1, 1), b, 1e-9)); +} + +/* ************************************************************************* */ +TEST(RISAMGraduatedFactor, Error) { + GMLoss::shared_ptr robust_loss = + GMLoss::Create(1.0, GMLoss::GradScheme::SCALE_INVARIANT); + SIGScheduler::shared_ptr scheduler = std::make_shared(); + + NonlinearFactor::shared_ptr factor = + RISAM::make_graduated>(robust_loss, scheduler, 0, 0.0, + noiseModel::Unit::Create(1)); + gtsam::Values values; + values.insert(0, 1.0); + CHECK(assert_equal(factor->error(values), 0.03125, 1e-9)); + + GraduatedFactor::shared_ptr grad_factor = + std::dynamic_pointer_cast(factor); + CHECK(assert_equal(grad_factor->residual(values), 1.0, 1e-9)); + CHECK(assert_equal(grad_factor->robustResidual(values), 0.25, 1e-9)); +} + +/* ************************************************************************* */ +TEST(RISAM, RISAMIntegrationTest) { + SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas( + (Vector(3) << 0.1, 0.1, M_PI / 100.0).finished()); + SharedDiagonal brNoise = + noiseModel::Diagonal::Sigmas((Vector(2) << M_PI / 100.0, 0.1).finished()); + + // Setup the Solver + GMLoss::shared_ptr robust_loss = + GMLoss::Create(GMLoss::shapeParamFromInfThresh(0.1, 3, 0.95), + GMLoss::GradScheme::SCALE_INVARIANT); + SIGScheduler::shared_ptr scheduler = std::make_shared(); + + RISAM::Parameters params; + params.isam2_params = ISAM2Params( + ISAM2DoglegLineSearchParams(0.02, 1.0, 1.5, 1e-3, false, 1e-4)); + RISAM risam(params); + + // Setup Container for the full problem + Values fullinit; + NonlinearFactorGraph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update risam + { + NonlinearFactorGraph newfactors; + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); + + risam.update(newfactors, init); + } + + // Add odometry from time 0 to time 5 + for (; i < 5; ++i) { + NonlinearFactorGraph newfactors; + newfactors.emplace_shared>( + i, i + 1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i + 1), Pose2(double(i + 1) + 0.1, -0.1, 0.01)); + fullinit.insert((i + 1), Pose2(double(i + 1) + 0.1, -0.1, 0.01)); + + risam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + NonlinearFactorGraph newfactors; + newfactors.emplace_shared>( + i, i + 1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.push_back( + RISAM::make_graduated>( + robust_loss, scheduler, i, 100, Rot2::fromAngle(M_PI / 2.0), 5.0, + brNoise)); + newfactors.push_back( + RISAM::make_graduated>( + robust_loss, scheduler, i, 101, Rot2::fromAngle(-M_PI / 2.0), 5.0, + brNoise)); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i + 1), Pose2(1.01, 0.01, 0.01)); + init.insert(100, Point2(5.0 / sqrt(2.0), 5.0 / sqrt(2.0))); + init.insert(101, Point2(5.0 / sqrt(2.0), -5.0 / sqrt(2.0))); + fullinit.insert((i + 1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(100, Point2(5.0 / sqrt(2.0), 5.0 / sqrt(2.0))); + fullinit.insert(101, Point2(5.0 / sqrt(2.0), -5.0 / sqrt(2.0))); + + risam.update(newfactors, init); + ++i; + } + + // Add odometry from time 6 to time 10 + for (; i < 10; ++i) { + NonlinearFactorGraph newfactors; + newfactors.emplace_shared>( + i, i + 1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i + 1), Pose2(double(i + 1) + 0.1, -0.1, 0.01)); + fullinit.insert((i + 1), Pose2(double(i + 1) + 0.1, -0.1, 0.01)); + + risam.update(newfactors, init); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + NonlinearFactorGraph newfactors; + newfactors.emplace_shared>( + i, i + 1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.push_back( + RISAM::make_graduated>( + robust_loss, scheduler, i, 100, Rot2::fromAngle((3 * M_PI) / 4.0), + 7.07106, brNoise)); + newfactors.push_back( + RISAM::make_graduated>( + robust_loss, scheduler, i, 101, Rot2::fromAngle(-(3 * M_PI) / 4.0), + 7.07106, brNoise)); + fullgraph.push_back(newfactors); + + // Add an Outlier Measurement [is not added to fullGraph] + newfactors.push_back( + RISAM::make_graduated>( + robust_loss, scheduler, i, 100, Rot2::fromAngle(0.4), 300, + brNoise)); + + Values init; + init.insert((i + 1), Pose2(double(i + 1) + 0.1, 0.1, 0.01)); + fullinit.insert((i + 1), Pose2(double(i + 1) + 0.1, 0.1, 0.01)); + + risam.update(newfactors, init); + ++i; + } + + /** Compare Results **/ + // Compute Actual + risam.update(); + Values actual = risam.calculateEstimate(); + std::set actual_outliers = risam.getOutliers(0.95); + // Compute Expected + LevenbergMarquardtParams parameters; + Values expected = + LevenbergMarquardtOptimizer(fullgraph, fullinit, parameters).optimize(); + + // Test Solution Quality + CHECK(assert_equal(expected, actual, 0.01)); + // Test Outlier Identification + CHECK(1 == actual_outliers.size()); + CHECK(fullgraph.size() == *(actual_outliers.begin())); +} + +/* ************************************************************************ */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************ */