Merged in enriquefernandezperdomo/gtsam/add_dcs_robust_kernel (pull request #180)
Add DCS robust kernelrelease/4.3a0
Frank Dellaert
commit
9e71ad793b
|
|
@ -696,7 +696,7 @@ Huber::Huber(double k, const ReweightScheme reweight)
|
|||
}
|
||||
|
||||
double Huber::weight(double error) const {
|
||||
return (error < k_) ? (1.0) : (k_ / fabs(error));
|
||||
return (fabs(error) > k_) ? k_ / fabs(error) : 1.0;
|
||||
}
|
||||
|
||||
void Huber::print(const std::string &s="") const {
|
||||
|
|
@ -799,6 +799,66 @@ Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) {
|
|||
return shared_ptr(new Welsh(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);
|
||||
}
|
||||
|
||||
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<const GemanMcClure*>(&expected);
|
||||
if (p == NULL) return false;
|
||||
return fabs(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;
|
||||
}
|
||||
|
||||
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<const DCS*>(&expected);
|
||||
if (p == NULL) return false;
|
||||
return fabs(c_ - p->c_) < tol;
|
||||
}
|
||||
|
||||
DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) {
|
||||
return shared_ptr(new DCS(c, reweight));
|
||||
}
|
||||
|
||||
} // namespace mEstimator
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -823,6 +823,65 @@ namespace gtsam {
|
|||
}
|
||||
};
|
||||
|
||||
/// 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<GemanMcClure> shared_ptr;
|
||||
|
||||
GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
|
||||
virtual ~GemanMcClure() {}
|
||||
virtual double weight(double error) const;
|
||||
virtual void print(const std::string &s) const;
|
||||
virtual bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
||||
protected:
|
||||
double c_;
|
||||
|
||||
private:
|
||||
/** 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(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<DCS> shared_ptr;
|
||||
|
||||
DCS(double c = 1.0, const ReweightScheme reweight = Block);
|
||||
virtual ~DCS() {}
|
||||
virtual double weight(double error) const;
|
||||
virtual void print(const std::string &s) const;
|
||||
virtual bool equals(const Base& expected, double tol=1e-8) const;
|
||||
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
|
||||
|
||||
protected:
|
||||
double c_;
|
||||
|
||||
private:
|
||||
/** 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(c_);
|
||||
}
|
||||
};
|
||||
|
||||
} ///\namespace mEstimator
|
||||
|
||||
/// Base class for robust error models
|
||||
|
|
|
|||
|
|
@ -322,7 +322,7 @@ TEST(NoiseModel, WhitenInPlace)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(NoiseModel, robustFunction)
|
||||
TEST(NoiseModel, robustFunctionHuber)
|
||||
{
|
||||
const double k = 5.0, error1 = 1.0, error2 = 10.0;
|
||||
const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k);
|
||||
|
|
@ -332,8 +332,28 @@ TEST(NoiseModel, robustFunction)
|
|||
DOUBLES_EQUAL(0.5, weight2, 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionGemanMcClure)
|
||||
{
|
||||
const double k = 1.0, error1 = 1.0, error2 = 10.0;
|
||||
const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
|
||||
const double weight1 = gmc->weight(error1),
|
||||
weight2 = gmc->weight(error2);
|
||||
DOUBLES_EQUAL(0.25 , weight1, 1e-8);
|
||||
DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustFunctionDCS)
|
||||
{
|
||||
const double k = 1.0, error1 = 1.0, error2 = 10.0;
|
||||
const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k);
|
||||
const double weight1 = dcs->weight(error1),
|
||||
weight2 = dcs->weight(error2);
|
||||
DOUBLES_EQUAL(1.0 , weight1, 1e-8);
|
||||
DOUBLES_EQUAL(0.00039211, weight2, 1e-8);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(NoiseModel, robustNoise)
|
||||
TEST(NoiseModel, robustNoiseHuber)
|
||||
{
|
||||
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();
|
||||
|
|
@ -353,6 +373,57 @@ TEST(NoiseModel, robustNoise)
|
|||
DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustNoiseGemanMcClure)
|
||||
{
|
||||
const double k = 1.0, error1 = 1.0, error2 = 100.0;
|
||||
const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
|
||||
Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
|
||||
Vector b = Vector2(error1, error2);
|
||||
const Robust::shared_ptr robust = Robust::Create(
|
||||
mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar),
|
||||
Unit::Create(2));
|
||||
|
||||
robust->WhitenSystem(A, b);
|
||||
|
||||
const double k2 = k*k;
|
||||
const double k4 = k2*k2;
|
||||
const double k2error = k2 + error2*error2;
|
||||
|
||||
const double sqrt_weight_error1 = sqrt(0.25);
|
||||
const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error));
|
||||
|
||||
DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8);
|
||||
DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8);
|
||||
DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8);
|
||||
DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8);
|
||||
DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8);
|
||||
}
|
||||
|
||||
TEST(NoiseModel, robustNoiseDCS)
|
||||
{
|
||||
const double k = 1.0, error1 = 1.0, error2 = 100.0;
|
||||
const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
|
||||
Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
|
||||
Vector b = Vector2(error1, error2);
|
||||
const Robust::shared_ptr robust = Robust::Create(
|
||||
mEstimator::DCS::Create(k, mEstimator::DCS::Scalar),
|
||||
Unit::Create(2));
|
||||
|
||||
robust->WhitenSystem(A, b);
|
||||
|
||||
const double sqrt_weight = 2.0*k/(k + error2*error2);
|
||||
|
||||
DOUBLES_EQUAL(error1, b(0), 1e-8);
|
||||
DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8);
|
||||
|
||||
DOUBLES_EQUAL(a00, A(0,0), 1e-8);
|
||||
DOUBLES_EQUAL(a01, A(0,1), 1e-8);
|
||||
DOUBLES_EQUAL(sqrt_weight*a10, A(1,0), 1e-8);
|
||||
DOUBLES_EQUAL(sqrt_weight*a11, A(1,1), 1e-8);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#define TEST_GAUSSIAN(gaussian)\
|
||||
EQUALITY(info, gaussian->information());\
|
||||
|
|
|
|||
Loading…
Reference in New Issue