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

Added mu to noisemodel

release/4.3a0
Alex Cunningham 2011-11-09 22:09:52 +00:00
parent 9fe47025d5
commit 70afdfb7d3
3 changed files with 137 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/linear/NoiseModel.cpp
View File

@ -275,6 +275,19 @@ Vector Diagonal::sample() const {
/* ************************************************************************* */
// Constrained
/* ************************************************************************* */
Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector& sigmas, bool smart) {
// FIXME: can't return a diagonal shared_ptr due to conversion
// if (smart) {
// // look for zeros to make a constraint
// for (size_t i=0; i< (size_t) sigmas.size(); ++i)
// if (sigmas(i)<1e-8)
// return MixedSigmas(mu, sigmas);
// return Diagonal::Sigmas(sigmas);
// }
return shared_ptr(new Constrained(mu, sigmas));
}
/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
gtsam::print(sigmas_, name + ": constrained sigmas");
@ -283,16 +296,28 @@ void Constrained::print(const std::string& name) const {
/* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const {
// ediv_ does the right thing with the errors
// FIXME: solve this more effectively
return ediv_(v, sigmas_);
}
/* ************************************************************************* */
double Constrained::distance(const Vector& v) const {
Vector w = whiten(v); // get noisemodel for constrained elements
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
if (isinf(w[i])) // whiten makes constrained variables infinite
w[i] = v[i] * sqrt(mu_[i]); // FIXME: may want to store sqrt rather than rebuild
return w.dot(w);
}
/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const {
// FIXME: replace with pass-through
throw logic_error("noiseModel::Constrained cannot Whiten");
}
/* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const {
// FIXME: replace with pass-through
throw logic_error("noiseModel::Constrained cannot Whiten");
}

67
gtsam/linear/NoiseModel.h
View File

@ -212,6 +212,7 @@ namespace gtsam {
/**
* Simple check for constrained-ness
* FIXME Find a better way of handling this
*/
virtual bool isConstrained() const {return false;}
@ -327,17 +328,28 @@ namespace gtsam {
* information matrix, but this class is specifically equipped to deal with
* singular noise models, specifically: whiten will return zero on those
* components that have zero sigma *and* zero error, infinity otherwise.
* FIXME: the "otherwise return infinity" does not solve anything
*
* The distance function in this function provides an error model
* for a penalty function with a scaling function, assuming a mask of
*/
class Constrained : public Diagonal {
protected:
// Constrained does not have member variables
// Instead (possibly zero) sigmas are stored in Diagonal Base class
// Sigmas are contained in the base class
// Penalty function parameters
Vector mu_;
/** protected constructor takes sigmas */
// Keeps only sigmas and calculates invsigmas when necessary
Constrained(const Vector& sigmas = zero(1)) :
Diagonal(sigmas, false) {}
Diagonal(sigmas, false), mu_(repeat(sigmas.size(), 1000.0)) {}
// Keeps only sigmas and calculates invsigmas when necessary
// allows for specifying mu
Constrained(const Vector& mu, const Vector& sigmas) :
Diagonal(sigmas, false), mu_(mu) {}
public:
@ -345,19 +357,38 @@ namespace gtsam {
virtual ~Constrained() {}
/// Access mu as a vector
const Vector& mu() const { return mu_; }
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas,
bool smart = false);
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
* TODO: make smart - check for zeros
*/
static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = false) {
return shared_ptr(new Constrained(sigmas));
return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart);
}
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(double m, const Vector& sigmas,
bool smart = false) {
return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart);
}
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
* TODO: allow for mu
*/
static shared_ptr MixedVariances(const Vector& variances) {
return shared_ptr(new Constrained(esqrt(variances)));
}
@ -365,24 +396,42 @@ namespace gtsam {
/**
* A diagonal noise model created by specifying a Vector of
* precisions, some of which might be inf
* TODO: allow for mu
*/
static shared_ptr MixedPrecisions(const Vector& precisions) {
return MixedVariances(reciprocal(precisions));
}
/**
* Fully constrained. TODO: subclass ?
* The distance function for a constrained noisemodel,
* for non-constrained versions, uses sigmas, otherwise
* uses the penalty function with mu
*/
virtual double distance(const Vector& v) const;
/** Fully constrained variations */
static shared_ptr All(size_t dim) {
return MixedSigmas(repeat(dim,0));
return shared_ptr(new Constrained(repeat(dim, 1000.0), repeat(dim,0)));
}
/** Fully constrained variations */
static shared_ptr All(size_t dim, const Vector& mu) {
return shared_ptr(new Constrained(mu, repeat(dim,0)));
}
/** Fully constrained variations */
static shared_ptr All(size_t dim, double m) {
return shared_ptr(new Constrained(repeat(dim, m), repeat(dim,0)));
}
virtual void print(const std::string& name) const;
/// Calculates error vector with weights applied
virtual Vector whiten(const Vector& v) const;
// Whitening Jacobians does not make sense for possibly constrained
// noise model and will throw an exception.
// FIXME: change to allow for use a normal linearization function
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
@ -393,6 +442,7 @@ namespace gtsam {
/**
* Check constrained is always true
* FIXME Find a better way of handling this
*/
virtual bool isConstrained() const {return true;}
@ -402,6 +452,7 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
ar & BOOST_SERIALIZATION_NVP(mu_);
}
}; // Constrained

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

@ -41,10 +41,6 @@ static Matrix Sigma = Matrix_(3, 3,
var, 0.0, 0.0,
0.0, var, 0.0,
0.0, 0.0, var);
//static Matrix Q = Matrix_(3, 3,
// prc, 0.0, 0.0,
// 0.0, prc, 0.0,
// 0.0, 0.0, prc);
static double inf = std::numeric_limits<double>::infinity();
@ -108,7 +104,8 @@ TEST(NoiseModel, constructors)
}
/* ************************************************************************* */
TEST(NoiseModel, Unit) {
TEST(NoiseModel, Unit)
{
Vector v = Vector_(3,5.0,10.0,15.0);
Gaussian::shared_ptr u(Unit::Create(3));
EXPECT(assert_equal(v,u->whiten(v)));
@ -135,23 +132,66 @@ TEST(NoiseModel, equals)
}
/* ************************************************************************* */
TEST(NoiseModel, sample) {
TEST(NoiseModel, sample)
{
Vector s = Vector_(3,1.0,2.0,3.0);
SharedDiagonal model = sharedSigmas(s);
Vector v = model->sample();
// no check as no way yet to set random seed
}
// TODO enable test once a mechanism for smart constraints exists
///* ************************************************************************* */
//TEST(NoiseModel, ConstrainedSmart )
//{
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma), true);
// Diagonal::shared_ptr n1 = boost::shared_dynamic_cast<Diagonal>(nonconstrained);
// Constrained::shared_ptr n2 = boost::shared_dynamic_cast<Constrained>(nonconstrained);
// EXPECT(n1);
// EXPECT(!n2);
//
// Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true);
// Diagonal::shared_ptr c1 = boost::shared_dynamic_cast<Diagonal>(constrained);
// Constrained::shared_ptr c2 = boost::shared_dynamic_cast<Constrained>(constrained);
// EXPECT(c1);
// EXPECT(c2);
//}
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedConstructors )
{
Constrained::shared_ptr actual;
size_t d = 3;
double m = 100.0;
Vector sigmas = Vector_(3, sigma, 0.0, 0.0);
Vector mu = Vector_(3, 200.0, 300.0, 400.0);
actual = Constrained::All(d);
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
actual = Constrained::All(d, m);
EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));
actual = Constrained::All(d, mu);
EXPECT(assert_equal(mu, actual->mu()));
actual = Constrained::MixedSigmas(mu, sigmas);
EXPECT(assert_equal(mu, actual->mu()));
actual = Constrained::MixedSigmas(m, sigmas);
EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu()));
}
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed )
{
Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
infeasible = Vector_(3, 1.0, 1.0, 1.0);
Constrained::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
EXPECT(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(inf,d->Mahalanobis(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->Mahalanobis(feasible),1e-9);
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
}
/* ************************************************************************* */
@ -163,8 +203,9 @@ TEST(NoiseModel, ConstrainedAll )
Constrained::shared_ptr i = Constrained::All(3);
EXPECT(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible)));
EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(inf,i->Mahalanobis(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->Mahalanobis(feasible),1e-9);
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
}
/* ************************************************************************* */