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

Merged in feature/constraints (pull request #49) 

Cleaning up constraints a bit
release/4.3a0
Frank Dellaert 2014-11-26 12:34:40 +01:00
parent 846513cdb5 3307a49e65
commit d6fe891d70
5 changed files with 68 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
gtsam/linear/NoiseModel.cpp
View File

@ -16,20 +16,19 @@
* @author Frank Dellaert
*/
#include <limits>
#include <iostream>
#include <typeinfo>
#include <stdexcept>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <gtsam/base/timing.h>
#include <gtsam/linear/NoiseModel.h>
#include <limits>
#include <iostream>
#include <typeinfo>
#include <stdexcept>
static double inf = std::numeric_limits<double>::infinity();
using namespace std;
namespace gtsam {
@ -290,42 +289,42 @@ void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
// Constrained
/* ************************************************************************* */
namespace internal {
// switch precisions and invsigmas to finite value
// TODO: why?? And, why not just ask s==0.0 below ?
static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
for (size_t i = 0; i < sigmas.size(); ++i)
if (!std::isfinite(1. / sigmas[i])) {
precisions[i] = 0.0;
invsigmas[i] = 0.0;
}
}
}
/* ************************************************************************* */
Constrained::Constrained(const Vector& sigmas)
: Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {
for (int i=0; i<sigmas.size(); ++i) {
if (!std::isfinite(1./sigmas(i))) {
precisions_(i) = 0.0; // Set to finite value
invsigmas_(i) = 0.0;
}
}
internal::fix(sigmas, precisions_, invsigmas_);
}
/* ************************************************************************* */
Constrained::Constrained(const Vector& mu, const Vector& sigmas)
: Diagonal(sigmas), mu_(mu) {
// assert(sigmas.size() == mu.size());
for (int i=0; i<sigmas.size(); ++i) {
if (!std::isfinite(1./sigmas(i))) {
precisions_(i) = 0.0; // Set to finite value
invsigmas_(i) = 0.0;
}
}
internal::fix(sigmas, precisions_, invsigmas_);
}
/* ************************************************************************* */
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);
// }
Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu,
const Vector& sigmas) {
return shared_ptr(new Constrained(mu, sigmas));
}
/* ************************************************************************* */
bool Constrained::constrained(size_t i) const {
// TODO why not just check sigmas_[i]==0.0 ?
return !std::isfinite(1./sigmas_[i]);
}
/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
gtsam::print(sigmas_, name + "constrained sigmas");
@ -352,9 +351,8 @@ Vector Constrained::whiten(const Vector& v) const {
/* ************************************************************************* */
double Constrained::distance(const Vector& v) const {
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
// TODO Find a better way of doing these checks
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
if (!std::isfinite(1./sigmas_[i])) // whiten makes constrained variables zero
if (constrained(i)) // whiten makes constrained variables zero
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
return w.dot(w);
}
@ -367,36 +365,23 @@ Matrix Constrained::Whiten(const Matrix& H) const {
/* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const {
// selective scaling
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
// indicating a hard constraint, we leave H's row i in place.
// Inlined: vector_scale_inplace(invsigmas(), H, true);
// vector_scale_inplace(v, A, true);
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
const double& invsigma = invsigmas_(i);
if (std::isfinite(1./sigmas_(i)))
H.row(i) *= invsigma;
}
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
if (!constrained(i)) // if constrained, leave row of H as is
H.row(i) *= invsigmas_(i);
}
/* ************************************************************************* */
void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
// selective scaling
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
// indicating a hard constraint, we leave H's row i in place.
const Vector& _invsigmas = invsigmas();
for(DenseIndex row = 0; row < _invsigmas.size(); ++row)
if(isfinite(_invsigmas(row)))
H.row(row) *= _invsigmas(row);
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
if (!constrained(i)) // if constrained, leave row of H as is
H.row(i) *= invsigmas_(i);
}
/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit() const {
Vector sigmas = ones(dim());
for (size_t i=0; i<dim(); ++i)
if (this->sigmas_(i) == 0.0)
if (constrained(i))
sigmas(i) = 0.0;
return MixedSigmas(mu_, sigmas);
}
@ -472,7 +457,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
const size_t& j = t.get<0>();
const Vector& rd = t.get<1>();
precisions(i) = t.get<2>();
if (precisions(i)==inf) mixed = true;
if (constrained(i)) mixed = true;
for (size_t j2=0; j2<j; ++j2)
Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
for (size_t j2=j; j2<n+1; ++j2)

37
gtsam/linear/NoiseModel.h
View File

@ -61,9 +61,9 @@ namespace gtsam {
Base(size_t dim = 1):dim_(dim) {}
virtual ~Base() {}
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
virtual bool is_constrained() const {
return false;
/// true if a constrained noise mode, saves slow/clumsy dynamic casting
virtual bool isConstrained() const {
return false; // default false
}
/// Dimensionality
@ -237,12 +237,6 @@ namespace gtsam {
*/
virtual Matrix R() const { return thisR();}
/**
* Simple check for constrained-ness
* FIXME Find a better way of handling this
*/
virtual bool isConstrained() const {return false;}
private:
/** Serialization function */
friend class boost::serialization::access;
@ -390,11 +384,14 @@ namespace gtsam {
virtual ~Constrained() {}
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
virtual bool is_constrained() const {
/// true if a constrained noise mode, saves slow/clumsy dynamic casting
virtual bool isConstrained() const {
return true;
}
/// Return true if a particular dimension is free or constrained
bool constrained(size_t i) const;
/// Access mu as a vector
const Vector& mu() const { return mu_; }
@ -402,24 +399,22 @@ namespace gtsam {
* 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 = true);
static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = true) {
return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart);
static shared_ptr MixedSigmas(const Vector& sigmas) {
return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas);
}
/**
* 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 = true) {
return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart);
static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
return MixedSigmas(repeat(sigmas.size(), m), sigmas);
}
/**
@ -482,12 +477,6 @@ namespace gtsam {
*/
virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
/**
* Check constrained is always true
* FIXME Find a better way of handling this
*/
virtual bool isConstrained() const { return true; }
/**
* Returns a Unit version of a constrained noisemodel in which
* constrained sigmas remain constrained and the rest are unit scaled

23
gtsam/nonlinear/NonlinearEquality.h
View File

@ -17,13 +17,13 @@
#pragma once
#include <limits>
#include <iostream>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <limits>
#include <iostream>
namespace gtsam {
/**
@ -216,16 +216,22 @@ protected:
X value_; /// fixed value for variable
GTSAM_CONCEPT_MANIFOLD_TYPE(X)
;GTSAM_CONCEPT_TESTABLE_TYPE(X)
;
GTSAM_CONCEPT_TESTABLE_TYPE(X)
public:
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
///TODO: comment
NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) :
Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(
/**
* Constructor
* @param value feasible value that values(key) shouild be equal to
* @param key the key for the unknown variable to be constrained
* @param mu a parameter which really turns this into a strong prior
*
*/
NonlinearEquality1(const X& value, Key key, double mu = 1000.0) :
Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key), value_(
value) {
}
@ -285,7 +291,6 @@ protected:
typedef NonlinearEquality2<VALUE> This;
GTSAM_CONCEPT_MANIFOLD_TYPE(X)
;
/** default constructor to allow for serialization */
NonlinearEquality2() {

5
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -132,10 +132,11 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
}
// TODO pass unwhitened + noise model to Gaussian factor
if (noiseModel_ && noiseModel_->is_constrained())
using noiseModel::Constrained;
if (noiseModel_ && noiseModel_->isConstrained())
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}

3
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -90,7 +90,7 @@ public:
// Create a writeable JacobianFactor in advance
// In case noise model is constrained, we need to provide a noise model
bool constrained = noiseModel_->is_constrained();
bool constrained = noiseModel_->isConstrained();
boost::shared_ptr<JacobianFactor> factor(
constrained ? new JacobianFactor(keys_, dims_, Dim,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
@ -107,7 +107,6 @@ public:
T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
// Evaluate error and set RHS vector b
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
Ab(size()).col(0) = -chart.local(measurement_, value);