Add a custom loss function for Robust, fix a bug in Asymmetric

Signed-off-by: Fan Jiang <i@fanjiang.me>

gtsam/linear/LossFunctions.cpp

@@ -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

gtsam/linear/LossFunctions.h

@@ -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

gtsam/linear/linear.i

@@ -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
 

gtsam/linear/tests/testNoiseModel.cpp

@@ -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);
 }