Add a custom loss function for Robust, fix a bug in Asymmetric
Signed-off-by: Fan Jiang <i@fanjiang.me>release/4.3a0
							parent
							
								
									8214bdc2ff
								
							
						
					
					
						commit
						9c40a2cd8b
					
				|  | @ -19,6 +19,7 @@ | |||
| #include <gtsam/linear/LossFunctions.h> | ||||
| 
 | ||||
| #include <iostream> | ||||
| #include <utility> | ||||
| #include <vector> | ||||
| 
 | ||||
| using namespace std; | ||||
|  | @ -438,7 +439,7 @@ AsymmetricTukey::AsymmetricTukey(double c, const ReweightScheme reweight) : Base | |||
| double AsymmetricTukey::weight(double distance) const { | ||||
|   distance = -distance; | ||||
|   if (distance >= 0.0) { | ||||
|     return distance; | ||||
|     return 1.0; | ||||
|   } else if (distance > -c_) { | ||||
|     const double one_minus_xc2 = 1.0 - distance * distance / csquared_; | ||||
|     return one_minus_xc2 * one_minus_xc2; | ||||
|  | @ -486,7 +487,7 @@ AsymmetricCauchy::AsymmetricCauchy(double k, const ReweightScheme reweight) : Ba | |||
| double AsymmetricCauchy::weight(double distance) const { | ||||
|   distance = -distance; | ||||
|   if (distance >= 0.0) { | ||||
|     return distance; | ||||
|     return 1.0; | ||||
|   } | ||||
|    | ||||
|     return ksquared_ / (ksquared_ + distance*distance); | ||||
|  | @ -517,6 +518,39 @@ AsymmetricCauchy::shared_ptr AsymmetricCauchy::Create(double k, const ReweightSc | |||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Custom
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| Custom::Custom(std::function<double(double)> weight, std::function<double(double)> 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); | ||||
| } | ||||
| 
 | ||||
| double Custom::loss(double distance) const { | ||||
|   return loss_(distance); | ||||
| } | ||||
| 
 | ||||
| void Custom::print(const std::string &s = "") const { | ||||
|   std::cout << s << ": Custom (" << name_ << ")" << std::endl; | ||||
| } | ||||
| 
 | ||||
| bool Custom::equals(const Base &expected, double tol) const { | ||||
|   const auto *p = dynamic_cast<const Custom *>(&expected); | ||||
|   if (p == nullptr) | ||||
|     return false; | ||||
|   return name_ == p->name_ && weight_.target<double(double)>() == p->weight_.target<double(double)>() && | ||||
|          loss_.target<double(double)>() == p->loss_.target<double(double)>() && reweight_ == p->reweight_; | ||||
| } | ||||
| 
 | ||||
| Custom::shared_ptr Custom::Create(std::function<double(double)> weight, std::function<double(double)> loss, | ||||
|                                   const ReweightScheme reweight, const std::string &name) { | ||||
|   return std::make_shared<Custom>(std::move(weight), std::move(loss), reweight, name); | ||||
| } | ||||
| 
 | ||||
| } // namespace mEstimator
 | ||||
| } // namespace noiseModel
 | ||||
| } // gtsam
 | ||||
|  |  | |||
|  | @ -544,6 +544,50 @@ class GTSAM_EXPORT AsymmetricCauchy : public Base { | |||
| #endif | ||||
| }; | ||||
| 
 | ||||
| // Type alias for the custom loss and weight functions
 | ||||
| using CustomLossFunction = std::function<double(double)>; | ||||
| using CustomWeightFunction = std::function<double(double)>; | ||||
| 
 | ||||
| /** Implementation of the "Custom" robust error model.
 | ||||
|  * | ||||
|  *  This model just takes two functions as input, one for the loss and one for the weight. | ||||
|  */ | ||||
| class GTSAM_EXPORT Custom : public Base { | ||||
|  protected: | ||||
|   std::function<double(double)> weight_, loss_; | ||||
|   std::string name_; | ||||
| 
 | ||||
|  public: | ||||
|   typedef std::shared_ptr<Custom> shared_ptr; | ||||
| 
 | ||||
|   Custom(CustomWeightFunction weight, CustomLossFunction loss, | ||||
|          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<double(double)> weight, std::function<double(double)> loss, | ||||
|                            const ReweightScheme reweight = Block, const std::string &name = "Custom"); | ||||
|   inline std::string& name() { return name_; } | ||||
| 
 | ||||
|   inline std::function<double(double)>& weightFunction() { return weight_; } | ||||
|   inline std::function<double(double)>& lossFunction() { return loss_; } | ||||
| 
 | ||||
|   // Default constructor for serialization
 | ||||
|   inline Custom() = default; | ||||
| 
 | ||||
|  private: | ||||
| #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template <class ARCHIVE> | ||||
|   void serialize(ARCHIVE &ar, const unsigned int /*version*/) { | ||||
|     ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); | ||||
|     ar &BOOST_SERIALIZATION_NVP(name_); | ||||
|   } | ||||
| #endif | ||||
| }; | ||||
| 
 | ||||
| }  // namespace mEstimator
 | ||||
| }  // namespace noiseModel
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -203,6 +203,24 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { | |||
|   double loss(double error) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class Custom: gtsam::noiseModel::mEstimator::Base { | ||||
|   Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight, | ||||
|          gtsam::noiseModel::mEstimator::CustomLossFunction loss, | ||||
|          gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight, | ||||
|          std::string name); | ||||
|   static gtsam::noiseModel::mEstimator::Custom* Create( | ||||
|       gtsam::noiseModel::mEstimator::CustomWeightFunction weight, | ||||
|       gtsam::noiseModel::mEstimator::CustomLossFunction loss, | ||||
|       gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight, | ||||
|       std::string name); | ||||
| 
 | ||||
|   // enabling serialization functionality | ||||
|   void serializable() const; | ||||
| 
 | ||||
|   double weight(double error) const; | ||||
|   double loss(double error) const; | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| }///\namespace mEstimator | ||||
| 
 | ||||
|  |  | |||
|  | @ -512,7 +512,7 @@ TEST(NoiseModel, robustFunctionAsymmetricCauchy) | |||
|   DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8); | ||||
|   // Test negative value to ensure we take absolute value of error.
 | ||||
|   DOUBLES_EQUAL(10.0, cauchy->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(1.0, cauchy->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(1.0, cauchy->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); | ||||
|  | @ -575,7 +575,7 @@ TEST(NoiseModel, robustFunctionAsymmetricTukey) | |||
|   DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8); | ||||
|   // Test negative value to ensure we take absolute value of error.
 | ||||
|   DOUBLES_EQUAL(10.0, tukey->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(1.0, tukey->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(1.0, tukey->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); | ||||
|  | @ -703,6 +703,35 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) | |||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(NoiseModel, robustNoiseCustomHuber) { | ||||
|   const double k = 10.0, error1 = 1.0, error2 = 100.0; | ||||
|   Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished(); | ||||
|   Vector b = Vector2(error1, error2); | ||||
|   const Robust::shared_ptr robust = | ||||
|       Robust::Create(mEstimator::Custom::Create( | ||||
|                          [k](double e) { | ||||
|                            const auto abs_e = std::abs(e); | ||||
|                            return abs_e <= k ? 1.0 : k / abs_e; | ||||
|                          }, | ||||
|                          [k](double e) { | ||||
|                            const auto abs_e = std::abs(e); | ||||
|                            return abs_e <= k ? abs_e * abs_e / 2.0 : k * abs_e - k * k / 2.0; | ||||
|                          }, | ||||
|                          noiseModel::mEstimator::Custom::Scalar, "Huber"), | ||||
|                      Unit::Create(2)); | ||||
| 
 | ||||
|   robust->WhitenSystem(A, b); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(error1, b(0), 1e-8); | ||||
|   DOUBLES_EQUAL(sqrt(k * error2), b(1), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(1.0, A(0, 0), 1e-8); | ||||
|   DOUBLES_EQUAL(10.0, A(0, 1), 1e-8); | ||||
|   DOUBLES_EQUAL(sqrt(k * 100.0), A(1, 0), 1e-8); | ||||
|   DOUBLES_EQUAL(sqrt(k / 100.0) * 1000.0, A(1, 1), 1e-8); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, lossFunctionAtZero) | ||||
| { | ||||
|   const double k = 5.0; | ||||
|  | @ -730,6 +759,12 @@ TEST(NoiseModel, lossFunctionAtZero) | |||
|   auto lsdz = mEstimator::L2WithDeadZone::Create(k); | ||||
|   DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); | ||||
|   DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); | ||||
|   auto assy_cauchy = mEstimator::AsymmetricCauchy::Create(k); | ||||
|   DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); | ||||
|   DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); | ||||
|   auto assy_tukey = mEstimator::AsymmetricTukey::Create(k); | ||||
|   DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); | ||||
|   DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue