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

add an option to reweight according to scalar or block

release/4.3a0
Yong-Dian Jian 2011-08-28 02:47:17 +00:00
parent 27d062a0f0
commit 25cd24409a
2 changed files with 55 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/linear/NoiseModel.cpp
View File

@ -446,27 +446,52 @@ Vector Base::weight(const Vector &error) const {
}
void Base::reweight(Matrix &A, Vector &error) const {
const Vector W = weight(error);
vector_scale_inplace(W,A);
error = emul(W, error);
if ( reweight_ == Block ) {
const double w = weight(error.norm());
A *= w;
error *= w;
}
else {
const Vector W = weight(error);
vector_scale_inplace(W,A);
error = emul(W, error);
}
}
void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
const Vector W = weight(error);
vector_scale_inplace(W,A1);
vector_scale_inplace(W,A2);
error = emul(W, error);
if ( reweight_ == Block ) {
const double w = weight(error.norm());
A1 *= w;
A2 *= w;
error *= w;
}
else {
const Vector W = weight(error);
vector_scale_inplace(W,A1);
vector_scale_inplace(W,A2);
error = emul(W, error);
}
}
void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
const Vector W = weight(error);
vector_scale_inplace(W,A1);
vector_scale_inplace(W,A2);
vector_scale_inplace(W,A3);
error = emul(W, error);
if ( reweight_ == Block ) {
const double w = weight(error.norm());
A1 *= w;
A2 *= w;
A3 *= w;
error *= w;
}
else {
const Vector W = weight(error);
vector_scale_inplace(W,A1);
vector_scale_inplace(W,A2);
vector_scale_inplace(W,A3);
error = emul(W, error);
}
}
Fair::Fair(const double c): c_(c) {
Fair::Fair(const double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
if ( c_ <= 0 ) {
cout << "MEstimator Fair takes only positive double in constructor. forced to 1.0" << endl;
c_ = 1.0;
@ -488,7 +513,8 @@ bool Fair::equals(const Base &expected, const double tol) const {
Fair::shared_ptr Fair::Create(const double c)
{ return shared_ptr(new Fair(c)); }
Huber::Huber(const double k): k_(k) {
Huber::Huber(const double k, const ReweightScheme reweight)
: Base(reweight), k_(k) {
if ( k_ <= 0 ) {
cout << "MEstimator Huber takes only positive double in constructor. forced to 1.0" << endl;
k_ = 1.0;

19
gtsam/linear/NoiseModel.h
View File

@ -507,8 +507,15 @@ namespace gtsam {
class Base {
public:
typedef boost::shared_ptr<Base> shared_ptr;
Base() {}
enum ReweightScheme { Scalar, Block };
typedef boost::shared_ptr<Base> shared_ptr;
protected:
ReweightScheme reweight_;
public:
Base():reweight_(Block) {}
Base(const ReweightScheme reweight):reweight_(reweight) {}
virtual ~Base() {}
virtual double weight(const double &error) const = 0;
virtual void print(const std::string &s) const = 0;
@ -525,12 +532,14 @@ namespace gtsam {
protected:
double c_;
public:
Fair(const double c);
Fair(const double c, const ReweightScheme reweight = Block);
virtual ~Fair() {}
virtual double weight(const double &error) const ;
virtual void print(const std::string &s) const ;
virtual bool equals(const Base& expected, const double tol=1e-8) const ;
static shared_ptr Create(const double c) ;
private:
Fair(){}
};
class Huber : public Base {
@ -539,12 +548,14 @@ namespace gtsam {
protected:
double k_;
public:
Huber(const double k);
Huber(const double k, const ReweightScheme reweight = Block);
virtual ~Huber() {}
virtual double weight(const double &error) const ;
virtual void print(const std::string &s) const ;
virtual bool equals(const Base& expected, const double tol=1e-8) const ;
static shared_ptr Create(const double k) ;
private:
Huber(){}
};
}