12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Signed-off-by: Fan Jiang <i@fanjiang.me>
release/4.3a0
Fan Jiang 2023-08-18 17:09:47 -04:00
parent 8214bdc2ff
commit 9c40a2cd8b
4 changed files with 135 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/linear/LossFunctions.cpp
View File

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

44
gtsam/linear/LossFunctions.h
View File

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

18
gtsam/linear/linear.i
View File

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

39
gtsam/linear/tests/testNoiseModel.cpp
View File

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