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

Merged twio changes in: the is_constrained() flag that does away with the dynamic cast, and the changes duy made in commit a61b49d to remove the constrained noise model hack. At the same time, both linearize methods (in expressionFactor and NonlinearFactor) now make use of is_constrained().

release/4.3a0
dellaert 2014-11-02 13:45:54 +01:00
parent b21db08ec1 649478f186
commit a8ab910c32
6 changed files with 39 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/Matrix.cpp
View File

@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...)
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
const DenseIndex m = A.rows();
if (inf_mask) {
// only scale the first v.size() rows of A to support augmented Matrix
for (DenseIndex i=0; i<v.size(); ++i) {
for (DenseIndex i=0; i<m; ++i) {
const double& vi = v(i);
if (std::isfinite(vi))
A.row(i) *= vi;

1
gtsam/base/Matrix.h
View File

@ -416,7 +416,6 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
* Arguments (Matrix, Vector) scales the columns,
* (Vector, Matrix) scales the rows
* @param inf_mask when true, will not scale with a NaN or inf value.
* The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
*/
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row

26
gtsam/linear/NoiseModel.cpp
View File

@ -339,12 +339,12 @@ Vector Constrained::whiten(const Vector& v) const {
// a hard constraint, we don't do anything.
const Vector& a = v;
const Vector& b = sigmas_;
// Now allows for whiten augmented vector with a new additional part coming
// from the Lagrange multiplier. So a.size() >= b.size()
Vector c = a;
for( DenseIndex i = 0; i < b.size(); i++ ) {
size_t n = a.size();
assert (b.size()==a.size());
Vector c(n);
for( size_t i = 0; i < n; i++ ) {
const double& ai = a(i), &bi = b(i);
if (bi!=0) c(i) = ai/bi;
c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
}
return c;
}
@ -362,11 +362,7 @@ double Constrained::distance(const Vector& v) const {
/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const {
// selective scaling
// Now allow augmented Matrix with a new additional part coming
// from the Lagrange multiplier.
Matrix M(H.block(0, 0, dim(), H.cols()));
Constrained::WhitenInPlace(M);
return M;
return vector_scale(invsigmas(), H, true);
}
/* ************************************************************************* */
@ -375,8 +371,6 @@ void Constrained::WhitenInPlace(Matrix& H) const {
// 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.
// Now allow augmented Matrix with a new additional part coming
// from the Lagrange multiplier.
// Inlined: vector_scale_inplace(invsigmas(), H, true);
// vector_scale_inplace(v, A, true);
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
@ -399,14 +393,12 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
}
/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const {
Vector sigmas = ones(dim()+augmentedDim);
Constrained::shared_ptr Constrained::unit() const {
Vector sigmas = ones(dim());
for (size_t i=0; i<dim(); ++i)
if (this->sigmas_(i) == 0.0)
sigmas(i) = 0.0;
Vector augmentedMu = zero(dim()+augmentedDim);
subInsert(augmentedMu, mu_, 0);
return MixedSigmas(augmentedMu, sigmas);
return MixedSigmas(mu_, sigmas);
}
/* ************************************************************************* */

15
gtsam/linear/NoiseModel.h
View File

@ -61,6 +61,11 @@ 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;
}
/// Dimensionality
inline size_t dim() const { return dim_;}
@ -385,6 +390,11 @@ namespace gtsam {
virtual ~Constrained() {}
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
virtual bool is_constrained() const {
return true;
}
/// Access mu as a vector
const Vector& mu() const { return mu_; }
@ -481,9 +491,8 @@ namespace gtsam {
/**
* Returns a Unit version of a constrained noisemodel in which
* constrained sigmas remain constrained and the rest are unit scaled
* Now support augmented part from the Lagrange multiplier.
*/
shared_ptr unit(size_t augmentedDim = 0) const;
shared_ptr unit() const;
private:
/** Serialization function */
@ -732,7 +741,7 @@ namespace gtsam {
};
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by:
/// Dipl.-Inform. Jan Oberl<EFBFBD>nder (M.Sc.), FZI Research Center for
/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
/// Information Technology, Karlsruhe, Germany.
/// oberlaender@fzi.de
/// Thanks Jan!

28
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -22,7 +22,6 @@
namespace gtsam {
/* ************************************************************************* */
void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << " keys = { ";
@ -32,10 +31,12 @@ void NonlinearFactor::print(const std::string& s,
std::cout << "}" << std::endl;
}
/* ************************************************************************* */
bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
return Base::equals(f);
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::map<Key, Key>& rekey_mapping) const {
shared_ptr new_factor = clone();
@ -48,6 +49,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
return new_factor;
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::vector<Key>& new_keys) const {
assert(new_keys.size() == this->keys().size());
@ -57,13 +59,13 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
}
/* ************************************************************************* */
void NoiseModelFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: ");
}
/* ************************************************************************* */
bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol)
@ -72,6 +74,7 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
&& noiseModel_->equals(*e->noiseModel_, tol)));
}
/* ************************************************************************* */
static void check(const SharedNoiseModel& noiseModel, size_t m) {
if (!noiseModel)
throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
@ -80,12 +83,14 @@ static void check(const SharedNoiseModel& noiseModel, size_t m) {
"NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
}
/* ************************************************************************* */
Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector b = unwhitenedError(c);
check(noiseModel_, b.size());
return noiseModel_->whiten(b);
}
/* ************************************************************************* */
double NoiseModelFactor::error(const Values& c) const {
if (this->active(c)) {
const Vector b = unwhitenedError(c);
@ -96,6 +101,7 @@ double NoiseModelFactor::error(const Values& c) const {
}
}
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
const Values& x) const {
@ -119,18 +125,12 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
}
// TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!!
noiseModel::Constrained::shared_ptr constrained = //
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained) {
// Create a factor of reduced row dimension d_
size_t d_ = b.size() - constrained->dim();
Vector zero_ = zero(d_);
Vector b_ = concatVectors(2, &b, &zero_);
noiseModel::Constrained::shared_ptr model = constrained->unit(d_);
return boost::make_shared<JacobianFactor>(terms, b_, model);
} else
return boost::make_shared<JacobianFactor>(terms, b);
if (noiseModel_->is_constrained())
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
/* ************************************************************************* */

8
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -107,14 +107,12 @@ public:
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Check whether noise model is constrained or not
noiseModel::Constrained::shared_ptr constrained = //
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
// Create a writeable JacobianFactor in advance
// In case noise model is constrained, we need to provide a noise model
bool constrained = noiseModel_->is_constrained();
boost::shared_ptr<JacobianFactor> factor(
constrained ? new JacobianFactor(keys_, dimensions_, Dim,
constrained->unit()) :
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
new JacobianFactor(keys_, dimensions_, Dim));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_