From 73542d980ab6cef78a2866b404ac01d05d6178a5 Mon Sep 17 00:00:00 2001 From: Francesco Papi Date: Tue, 12 Nov 2019 10:27:34 -0800 Subject: [PATCH 01/14] Move mEstimator into separate file --- gtsam/linear/LossFunctions.cpp | 423 +++++++++++++++++++++++++++++++++ gtsam/linear/LossFunctions.h | 379 +++++++++++++++++++++++++++++ gtsam/linear/NoiseModel.cpp | 397 ------------------------------- gtsam/linear/NoiseModel.h | 346 +-------------------------- 4 files changed, 804 insertions(+), 741 deletions(-) create mode 100644 gtsam/linear/LossFunctions.cpp create mode 100644 gtsam/linear/LossFunctions.h diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp new file mode 100644 index 000000000..80b3e3312 --- /dev/null +++ b/gtsam/linear/LossFunctions.cpp @@ -0,0 +1,423 @@ +/* ---------------------------------------------------------------------------- + + * 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 NoiseModel.cpp + * @date Jan 13, 2010 + * @author Richard Roberts + * @author Frank Dellaert + */ + +#include + +using namespace std; + +namespace gtsam { +namespace noiseModel { + +/* ************************************************************************* */ +// M-Estimator +/* ************************************************************************* */ + +namespace mEstimator { + +Vector Base::weight(const Vector& error) const { + const size_t n = error.rows(); + Vector w(n); + for (size_t i = 0; i < n; ++i) + w(i) = weight(error(i)); + return w; +} + +// The following three functions re-weight block matrices and a vector +// according to their weight implementation + +void Base::reweight(Vector& error) const { + if (reweight_ == Block) { + const double w = sqrtWeight(error.norm()); + error *= w; + } else { + error.array() *= weight(error).cwiseSqrt().array(); + } +} + +// Reweight n block matrices with one error vector +void Base::reweight(vector &A, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + for(Matrix& Aj: A) { + Aj *= w; + } + error *= w; + } + else { + const Vector W = sqrtWeight(error); + for(Matrix& Aj: A) { + vector_scale_inplace(W,Aj); + } + error = W.cwiseProduct(error); + } +} + +// Reweight one block matrix with one error vector +void Base::reweight(Matrix &A, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + A *= w; + error *= w; + } + else { + const Vector W = sqrtWeight(error); + vector_scale_inplace(W,A); + error = W.cwiseProduct(error); + } +} + +// Reweight two block matrix with one error vector +void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + A1 *= w; + A2 *= w; + error *= w; + } + else { + const Vector W = sqrtWeight(error); + vector_scale_inplace(W,A1); + vector_scale_inplace(W,A2); + error = W.cwiseProduct(error); + } +} + +// Reweight three block matrix with one error vector +void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + A1 *= w; + A2 *= w; + A3 *= w; + error *= w; + } + else { + const Vector W = sqrtWeight(error); + vector_scale_inplace(W,A1); + vector_scale_inplace(W,A2); + vector_scale_inplace(W,A3); + error = W.cwiseProduct(error); + } +} + +/* ************************************************************************* */ +// Null model +/* ************************************************************************* */ + +void Null::print(const std::string &s="") const +{ cout << s << "null ()" << endl; } + +Null::shared_ptr Null::Create() +{ return shared_ptr(new Null()); } + +/* ************************************************************************* */ +// Fair +/* ************************************************************************* */ + +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { + if (c_ <= 0) { + throw runtime_error("mEstimator Fair takes only positive double in constructor."); + } +} + +double Fair::weight(double error) const { + return 1.0 / (1.0 + std::abs(error) / c_); +} + +double Fair::residual(double error) const { + const double absError = std::abs(error); + const double normalizedError = absError / c_; + const double c_2 = c_ * c_; + return c_2 * (normalizedError - std::log(1 + normalizedError)); +} + +void Fair::print(const std::string &s="") const +{ cout << s << "fair (" << c_ << ")" << endl; } + +bool Fair::equals(const Base &expected, double tol) const { + const Fair* p = dynamic_cast (&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_ ) < tol; +} + +Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) +{ return shared_ptr(new Fair(c, reweight)); } + +/* ************************************************************************* */ +// Huber +/* ************************************************************************* */ + +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { + if (k_ <= 0) { + throw runtime_error("mEstimator Huber takes only positive double in constructor."); + } +} + +double Huber::weight(double error) const { + const double absError = std::abs(error); + return (absError <= k_) ? (1.0) : (k_ / absError); +} + +double Huber::residual(double error) const { + const double absError = std::abs(error); + if (absError <= k_) { // |x| <= k + return error*error / 2; + } else { // |x| > k + return k_ * (absError - (k_/2)); + } +} + +void Huber::print(const std::string &s="") const { + cout << s << "huber (" << k_ << ")" << endl; +} + +bool Huber::equals(const Base &expected, double tol) const { + const Huber* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(k_ - p->k_) < tol; +} + +Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Huber(c, reweight)); +} + +/* ************************************************************************* */ +// Cauchy +/* ************************************************************************* */ + +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) { + if (k <= 0) { + throw runtime_error("mEstimator Cauchy takes only positive double in constructor."); + } +} + +double Cauchy::weight(double error) const { + return ksquared_ / (ksquared_ + error*error); +} + +double Cauchy::residual(double error) const { + const double xc2 = error / k_; + const double val = std::log(1 + (xc2*xc2)); + return ksquared_ * val * 0.5; +} + +void Cauchy::print(const std::string &s="") const { + cout << s << "cauchy (" << k_ << ")" << endl; +} + +bool Cauchy::equals(const Base &expected, double tol) const { + const Cauchy* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(ksquared_ - p->ksquared_) < tol; +} + +Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Cauchy(c, reweight)); +} + +/* ************************************************************************* */ +// Tukey +/* ************************************************************************* */ + +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) { + if (c <= 0) { + throw runtime_error("mEstimator Tukey takes only positive double in constructor."); + } +} + +double Tukey::weight(double error) const { + if (std::abs(error) <= c_) { + const double xc2 = error*error/csquared_; + return (1.0-xc2)*(1.0-xc2); + } + return 0.0; +} + +double Tukey::residual(double error) const { + double absError = std::abs(error); + if (absError <= c_) { + const double xc2 = error*error/csquared_; + const double t = (1 - xc2)*(1 - xc2)*(1 - xc2); + return csquared_ * (1 - t) / 6.0; + } else { + return csquared_ / 6.0; + } +} + +void Tukey::print(const std::string &s="") const { + std::cout << s << ": Tukey (" << c_ << ")" << std::endl; +} + +bool Tukey::equals(const Base &expected, double tol) const { + const Tukey* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Tukey(c, reweight)); +} + +/* ************************************************************************* */ +// Welsch +/* ************************************************************************* */ + +Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} + +double Welsch::weight(double error) const { + const double xc2 = (error*error)/csquared_; + return std::exp(-xc2); +} + +double Welsch::residual(double error) const { + const double xc2 = (error*error)/csquared_; + return csquared_ * 0.5 * (1 - std::exp(-xc2) ); +} + +void Welsch::print(const std::string &s="") const { + std::cout << s << ": Welsch (" << c_ << ")" << std::endl; +} + +bool Welsch::equals(const Base &expected, double tol) const { + const Welsch* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Welsch(c, reweight)); +} + +/* ************************************************************************* */ +// GemanMcClure +/* ************************************************************************* */ +GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double GemanMcClure::weight(double error) const { + const double c2 = c_*c_; + const double c4 = c2*c2; + const double c2error = c2 + error*error; + return c4/(c2error*c2error); +} + +double GemanMcClure::residual(double error) const { + const double c2 = c_*c_; + const double error2 = error*error; + return 0.5 * (c2 * error2) / (c2 + error2); +} + +void GemanMcClure::print(const std::string &s="") const { + std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; +} + +bool GemanMcClure::equals(const Base &expected, double tol) const { + const GemanMcClure* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new GemanMcClure(c, reweight)); +} + +/* ************************************************************************* */ +// DCS +/* ************************************************************************* */ +DCS::DCS(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double DCS::weight(double error) const { + const double e2 = error*error; + if (e2 > c_) + { + const double w = 2.0*c_/(c_ + e2); + return w*w; + } + + return 1.0; +} + +double DCS::residual(double error) const { + // This is the simplified version of Eq 9 from (Agarwal13icra) + // after you simplify and cancel terms. + const double e2 = error*error; + const double e4 = e2*e2; + const double c2 = c_*c_; + + return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_)); +} + +void DCS::print(const std::string &s="") const { + std::cout << s << ": DCS (" << c_ << ")" << std::endl; +} + +bool DCS::equals(const Base &expected, double tol) const { + const DCS* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new DCS(c, reweight)); +} + +/* ************************************************************************* */ +// L2WithDeadZone +/* ************************************************************************* */ + +L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) + : Base(reweight), k_(k) { + if (k_ <= 0) { + throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor."); + } +} + +double L2WithDeadZone::weight(double error) const { + // note that this code is slightly uglier than residual, because there are three distinct + // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two + // cases (deadzone, non-deadzone) in residual. + if (std::abs(error) <= k_) return 0.0; + else if (error > k_) return (-k_+error)/error; + else return (k_+error)/error; +} + +double L2WithDeadZone::residual(double error) const { + const double abs_error = std::abs(error); + return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); +} + +void L2WithDeadZone::print(const std::string &s="") const { + std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; +} + +bool L2WithDeadZone::equals(const Base &expected, double tol) const { + const L2WithDeadZone* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(k_ - p->k_) < tol; +} + +L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { + return shared_ptr(new L2WithDeadZone(k, reweight)); +} + +} // namespace mEstimator +} // namespace noiseModel +} // gtsam diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h new file mode 100644 index 000000000..1f7cc1377 --- /dev/null +++ b/gtsam/linear/LossFunctions.h @@ -0,0 +1,379 @@ + +/* ---------------------------------------------------------------------------- + + * 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 NoiseModel.h + * @date Jan 13, 2010 + * @author Richard Roberts + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gtsam { +namespace noiseModel { +// clang-format off +/** + * The mEstimator name space contains all robust error functions. + * It mirrors the exposition at + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf + * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. + * + * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: + * + * Name Symbol Least-Squares L1-norm Huber + * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; + + protected: + /** the rows can be weighted independently according to the error + * or uniformly with the norm of the right hand side */ + ReweightScheme reweight_; + + public: + Base(const ReweightScheme reweight = Block) : reweight_(reweight) {} + virtual ~Base() {} + + /* + * This method is responsible for returning the total penalty for a given + * amount of error. For example, this method is responsible for implementing + * the quadratic function for an L2 penalty, the absolute value function for + * an L1 penalty, etc. + * + * TODO(mikebosse): When the residual function has as input the norm of the + * residual vector, then it prevents implementations of asymmeric loss + * functions. It would be better for this function to accept the vector and + * internally call the norm if necessary. + */ + virtual double residual(double error) const { return 0; }; + + /* + * This method is responsible for returning the weight function for a given + * amount of error. The weight function is related to the analytic derivative + * of the residual function. See + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf + * for details. This method is required when optimizing cost functions with + * robust penalties using iteratively re-weighted least squares. + */ + virtual double weight(double error) const = 0; + + virtual void print(const std::string &s) const = 0; + virtual bool equals(const Base &expected, double tol = 1e-8) const = 0; + + double sqrtWeight(double error) const { return std::sqrt(weight(error)); } + + /** produce a weight vector according to an error vector and the implemented + * robust function */ + Vector weight(const Vector &error) const; + + /** square root version of the weight function */ + Vector sqrtWeight(const Vector &error) const { + return weight(error).cwiseSqrt(); + } + + /** reweight block matrices and a vector according to their weight + * implementation */ + void reweight(Vector &error) const; + void reweight(std::vector &A, Vector &error) const; + void reweight(Matrix &A, Vector &error) const; + void reweight(Matrix &A1, Matrix &A2, Vector &error) const; + void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(reweight_); + } +}; + +/// Null class is not robust so is a Gaussian ? +class GTSAM_EXPORT Null : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + Null(const ReweightScheme reweight = Block) : Base(reweight) {} + ~Null() {} + double weight(double /*error*/) const { return 1.0; } + double residual(double error) const { return error; } + void print(const std::string &s) const; + bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } + static shared_ptr Create(); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +/// Fair implements the "Fair" robust error model (Zhang97ivc) +class GTSAM_EXPORT Fair : public Base { + protected: + double c_; + + public: + typedef boost::shared_ptr shared_ptr; + + Fair(double c = 1.3998, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// Huber implements the "Huber" robust error model (Zhang97ivc) +class GTSAM_EXPORT Huber : public Base { + protected: + double k_; + + public: + typedef boost::shared_ptr shared_ptr; + + Huber(double k = 1.345, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(k_); + } +}; + +/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed +/// by: +/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for +/// Information Technology, Karlsruhe, Germany. +/// oberlaender@fzi.de +/// Thanks Jan! +class GTSAM_EXPORT Cauchy : public Base { + protected: + double k_, ksquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Cauchy(double k = 0.1, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(k_); + } +}; + +/// Tukey implements the "Tukey" robust error model (Zhang97ivc) +class GTSAM_EXPORT Tukey : public Base { + protected: + double c_, csquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Tukey(double c = 4.6851, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// Welsch implements the "Welsch" robust error model (Zhang97ivc) +class GTSAM_EXPORT Welsch : public Base { + protected: + double c_, csquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Welsch(double c = 2.9846, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +/// @name Deprecated +/// @{ +// Welsh implements the "Welsch" robust error model (Zhang97ivc) +// This was misspelled in previous versions of gtsam and should be +// removed in the future. +using Welsh = Welsch; +#endif + +/// GemanMcClure implements the "Geman-McClure" robust error model +/// (Zhang97ivc). +/// +/// Note that Geman-McClure weight function uses the parameter c == 1.0, +/// but here it's allowed to use different values, so we actually have +/// the generalized Geman-McClure from (Agarwal15phd). +class GTSAM_EXPORT GemanMcClure : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); + ~GemanMcClure() {} + double weight(double error) const override; + double residual(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); + + protected: + double c_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// DCS implements the Dynamic Covariance Scaling robust error model +/// from the paper Robust Map Optimization (Agarwal13icra). +/// +/// Under the special condition of the parameter c == 1.0 and not +/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. +class GTSAM_EXPORT DCS : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + DCS(double c = 1.0, const ReweightScheme reweight = Block); + ~DCS() {} + double weight(double error) const override; + double residual(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); + + protected: + double c_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of +/// width 2*k, centered at the origin. The resulting penalty within the dead +/// zone is always zero, and grows quadratically outside the dead zone. In this +/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being +/// robust to outliers. This penalty can be used to create barrier functions in +/// a general way. +class GTSAM_EXPORT L2WithDeadZone : public Base { + protected: + double k_; + + public: + typedef boost::shared_ptr shared_ptr; + + L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(k_); + } +}; + +} // namespace mEstimator +} // namespace noiseModel +} // namespace gtsam diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index e0b7c8b17..4e599d45f 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -616,403 +616,6 @@ void Unit::print(const std::string& name) const { cout << name << "unit (" << dim_ << ") " << endl; } -/* ************************************************************************* */ -// M-Estimator -/* ************************************************************************* */ - -namespace mEstimator { - -Vector Base::weight(const Vector& error) const { - const size_t n = error.rows(); - Vector w(n); - for (size_t i = 0; i < n; ++i) - w(i) = weight(error(i)); - return w; -} - -// The following three functions re-weight block matrices and a vector -// according to their weight implementation - -void Base::reweight(Vector& error) const { - if (reweight_ == Block) { - const double w = sqrtWeight(error.norm()); - error *= w; - } else { - error.array() *= weight(error).cwiseSqrt().array(); - } -} - -// Reweight n block matrices with one error vector -void Base::reweight(vector &A, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - for(Matrix& Aj: A) { - Aj *= w; - } - error *= w; - } - else { - const Vector W = sqrtWeight(error); - for(Matrix& Aj: A) { - vector_scale_inplace(W,Aj); - } - error = W.cwiseProduct(error); - } -} - -// Reweight one block matrix with one error vector -void Base::reweight(Matrix &A, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - A *= w; - error *= w; - } - else { - const Vector W = sqrtWeight(error); - vector_scale_inplace(W,A); - error = W.cwiseProduct(error); - } -} - -// Reweight two block matrix with one error vector -void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - A1 *= w; - A2 *= w; - error *= w; - } - else { - const Vector W = sqrtWeight(error); - vector_scale_inplace(W,A1); - vector_scale_inplace(W,A2); - error = W.cwiseProduct(error); - } -} - -// Reweight three block matrix with one error vector -void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - A1 *= w; - A2 *= w; - A3 *= w; - error *= w; - } - else { - const Vector W = sqrtWeight(error); - vector_scale_inplace(W,A1); - vector_scale_inplace(W,A2); - vector_scale_inplace(W,A3); - error = W.cwiseProduct(error); - } -} - -/* ************************************************************************* */ -// Null model -/* ************************************************************************* */ - -void Null::print(const std::string &s="") const -{ cout << s << "null ()" << endl; } - -Null::shared_ptr Null::Create() -{ return shared_ptr(new Null()); } - -/* ************************************************************************* */ -// Fair -/* ************************************************************************* */ - -Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { - if (c_ <= 0) { - throw runtime_error("mEstimator Fair takes only positive double in constructor."); - } -} - -double Fair::weight(double error) const { - return 1.0 / (1.0 + std::abs(error) / c_); -} - -double Fair::residual(double error) const { - const double absError = std::abs(error); - const double normalizedError = absError / c_; - const double c_2 = c_ * c_; - return c_2 * (normalizedError - std::log(1 + normalizedError)); -} - -void Fair::print(const std::string &s="") const -{ cout << s << "fair (" << c_ << ")" << endl; } - -bool Fair::equals(const Base &expected, double tol) const { - const Fair* p = dynamic_cast (&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_ ) < tol; -} - -Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) -{ return shared_ptr(new Fair(c, reweight)); } - -/* ************************************************************************* */ -// Huber -/* ************************************************************************* */ - -Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { - if (k_ <= 0) { - throw runtime_error("mEstimator Huber takes only positive double in constructor."); - } -} - -double Huber::weight(double error) const { - const double absError = std::abs(error); - return (absError <= k_) ? (1.0) : (k_ / absError); -} - -double Huber::residual(double error) const { - const double absError = std::abs(error); - if (absError <= k_) { // |x| <= k - return error*error / 2; - } else { // |x| > k - return k_ * (absError - (k_/2)); - } -} - -void Huber::print(const std::string &s="") const { - cout << s << "huber (" << k_ << ")" << endl; -} - -bool Huber::equals(const Base &expected, double tol) const { - const Huber* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(k_ - p->k_) < tol; -} - -Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Huber(c, reweight)); -} - -/* ************************************************************************* */ -// Cauchy -/* ************************************************************************* */ - -Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) { - if (k <= 0) { - throw runtime_error("mEstimator Cauchy takes only positive double in constructor."); - } -} - -double Cauchy::weight(double error) const { - return ksquared_ / (ksquared_ + error*error); -} - -double Cauchy::residual(double error) const { - const double xc2 = error / k_; - const double val = std::log(1 + (xc2*xc2)); - return ksquared_ * val * 0.5; -} - -void Cauchy::print(const std::string &s="") const { - cout << s << "cauchy (" << k_ << ")" << endl; -} - -bool Cauchy::equals(const Base &expected, double tol) const { - const Cauchy* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(ksquared_ - p->ksquared_) < tol; -} - -Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Cauchy(c, reweight)); -} - -/* ************************************************************************* */ -// Tukey -/* ************************************************************************* */ - -Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) { - if (c <= 0) { - throw runtime_error("mEstimator Tukey takes only positive double in constructor."); - } -} - -double Tukey::weight(double error) const { - if (std::abs(error) <= c_) { - const double xc2 = error*error/csquared_; - return (1.0-xc2)*(1.0-xc2); - } - return 0.0; -} - -double Tukey::residual(double error) const { - double absError = std::abs(error); - if (absError <= c_) { - const double xc2 = error*error/csquared_; - const double t = (1 - xc2)*(1 - xc2)*(1 - xc2); - return csquared_ * (1 - t) / 6.0; - } else { - return csquared_ / 6.0; - } -} - -void Tukey::print(const std::string &s="") const { - std::cout << s << ": Tukey (" << c_ << ")" << std::endl; -} - -bool Tukey::equals(const Base &expected, double tol) const { - const Tukey* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Tukey(c, reweight)); -} - -/* ************************************************************************* */ -// Welsch -/* ************************************************************************* */ - -Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} - -double Welsch::weight(double error) const { - const double xc2 = (error*error)/csquared_; - return std::exp(-xc2); -} - -double Welsch::residual(double error) const { - const double xc2 = (error*error)/csquared_; - return csquared_ * 0.5 * (1 - std::exp(-xc2) ); -} - -void Welsch::print(const std::string &s="") const { - std::cout << s << ": Welsch (" << c_ << ")" << std::endl; -} - -bool Welsch::equals(const Base &expected, double tol) const { - const Welsch* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Welsch(c, reweight)); -} - -/* ************************************************************************* */ -// GemanMcClure -/* ************************************************************************* */ -GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double GemanMcClure::weight(double error) const { - const double c2 = c_*c_; - const double c4 = c2*c2; - const double c2error = c2 + error*error; - return c4/(c2error*c2error); -} - -double GemanMcClure::residual(double error) const { - const double c2 = c_*c_; - const double error2 = error*error; - return 0.5 * (c2 * error2) / (c2 + error2); -} - -void GemanMcClure::print(const std::string &s="") const { - std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; -} - -bool GemanMcClure::equals(const Base &expected, double tol) const { - const GemanMcClure* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new GemanMcClure(c, reweight)); -} - -/* ************************************************************************* */ -// DCS -/* ************************************************************************* */ -DCS::DCS(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double DCS::weight(double error) const { - const double e2 = error*error; - if (e2 > c_) - { - const double w = 2.0*c_/(c_ + e2); - return w*w; - } - - return 1.0; -} - -double DCS::residual(double error) const { - // This is the simplified version of Eq 9 from (Agarwal13icra) - // after you simplify and cancel terms. - const double e2 = error*error; - const double e4 = e2*e2; - const double c2 = c_*c_; - - return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_)); -} - -void DCS::print(const std::string &s="") const { - std::cout << s << ": DCS (" << c_ << ")" << std::endl; -} - -bool DCS::equals(const Base &expected, double tol) const { - const DCS* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new DCS(c, reweight)); -} - -/* ************************************************************************* */ -// L2WithDeadZone -/* ************************************************************************* */ - -L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) - : Base(reweight), k_(k) { - if (k_ <= 0) { - throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor."); - } -} - -double L2WithDeadZone::weight(double error) const { - // note that this code is slightly uglier than residual, because there are three distinct - // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two - // cases (deadzone, non-deadzone) in residual. - if (std::abs(error) <= k_) return 0.0; - else if (error > k_) return (-k_+error)/error; - else return (k_+error)/error; -} - -double L2WithDeadZone::residual(double error) const { - const double abs_error = std::abs(error); - return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); -} - -void L2WithDeadZone::print(const std::string &s="") const { - std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; -} - -bool L2WithDeadZone::equals(const Base &expected, double tol) const { - const L2WithDeadZone* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(k_ - p->k_) < tol; -} - -L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { - return shared_ptr(new L2WithDeadZone(k, reweight)); -} - -} // namespace mEstimator - /* ************************************************************************* */ // Robust /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index f73fde51c..6fe50ed4c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -39,6 +40,7 @@ namespace gtsam { class Constrained; class Isotropic; class Unit; + class RobustModel; //--------------------------------------------------------------------------------------- @@ -626,350 +628,6 @@ namespace gtsam { } }; - /** - * The mEstimator name space contains all robust error functions. - * It mirrors the exposition at - * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. - * - * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: - * - * Name Symbol Least-Squares L1-norm Huber - * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; - - protected: - /** the rows can be weighted independently according to the error - * or uniformly with the norm of the right hand side */ - ReweightScheme reweight_; - - public: - Base(const ReweightScheme reweight = Block):reweight_(reweight) {} - virtual ~Base() {} - - /* - * This method is responsible for returning the total penalty for a given amount of error. - * For example, this method is responsible for implementing the quadratic function for an - * L2 penalty, the absolute value function for an L1 penalty, etc. - * - * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes - * implement a residual function, and GTSAM calls the weight function to evaluate the - * total penalty, rather than calling the residual function. The weight function should be - * used during iteratively reweighted least squares optimization, but should not be used to - * evaluate the total penalty. The long-term solution is for all mEstimators to implement - * both a weight and a residual function, and for GTSAM to call the residual function when - * evaluating the total penalty. But for now, I'm leaving this residual method as pure - * virtual, so the existing mEstimators can inherit this default fallback behavior. - */ - virtual double residual(double error) const { return 0; }; - - /* - * This method is responsible for returning the weight function for a given amount of error. - * The weight function is related to the analytic derivative of the residual function. See - * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * for details. This method is required when optimizing cost functions with robust penalties - * using iteratively re-weighted least squares. - */ - virtual double weight(double error) const = 0; - - virtual void print(const std::string &s) const = 0; - virtual bool equals(const Base& expected, double tol=1e-8) const = 0; - - double sqrtWeight(double error) const { - return std::sqrt(weight(error)); - } - - /** produce a weight vector according to an error vector and the implemented - * robust function */ - Vector weight(const Vector &error) const; - - /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const { - return weight(error).cwiseSqrt(); - } - - /** reweight block matrices and a vector according to their weight implementation */ - void reweight(Vector &error) const; - void reweight(std::vector &A, Vector &error) const; - void reweight(Matrix &A, Vector &error) const; - void reweight(Matrix &A1, Matrix &A2, Vector &error) const; - void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(reweight_); - } - }; - - /// Null class is not robust so is a Gaussian ? - class GTSAM_EXPORT Null : public Base { - public: - typedef boost::shared_ptr shared_ptr; - - Null(const ReweightScheme reweight = Block) : Base(reweight) {} - ~Null() {} - double weight(double /*error*/) const { return 1.0; } - double residual(double error) const { return error; } - void print(const std::string &s) const; - bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } - static shared_ptr Create() ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - }; - - /// Fair implements the "Fair" robust error model (Zhang97ivc) - class GTSAM_EXPORT Fair : public Base { - protected: - double c_; - - public: - typedef boost::shared_ptr shared_ptr; - - Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// Huber implements the "Huber" robust error model (Zhang97ivc) - class GTSAM_EXPORT Huber : public Base { - protected: - double k_; - - public: - typedef boost::shared_ptr shared_ptr; - - Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(k_); - } - }; - - /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: - /// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for - /// Information Technology, Karlsruhe, Germany. - /// oberlaender@fzi.de - /// Thanks Jan! - class GTSAM_EXPORT Cauchy : public Base { - protected: - double k_, ksquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(k_); - } - }; - - /// Tukey implements the "Tukey" robust error model (Zhang97ivc) - class GTSAM_EXPORT Tukey : public Base { - protected: - double c_, csquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// Welsch implements the "Welsch" robust error model (Zhang97ivc) - class GTSAM_EXPORT Welsch : public Base { - protected: - double c_, csquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Welsch(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /// @name Deprecated - /// @{ - // Welsh implements the "Welsch" robust error model (Zhang97ivc) - // This was misspelled in previous versions of gtsam and should be - // removed in the future. - using Welsh = Welsch; -#endif - - /// GemanMcClure implements the "Geman-McClure" robust error model - /// (Zhang97ivc). - /// - /// Note that Geman-McClure weight function uses the parameter c == 1.0, - /// but here it's allowed to use different values, so we actually have - /// the generalized Geman-McClure from (Agarwal15phd). - class GTSAM_EXPORT GemanMcClure : public Base { - public: - typedef boost::shared_ptr shared_ptr; - - GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); - ~GemanMcClure() {} - double weight(double error) const override; - double residual(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) ; - - protected: - double c_; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// DCS implements the Dynamic Covariance Scaling robust error model - /// from the paper Robust Map Optimization (Agarwal13icra). - /// - /// Under the special condition of the parameter c == 1.0 and not - /// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. - class GTSAM_EXPORT DCS : public Base { - public: - typedef boost::shared_ptr shared_ptr; - - DCS(double c = 1.0, const ReweightScheme reweight = Block); - ~DCS() {} - double weight(double error) const override; - double residual(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) ; - - protected: - double c_; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k, - /// centered at the origin. The resulting penalty within the dead zone is always zero, and - /// grows quadratically outside the dead zone. In this sense, the L2WithDeadZone penalty is - /// "robust to inliers", rather than being robust to outliers. This penalty can be used to - /// create barrier functions in a general way. - class GTSAM_EXPORT L2WithDeadZone : public Base { - protected: - double k_; - - public: - typedef boost::shared_ptr shared_ptr; - - L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(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); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(k_); - } - }; - - } ///\namespace mEstimator - /** * Base class for robust error models * The robust M-estimators above simply tell us how to re-weight the residual, and are From 92008c9319c56bb2edc3a3de7da091a21191b306 Mon Sep 17 00:00:00 2001 From: Francesco Papi Date: Tue, 12 Nov 2019 14:07:22 -0800 Subject: [PATCH 02/14] Add functionalities to get introspection of the loss function weights and unweighted residuals for the NoiseModelFactor class --- gtsam/linear/NoiseModel.h | 20 ++++++++++++++++++-- gtsam/nonlinear/NonlinearFactor.cpp | 22 ++++++++++++++++++++++ gtsam/nonlinear/NonlinearFactor.h | 10 ++++++++++ 3 files changed, 50 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6fe50ed4c..6c42fc7ba 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -117,6 +117,14 @@ namespace gtsam { v = unwhiten(v); } + /** Useful function for robust noise models to get the unweighted but whitened error */ + virtual Vector unweightedWhiten(const Vector& v) const { + return whiten(v); + } + + /** get the weight from the effective loss function on residual vector v */ + virtual double weight(const Vector& v) const { return 1.0; } + private: /** Serialization function */ friend class boost::serialization::access; @@ -684,9 +692,9 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } + // Fold the use of the m-estimator residual(...) function into distance(...) inline virtual double distance(const Vector& v) const - { return this->whiten(v).squaredNorm(); } - // TODO(mike): fold the use of the m-estimator residual(...) function into distance(...) + { return robust_->residual(this->unweightedWhiten(v).norm()); } inline virtual double distance_non_whitened(const Vector& v) const { return robust_->residual(v.norm()); } // TODO: these are really robust iterated re-weighting support functions @@ -696,6 +704,14 @@ namespace gtsam { virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; + virtual Vector unweightedWhiten(const Vector& v) const { + return noise_->unweightedWhiten(v); + } + virtual double weight(const Vector& v) const { + // Todo(mikebosse): make the robust weight function input a vector. + return robust_->weight(v.norm()); + } + static shared_ptr Create( const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 5c89425c8..ee14e8073 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -93,6 +93,28 @@ Vector NoiseModelFactor::whitenedError(const Values& c) const { return noiseModel_ ? noiseModel_->whiten(b) : b; } +/* ************************************************************************* */ +Vector NoiseModelFactor::unweightedWhitenedError(const Values& c) const { + const Vector b = unwhitenedError(c); + check(noiseModel_, b.size()); + return noiseModel_ ? noiseModel_->unweightedWhiten(b) : b; +} + +/* ************************************************************************* */ +double NoiseModelFactor::weight(const Values& c) const { + if (active(c)) { + if (noiseModel_) { + const Vector b = unwhitenedError(c); + check(noiseModel_, b.size()); + return 0.5 * noiseModel_->weight(b); + } + else + return 1.0; + } else { + return 0.0; + } +} + /* ************************************************************************* */ double NoiseModelFactor::error(const Values& c) const { if (active(c)) { diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 1942734c5..63547a248 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -226,6 +226,16 @@ public: */ Vector whitenedError(const Values& c) const; + /** + * Vector of errors, whitened, but unweighted by any loss function + */ + Vector unweightedWhitenedError(const Values& c) const; + + /** + * Compute the effective weight of the factor from the noise model. + */ + double weight(const Values& c) const; + /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. From 8cdb009af953a26f5b079722dcc397a89fa69331 Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Wed, 13 Nov 2019 13:13:48 -0800 Subject: [PATCH 03/14] missing iosteam header include --- gtsam/linear/LossFunctions.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 80b3e3312..2a82122cd 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -18,6 +18,8 @@ #include +#include + using namespace std; namespace gtsam { From 8fe843513614927797138f65c6e97f8346a08832 Mon Sep 17 00:00:00 2001 From: Lukas Solanka Date: Tue, 19 Nov 2019 09:56:08 +0100 Subject: [PATCH 04/14] make include_directories relative for install interface --- gtsam/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 97f7a2c41..82cdf2bad 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -136,13 +136,13 @@ endif() target_include_directories(gtsam BEFORE PUBLIC # SuiteSparse_config $ - $ + $ # CCOLAMD $ - $ + $ # main gtsam includes: $ - $ + $ # config.h $ # unit tests: From 13250d5bd75b64b70cd61aa69a1e98bdf9d9e907 Mon Sep 17 00:00:00 2001 From: Ruffin Date: Sun, 24 Nov 2019 13:50:21 -0800 Subject: [PATCH 05/14] Include build dependencies --- package.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/package.xml b/package.xml index e14e77f2a..1aa729c03 100644 --- a/package.xml +++ b/package.xml @@ -8,6 +8,11 @@ BSD cmake + + boost + eigen + tbb + cmake From 516ccf532cedd28c793332e8387fd6b1f3044bcb Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Wed, 27 Nov 2019 13:17:59 -0800 Subject: [PATCH 06/14] Improve numerical precision of residual functions --- gtsam/linear/LossFunctions.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 2a82122cd..6bc737e2c 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -145,7 +145,7 @@ double Fair::residual(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; - return c_2 * (normalizedError - std::log(1 + normalizedError)); + return c_2 * (normalizedError - std::log1p(normalizedError)); } void Fair::print(const std::string &s="") const @@ -213,8 +213,7 @@ double Cauchy::weight(double error) const { } double Cauchy::residual(double error) const { - const double xc2 = error / k_; - const double val = std::log(1 + (xc2*xc2)); + const double val = std::log1p(error * error / ksquared_); return ksquared_ * val * 0.5; } @@ -244,8 +243,8 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c double Tukey::weight(double error) const { if (std::abs(error) <= c_) { - const double xc2 = error*error/csquared_; - return (1.0-xc2)*(1.0-xc2); + const double one_minus_xc2 = 1.0 - error*error/csquared_; + return one_minus_xc2 * one_minus_xc2; } return 0.0; } @@ -253,8 +252,8 @@ double Tukey::weight(double error) const { double Tukey::residual(double error) const { double absError = std::abs(error); if (absError <= c_) { - const double xc2 = error*error/csquared_; - const double t = (1 - xc2)*(1 - xc2)*(1 - xc2); + const double one_minus_xc2 = 1.0 - error*error/csquared_; + const double t = one_minus_xc2*one_minus_xc2*one_minus_xc2; return csquared_ * (1 - t) / 6.0; } else { return csquared_ / 6.0; @@ -288,7 +287,7 @@ double Welsch::weight(double error) const { double Welsch::residual(double error) const { const double xc2 = (error*error)/csquared_; - return csquared_ * 0.5 * (1 - std::exp(-xc2) ); + return csquared_ * 0.5 * -std::expm1(-xc2); } void Welsch::print(const std::string &s="") const { From 044582ca4a0ac3604975dfb42f4799788dde22f1 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 3 Dec 2019 13:41:37 +0100 Subject: [PATCH 07/14] also update gtsam version in ROS package.xml --- package.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/package.xml b/package.xml index 1aa729c03..f8554729e 100644 --- a/package.xml +++ b/package.xml @@ -1,18 +1,18 @@ gtsam - 4.0.0 + 4.0.2 gtsam Frank Dellaert BSD cmake - + boost eigen tbb - + cmake From c7111dbbd30c6d628856dc7599d8567116ff002b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Dec 2019 15:41:49 -0500 Subject: [PATCH 08/14] install GtsamPrinting.cmake as part of installation --- cmake/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index f2ca9933e..d612e2fae 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -20,6 +20,7 @@ install(FILES GtsamPythonWrap.cmake GtsamCythonWrap.cmake GtsamTesting.cmake + GtsamPrinting.cmake FindCython.cmake FindNumPy.cmake README.html From 7fa705e6095d6fd23091c273e68318b67035161e Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Sat, 7 Dec 2019 14:07:56 -0800 Subject: [PATCH 09/14] Eigen alignment fix in ExpressionNode and RotateFactor --- gtsam/nonlinear/internal/ExpressionNode.h | 2 ++ gtsam/slam/RotateFactor.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d11908b54..6752d7fc0 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -149,6 +149,8 @@ public: ExecutionTraceStorage* traceStorage) const { return constant_; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; //----------------------------------------------------------------------------- diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 9d0975df3..b6ccc36a2 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -112,6 +112,8 @@ public: } return error; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace gtsam From 087221ac955c9d66a15c07f383562dec64dc38ea Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 11 Dec 2019 16:31:43 +0100 Subject: [PATCH 10/14] travis: use clang-9 --- .travis.sh | 51 ++++++++++++++------------------ .travis.yml | 80 +++++++++++++++++++++++++++----------------------- CMakeLists.txt | 2 +- 3 files changed, 66 insertions(+), 67 deletions(-) diff --git a/.travis.sh b/.travis.sh index 3cec20f53..5de2d8e69 100755 --- a/.travis.sh +++ b/.travis.sh @@ -1,7 +1,7 @@ #!/bin/bash # common tasks before either build or test -function prepare () +function configure() { set -e # Make sure any error makes the script to return an error code set -x # echo @@ -14,21 +14,23 @@ function prepare () rm -fr $BUILD_DIR || true mkdir $BUILD_DIR && cd $BUILD_DIR - if [ -z "$CMAKE_BUILD_TYPE" ]; then - CMAKE_BUILD_TYPE=Debug - fi - - if [ -z "$GTSAM_ALLOW_DEPRECATED_SINCE_V4" ]; then - GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF - fi - if [ ! -z "$GCC_VERSION" ]; then - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-$GCC_VERSION 60 \ - --slave /usr/bin/g++ g++ /usr/bin/g++-$GCC_VERSION - sudo update-alternatives --set gcc /usr/bin/gcc-$GCC_VERSION + export CC=gcc-$GCC_VERSION + export CXX=g++-$GCC_VERSION fi + + # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs + cmake $SOURCE_DIR \ + -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ + -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ + -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ + -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ + -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ + -DCMAKE_VERBOSE_MAKEFILE=ON } + # common tasks after either build or test function finish () { @@ -41,17 +43,12 @@ function finish () # compile the code with the intent of populating the cache function build () { - prepare + export GTSAM_BUILD_EXAMPLES_ALWAYS=ON + export GTSAM_BUILD_TESTS=OFF - cmake $SOURCE_DIR \ - -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \ - -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \ - -DGTSAM_BUILD_EXAMPLES_ALWAYS=ON \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4 + configure - # Actual build: - VERBOSE=1 make -j2 + make -j2 finish } @@ -59,14 +56,10 @@ function build () # run the tests function test () { - prepare + export GTSAM_BUILD_EXAMPLES_ALWAYS=OFF + export GTSAM_BUILD_TESTS=ON - cmake $SOURCE_DIR \ - -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \ - -DGTSAM_BUILD_TESTS=ON \ - -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \ - -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF + configure # Actual build: make -j2 check @@ -79,7 +72,7 @@ case $1 in -b) build ;; - -t) + -t) test ;; esac diff --git a/.travis.yml b/.travis.yml index 1e2d6760a..bcb6ceb4a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,11 +7,12 @@ addons: apt: sources: - ubuntu-toolchain-r-test + - sourceline: 'deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial-9 main' + key_url: 'https://apt.llvm.org/llvm-snapshot.gpg.key' packages: - - g++-8 - - clang-3.8 - - build-essential - - pkg-config + - g++-9 + - clang-9 + - build-essential pkg-config - cmake - libpython-dev python-numpy - libboost-all-dev @@ -28,8 +29,14 @@ stages: - compile - test +env: + global: + - MAKEFLAGS="-j2" + - CCACHE_SLOPPINESS=pch_defines,time_macros + # Compile stage without building examples/tests to populate the caches. jobs: +# -------- STAGE 1: COMPILE ----------- include: # on Mac, GCC - stage: compile @@ -68,46 +75,45 @@ jobs: - stage: compile os: linux compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: linux compiler: clang - env: CMAKE_BUILD_TYPE=Release + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release script: bash .travis.sh -b # on Linux, with deprecated ON to make sure that path still compiles - stage: compile os: linux compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON script: bash .travis.sh -b - -# Matrix configuration: -os: - - osx - - linux -compiler: - - gcc - - clang -env: - global: - - MAKEFLAGS="-j2" - - CCACHE_SLOPPINESS=pch_defines,time_macros - - GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF - - GTSAM_BUILD_UNSTABLE=ON - matrix: - - CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF - - CMAKE_BUILD_TYPE=Release -script: - - bash .travis.sh -t - -matrix: - exclude: - # Exclude g++ debug on Linux as it consistently times out - - os: linux - compiler: gcc - env : CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF - # Exclude clang on Linux/clang in release until issue #57 is solved - - os: linux - compiler: clang - env : CMAKE_BUILD_TYPE=Release +# -------- STAGE 2: TESTS ----------- +# on Mac, GCC + - stage: test + os: osx + compiler: clang + env: CMAKE_BUILD_TYPE=Release + script: bash .travis.sh -t + - stage: test + os: osx + compiler: clang + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + script: bash .travis.sh -t + - stage: test + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Release + script: bash .travis.sh -t +# Exclude g++ debug on Linux as it consistently times out +# - stage: test +# os: linux +# compiler: gcc +# env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF +# script: bash .travis.sh -t +# Exclude clang on Linux/clang in release until issue #57 is solved +# - stage: test +# os: linux +# compiler: clang +# env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release +# script: bash .travis.sh -t diff --git a/CMakeLists.txt b/CMakeLists.txt index 732717141..2af6341ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -434,7 +434,7 @@ add_subdirectory(timing) # Build gtsam_unstable if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) -endif(GTSAM_BUILD_UNSTABLE) +endif() # Matlab toolbox if (GTSAM_INSTALL_MATLAB_TOOLBOX) From 4f37ad3057c10ae5ad2ae1e0bf98bbab8e83036b Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 11 Dec 2019 19:05:44 -0500 Subject: [PATCH 11/14] Change to normal allocator --- gtsam/base/GenericValue.h | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 52899fe45..aee6c0e62 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -89,12 +89,9 @@ public: /** * Create a duplicate object returned as a pointer to the generic Value interface. - * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. - * The result must be deleted with Value::deallocate_, not with the 'delete' operator. */ virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in + GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in return ptr; } @@ -102,8 +99,7 @@ public: * Destroy and deallocate this object, only if it was originally allocated using clone_(). */ virtual void deallocate_() const { - this->~GenericValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*) this); // Release memory from pool + delete this; } /** @@ -118,10 +114,7 @@ public: // Call retract on the derived class using the retract trait function const T retractResult = traits::Retract(GenericValue::value(), delta); - // Create a Value pointer copy of the result - void* resultAsValuePlace = - boost::singleton_pool::malloc(); - Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult); + Value* resultAsValue = new GenericValue(retractResult); // Return the pointer to the Value base class return resultAsValue; @@ -172,12 +165,6 @@ public: return *this; } - private: - - /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { - }; - private: /** Serialization function */ From d0971104ced62c288e26d5e5c6e17ddc4cb68c1d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 10 Dec 2019 12:20:31 +0100 Subject: [PATCH 12/14] Fix broken 64bit hash in tbb Fixes #181 --- gtsam/base/ConcurrentMap.h | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index b8388057d..2d7cbd6db 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -29,14 +29,22 @@ # undef max # undef ERROR +#include // std::hash() + // Use TBB concurrent_unordered_map for ConcurrentMap -# define CONCURRENT_MAP_BASE tbb::concurrent_unordered_map +template +using ConcurrentMapBase = tbb::concurrent_unordered_map< + KEY, + VALUE, + std::hash + >; #else // If we're not using TBB, use a FastMap for ConcurrentMap -# include -# define CONCURRENT_MAP_BASE gtsam::FastMap +#include +template +using ConcurrentMapBase = gtsam::FastMap; #endif @@ -57,11 +65,11 @@ namespace gtsam { * @addtogroup base */ template -class ConcurrentMap : public CONCURRENT_MAP_BASE { +class ConcurrentMap : public ConcurrentMapBase { public: - typedef CONCURRENT_MAP_BASE Base; + typedef ConcurrentMapBase Base; /** Default constructor */ ConcurrentMap() {} From fb05fa6bd70853d0853b77bd99338b413f79d702 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 14 Dec 2019 23:26:22 +0100 Subject: [PATCH 13/14] Fix MSVC link errors (missing DLLEXPORTs) --- gtsam/base/DSFMap.h | 4 ++-- gtsam/base/ThreadsafeException.h | 3 ++- gtsam/base/timing.h | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 16 ++++++++-------- gtsam/symbolic/SymbolicBayesTree.h | 6 +++--- 5 files changed, 16 insertions(+), 15 deletions(-) diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h index dfce185dc..1d6bfdefa 100644 --- a/gtsam/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -115,8 +115,8 @@ class DSFMap { /// Small utility class for representing a wrappable pairs of ints. class IndexPair : public std::pair { public: - IndexPair(): std::pair(0,0) {} - IndexPair(size_t i, size_t j) : std::pair(i,j) {} + inline IndexPair(): std::pair(0,0) {} + inline IndexPair(size_t i, size_t j) : std::pair(i,j) {} inline size_t i() const { return first; }; inline size_t j() const { return second; }; }; diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index ff82ff27c..15a76fbf1 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -22,6 +22,7 @@ #include // for GTSAM_USE_TBB #include +#include #include #include @@ -117,7 +118,7 @@ public: }; /// Thread-safe runtime error exception -class RuntimeErrorThreadsafe: public ThreadsafeException { +class GTSAM_EXPORT RuntimeErrorThreadsafe: public ThreadsafeException { public: /// Construct with a string describing the exception RuntimeErrorThreadsafe(const std::string& description) : diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 557500e73..972bb45f7 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -196,7 +196,7 @@ namespace gtsam { /** * Small class that calls internal::tic at construction, and internol::toc when destroyed */ - class AutoTicToc { + class GTSAM_EXPORT AutoTicToc { private: size_t id_; const char* label_; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 9757cca73..65fdd1c92 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -31,7 +31,7 @@ namespace gtsam { /** The common parameters for Nonlinear optimizers. Most optimizers * deriving from NonlinearOptimizer also subclass the parameters. */ -class NonlinearOptimizerParams { +class GTSAM_EXPORT NonlinearOptimizerParams { public: /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { @@ -52,7 +52,7 @@ public: virtual ~NonlinearOptimizerParams() { } - GTSAM_EXPORT virtual void print(const std::string& str = "") const; + virtual void print(const std::string& str = "") const; size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { return relativeErrorTol; } @@ -68,8 +68,8 @@ public: verbosity = verbosityTranslator(src); } - GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s) ; - GTSAM_EXPORT static std::string verbosityTranslator(Verbosity value) ; + static Verbosity verbosityTranslator(const std::string &s) ; + static std::string verbosityTranslator(Verbosity value) ; /** See NonlinearOptimizerParams::linearSolverType */ enum LinearSolverType { @@ -144,10 +144,10 @@ public: } private: - GTSAM_EXPORT std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - GTSAM_EXPORT LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - GTSAM_EXPORT std::string orderingTypeTranslator(Ordering::OrderingType type) const; - GTSAM_EXPORT Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; + std::string linearSolverTranslator(LinearSolverType linearSolverType) const; + LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; + std::string orderingTypeTranslator(Ordering::OrderingType type) const; + Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; }; // For backward compatibility: diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index 5f7bdde7e..e28f28764 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ /// A clique in a SymbolicBayesTree - class SymbolicBayesTreeClique : + class GTSAM_EXPORT SymbolicBayesTreeClique : public BayesTreeCliqueBase { public: @@ -45,7 +45,7 @@ namespace gtsam { /* ************************************************************************* */ /// A Bayes tree that represents the connectivity between variables but is not associated with any /// probability functions. - class SymbolicBayesTree : + class GTSAM_EXPORT SymbolicBayesTree : public BayesTree { private: @@ -59,7 +59,7 @@ namespace gtsam { SymbolicBayesTree() {} /** check equality */ - GTSAM_EXPORT bool equals(const This& other, double tol = 1e-9) const; + bool equals(const This& other, double tol = 1e-9) const; private: /** Serialization function */ From d76fc052b03fdd31ae124a58d40fc098a9c2f33d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 20 Dec 2019 10:06:15 +0100 Subject: [PATCH 14/14] fix missing include --- gtsam_unstable/partition/GenericGraph.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam_unstable/partition/GenericGraph.h b/gtsam_unstable/partition/GenericGraph.h index ec0027635..3dc640e97 100644 --- a/gtsam_unstable/partition/GenericGraph.h +++ b/gtsam_unstable/partition/GenericGraph.h @@ -12,6 +12,7 @@ #include #include #include +#include #include #include "PartitionWorkSpace.h"